buf = f.read()
addr = 0
while addr < len(buf):
- #time.sleep(0.1)
+ time.sleep(0.1)
if check_crc(self.ser, buf, addr, SPM_PAGE_SIZE) == 0:
sys.stdout.write("*")
sys.stdout.flush()
filename = os.path.join(MICROB_PATH, "../mainboard/main.bin")
self.bootloader(filename, 1)
- def do_mechboard(self, args):
- filename = os.path.join(MICROB_PATH, "../mechboard/main.bin")
+ def do_cobboard(self, args):
+ filename = os.path.join(MICROB_PATH, "../cobboard/main.bin")
self.bootloader(filename, 2)
- def do_sensorboard(self, args):
- filename = os.path.join(MICROB_PATH, "../sensorboard/main.bin")
+ def do_ballboard(self, args):
+ filename = os.path.join(MICROB_PATH, "../ballboard/main.bin")
self.bootloader(filename, 3)
def do_toto(self, args):
#
-# Automatically generated by make menuconfig: don't edit
+# Automatically generated make config: don't edit
#
#
#
# Base modules
#
+
+#
+# Enable math library in generation options to see all modules
+#
CONFIG_MODULE_CIRBUF=y
# CONFIG_MODULE_CIRBUF_LARGE is not set
# CONFIG_MODULE_FIXED_POINT is not set
#
# Communication modules
#
+
+#
+# uart needs circular buffer, mf2 client may need scheduler
+#
CONFIG_MODULE_UART=y
# CONFIG_MODULE_UART_9BITS is not set
CONFIG_MODULE_UART_CREATE_CONFIG=y
# Control system modules
#
# CONFIG_MODULE_CONTROL_SYSTEM_MANAGER is not set
+
+#
+# Filters
+#
CONFIG_MODULE_PID=y
CONFIG_MODULE_PID_CREATE_CONFIG=y
# CONFIG_MODULE_RAMP is not set
#
# Radio devices
#
+
+#
+# Some radio devices require SPI to be activated
+#
# CONFIG_MODULE_CC2420 is not set
# CONFIG_MODULE_CC2420_CREATE_CONFIG is not set
#
# Crypto modules
#
+
+#
+# Crypto modules depend on utils module
+#
# CONFIG_MODULE_AES is not set
# CONFIG_MODULE_AES_CTR is not set
# CONFIG_MODULE_MD5 is not set
#
# Encodings modules
#
+
+#
+# Encoding modules depend on utils module
+#
# CONFIG_MODULE_BASE64 is not set
# CONFIG_MODULE_HAMMING is not set
#
# Debug modules
#
+
+#
+# Debug modules depend on utils module
+#
# CONFIG_MODULE_DIAGNOSTIC is not set
# CONFIG_MODULE_DIAGNOSTIC_CREATE_CONFIG is not set
# CONFIG_MODULE_ERROR is not set
#define TSOP_STA_TIME_SHORT_US (1.5 * TSOP_STA_N_PERIODS * TSOP_STA_PERIOD_US)
#define TSOP_STA_TIME_LONG_US (2.5 * TSOP_STA_N_PERIODS * TSOP_STA_PERIOD_US)
-#define TSOP_STA_TIME_SHORT ((uint16_t)(TSOP_STA_TIME_SHORT_US*2))
-#define TSOP_STA_TIME_LONG ((uint16_t)(TSOP_STA_TIME_LONG_US*2))
+#define TSOP_STA_TIME_SHORT ((uint16_t)(TSOP_STA_TIME_SHORT_US/4))
+#define TSOP_STA_TIME_LONG ((uint16_t)(TSOP_STA_TIME_LONG_US/4))
#define TSOP_STA_FRAME_LEN 16
#define TSOP_STA_FRAME_MASK ((1UL << TSOP_STA_FRAME_LEN) - 1)
#define TSOP_OPP_TIME_SHORT_US (1.5 * TSOP_OPP_N_PERIODS * TSOP_OPP_PERIOD_US)
#define TSOP_OPP_TIME_LONG_US (2.5 * TSOP_OPP_N_PERIODS * TSOP_OPP_PERIOD_US)
-#define TSOP_OPP_TIME_SHORT ((uint16_t)(TSOP_OPP_TIME_SHORT_US*2))
-#define TSOP_OPP_TIME_LONG ((uint16_t)(TSOP_OPP_TIME_LONG_US*2))
+#define TSOP_OPP_TIME_SHORT ((uint16_t)(TSOP_OPP_TIME_SHORT_US/4))
+#define TSOP_OPP_TIME_LONG ((uint16_t)(TSOP_OPP_TIME_LONG_US/4))
#define TSOP_OPP_FRAME_LEN 16
#define TSOP_OPP_FRAME_MASK ((1UL << TSOP_OPP_FRAME_LEN) - 1)
#define EICRx_TSOP EICRB /* EICRA is not ok, cannot do intr on any edge */
-#define INTx_TSOP_OPP INT5
-#define ISCx0_TSOP_OPP ISC50
-#define ISCx1_TSOP_OPP ISC(1
-#define SIG_TSOP_OPP SIG_INTERRUPT5
-#define TSOP_OPP_READ() (!(PINE & 0x20))
-
-#define INTx_TSOP_STA INT6
-#define ISCx0_TSOP_STA ISC60
-#define ISCx1_TSOP_STA ISC61
-#define SIG_TSOP_STA SIG_INTERRUPT6
-#define TSOP_STA_READ() (!(PINE & 0x40))
+#define INTx_TSOP_OPP INT6
+#define ISCx0_TSOP_OPP ISC60
+#define ISCx1_TSOP_OPP ISC61
+#define SIG_TSOP_OPP SIG_INTERRUPT6
+#define TSOP_OPP_READ() (!(PINE & 0x40))
+
+#define INTx_TSOP_STA INT5
+#define ISCx0_TSOP_STA ISC50
+#define ISCx1_TSOP_STA ISC51
+#define SIG_TSOP_STA SIG_INTERRUPT5
+#define TSOP_STA_READ() (!(PINE & 0x20))
#define TSOP_FREQ_455_MHZ 0.455
-#define N_PERIODS_455 10.
+#define N_PERIODS_455 15.
#define TSOP_FREQ_38_MHZ 0.038
#define N_PERIODS_38 15.
#define TSOP_FREQ_30_MHZ 0.030
/* TSOP STATIC */
-#define TSOP_STA_PERIOD_US (1./TSOP_FREQ_38_MHZ)
-#define TSOP_STA_N_PERIODS (N_PERIODS_38)
+#define TSOP_STA_PERIOD_US (1./TSOP_FREQ_455_MHZ)
+#define TSOP_STA_N_PERIODS (N_PERIODS_455)
#define TSOP_STA_TIME_SHORT_US (1.5 * TSOP_STA_N_PERIODS * TSOP_STA_PERIOD_US)
#define TSOP_STA_TIME_LONG_US (2.5 * TSOP_STA_N_PERIODS * TSOP_STA_PERIOD_US)
-#define TSOP_STA_TIME_SHORT ((uint16_t)(TSOP_STA_TIME_SHORT_US*2))
-#define TSOP_STA_TIME_LONG ((uint16_t)(TSOP_STA_TIME_LONG_US*2))
+#define TSOP_STA_TIME_SHORT ((uint16_t)(TSOP_STA_TIME_SHORT_US/4))
+#define TSOP_STA_TIME_LONG ((uint16_t)(TSOP_STA_TIME_LONG_US/4))
#define TSOP_STA_FRAME_LEN 16
#define TSOP_STA_FRAME_MASK ((1UL << TSOP_STA_FRAME_LEN) - 1)
#define TSOP_OPP_TIME_SHORT_US (1.5 * TSOP_OPP_N_PERIODS * TSOP_OPP_PERIOD_US)
#define TSOP_OPP_TIME_LONG_US (2.5 * TSOP_OPP_N_PERIODS * TSOP_OPP_PERIOD_US)
-#define TSOP_OPP_TIME_SHORT ((uint16_t)(TSOP_OPP_TIME_SHORT_US*2))
-#define TSOP_OPP_TIME_LONG ((uint16_t)(TSOP_OPP_TIME_LONG_US*2))
+#define TSOP_OPP_TIME_SHORT ((uint16_t)(TSOP_OPP_TIME_SHORT_US/4))
+#define TSOP_OPP_TIME_LONG ((uint16_t)(TSOP_OPP_TIME_LONG_US/4))
#define TSOP_OPP_FRAME_LEN 16
#define TSOP_OPP_FRAME_MASK ((1UL << TSOP_OPP_FRAME_LEN) - 1)
/* first rising edge */
if (status->len == 0 && cur_tsop && diff_time > status->time_long) {
+ LED6_ON();
status->len = 1;
status->val = 1;
status->frame = 0;
status->ref_time = ref_time;
status->mask = 1;
}
- /* any short edge */
+ /* any short pulse */
else if (status->len != 0 && diff_time < status->time_short) {
if (status->len & 1) {
if (status->val)
}
status->len ++;
}
- /* any long edge */
+ /* any long pulse */
else if (status->len != 0 && diff_time < status->time_long) {
status->val = !status->val;
if (status->val)
frame_mask = (1 << status->frame_len) - 1;
if (tail_next != status->head) {
+ LED5_ON();
status->ring[status->tail].frame = (status->frame & frame_mask);
status->ring[status->tail].ref_time = status->ref_time;
status->ring[status->tail].time = status->start_time;
status->ring[status->tail].tick = tick;
status->tail = tail_next;
- if ((status->led_cpt & 0x7) == 0)
- LED3_TOGGLE();
+/* if ((status->led_cpt & 0x7) == 0) */
+/* LED3_TOGGLE(); */
status->led_cpt ++;
}
status->len = 0;
status->prev_time = cur_time;
status->prev_tsop = cur_tsop;
+ LED3_OFF();
+ LED4_OFF();
+ LED5_OFF();
+ LED6_OFF();
}
/* decode frame */
running = 1;
sei();
- if (cur_tsop)
- LED5_ON();
- else
- LED5_OFF();
+/* if (cur_tsop) */
+/* LED5_ON(); */
+/* else */
+/* LED5_OFF(); */
decode_frame(&static_beacon, ref_time, cur_time, cur_tsop);
running = 1;
sei();
- if (cur_tsop)
- LED6_ON();
- else
- LED6_OFF();
+/* if (cur_tsop) */
+/* LED6_ON(); */
+/* else */
+/* LED6_OFF(); */
- decode_frame(&opp_beacon, ref_time, cur_time, cur_tsop);
+ //decode_frame(&opp_beacon, ref_time, cur_time, cur_tsop);
running = 0;
}
return TIM3_UNIT/icr_diff;
}
+static int8_t check_sta_frame(uint16_t frame, uint16_t time)
+{
+ int8_t beacon_id;
+
+ /* ignore bad cksum */
+ if (verify_cksum(frame) == 0xFFFF)
+ goto fail;
+
+ beacon_id = (frame >> TSOP_STA_BEACON_ID_SHIFT) & TSOP_STA_BEACON_ID_MASK;
+
+ if (beacon_id != TSOP_STA_BEACON_ID0 &&
+ beacon_id != TSOP_STA_BEACON_ID1)
+ goto fail;
+
+ /* if motor speed is not good, skip values */
+ if (current_motor_period < MOTOR_PERIOD_MIN)
+ goto fail;
+ if (current_motor_period > MOTOR_PERIOD_MAX)
+ goto fail;
+
+ return beacon_id;
+
+ fail:
+ /* display if needed */
+ if (beacon_tsop.debug_frame) {
+ printf("STA ID=%d frame=%x time=%d\r\n",
+ beacon_id, frame, time);
+ }
+ return -1;
+}
+
+
/* process the received frame ring */
static void process_sta_ring(struct frame_status *status)
{
uint8_t head, head_next;
uint16_t frame, frametick;
uint8_t found = 0;
- uint8_t beacon_id;
+ int8_t beacon_id;
/* beacon 0 */
uint16_t data0, time0, ref_time0;
head_next = (head+1) & FRAME_RING_MASK;
frame = status->ring[head].frame;
- /* ignore bad cksum */
- if (verify_cksum(frame) == 0xFFFF)
- continue;
-
- beacon_id = (frame >> TSOP_STA_BEACON_ID_SHIFT) & TSOP_STA_BEACON_ID_MASK;
- if (beacon_id != TSOP_STA_BEACON_ID0 &&
- beacon_id != TSOP_STA_BEACON_ID1)
+ beacon_id = check_sta_frame(frame, status->ring[head].time);
+ if (beacon_id < 0) {
+ head = head_next;
continue;
-
- /* if motor speed is not good, skip values */
- if (current_motor_period < MOTOR_PERIOD_MIN)
- continue;
- if (current_motor_period > MOTOR_PERIOD_MAX)
- continue;
-
- /* display if needed */
- if (beacon_tsop.debug_frame) {
- printf("STA ID=%d time=%d\r\n",
- beacon_id, status->ring[head].time);
}
if (beacon_id == TSOP_STA_BEACON_ID0) {
if (angle0 > M_PI)
angle0 -= M_PI;
+ /* display if needed */
+ if (beacon_tsop.debug_frame) {
+ printf("STA ID=%d dist0=%2.2f angle0=%2.2f dist1=%2.2f angle1=%2.2f\r\n",
+ beacon_id, dist0, angle0 * 180. / M_PI, dist1, angle1 * 180. / M_PI);
+ }
+
if (ad_to_posxya(&pos, &a, 0, &beacon0, &beacon1, angle0, dist0,
angle1, dist1) < 0)
return;
xmit_static((uint16_t)pos.x, (uint16_t)pos.y, (uint16_t)a);
}
+static int8_t check_opp_frame(uint16_t frame, uint16_t time)
+{
+ int8_t beacon_id = -1;
+
+ /* ignore bad cksum */
+ if (verify_cksum(frame) == 0xFFFF)
+ goto fail;
+
+ beacon_id = (frame >> TSOP_OPP_BEACON_ID_SHIFT) & TSOP_OPP_BEACON_ID_MASK;
+ if (beacon_id != TSOP_OPP_BEACON_ID)
+ goto fail;
+
+ /* if motor speed is not good, skip values */
+ if (current_motor_period < MOTOR_PERIOD_MIN)
+ goto fail;
+ if (current_motor_period > MOTOR_PERIOD_MAX)
+ goto fail;
+
+ return beacon_id;
+ fail:
+ /* display if needed */
+ if (beacon_tsop.debug_frame) {
+ printf("OPP ID=%d frame=%x time=%d\r\n",
+ beacon_id, frame, time);
+ }
+ return -1;
+}
+
/* process the received frame ring */
static void process_opp_ring(struct frame_status *status)
{
uint8_t head_next;
uint16_t frame;
uint8_t found = 0;
- uint8_t beacon_id;
uint16_t data, time, ref_time;
double angle;
double dist;
head_next = (status->head+1) & FRAME_RING_MASK;
frame = status->ring[status->head].frame;
- /* ignore bad cksum */
- if (verify_cksum(frame) == 0xFFFF)
- continue;
-
- beacon_id = (frame >> TSOP_OPP_BEACON_ID_SHIFT) & TSOP_OPP_BEACON_ID_MASK;
- if (beacon_id != TSOP_OPP_BEACON_ID)
- continue;
-
- /* if motor speed is not good, skip values */
- if (current_motor_period < MOTOR_PERIOD_MIN)
- continue;
- if (current_motor_period > MOTOR_PERIOD_MAX)
+ if (check_opp_frame(frame, status->ring[status->head].time) < 0) {
+ status->head = head_next;
continue;
+ }
found = 1;
data = (frame >> TSOP_OPP_FRAME_DATA_SHIFT) & TSOP_OPP_FRAME_DATA_MASK;
time = status->ring[status->head].time;
ref_time = status->ring[status->head].ref_time;
- /* display if needed */
- if (beacon_tsop.debug_frame) {
- printf("OPP ID=%d data=%d time=%d\r\n",
- beacon_id, data,
- status->ring[status->head].time);
- }
status->head = head_next;
}
return; /* fail */
angle *= 3600; /* angle in 1/10 deg */
+ /* display if needed */
+ if (beacon_tsop.debug_frame) {
+ printf("OPP dist=%2.2f angle=%2.2f\r\n", dist, angle/10);
+ }
xmit_opp((uint16_t)dist, (uint16_t)angle);
}
speed = 0;
/* enabled laser when rotation speed if at least 5tr/s */
- if (speed > 5000)
+ if (1 || speed > 5000) /* XXX */
LASER_ON();
else
LASER_OFF();
out = pid_do_filter(&beacon_tsop.pid, err);
if (out < 0)
out = 0;
- /* XXX */
if (out > 3000)
out = 3000;
if (x == 0 && beacon_tsop.debug_speed)
- printf("%ld %ld %u %u / %u\n",
+ printf("%ld %ld %u %u / %u\r\n",
speed, out, diff_icr, cpt_icr, cpt);
pwm_ng_set(&beacon_tsop.pwm_motor, out);
#
-# Automatically generated by make menuconfig: don't edit
+# Automatically generated make config: don't edit
#
#
#
# Base modules
#
+
+#
+# Enable math library in generation options to see all modules
+#
CONFIG_MODULE_CIRBUF=y
# CONFIG_MODULE_CIRBUF_LARGE is not set
# CONFIG_MODULE_FIXED_POINT is not set
#
# Communication modules
#
+
+#
+# uart needs circular buffer, mf2 client may need scheduler
+#
CONFIG_MODULE_UART=y
# CONFIG_MODULE_UART_9BITS is not set
CONFIG_MODULE_UART_CREATE_CONFIG=y
# Control system modules
#
# CONFIG_MODULE_CONTROL_SYSTEM_MANAGER is not set
+
+#
+# Filters
+#
# CONFIG_MODULE_PID is not set
# CONFIG_MODULE_PID_CREATE_CONFIG is not set
# CONFIG_MODULE_RAMP is not set
#
# Radio devices
#
+
+#
+# Some radio devices require SPI to be activated
+#
# CONFIG_MODULE_CC2420 is not set
# CONFIG_MODULE_CC2420_CREATE_CONFIG is not set
#
# Crypto modules
#
+
+#
+# Crypto modules depend on utils module
+#
# CONFIG_MODULE_AES is not set
# CONFIG_MODULE_AES_CTR is not set
# CONFIG_MODULE_MD5 is not set
#
# Encodings modules
#
+
+#
+# Encoding modules depend on utils module
+#
# CONFIG_MODULE_BASE64 is not set
# CONFIG_MODULE_HAMMING is not set
#
# Debug modules
#
+
+#
+# Debug modules depend on utils module
+#
# CONFIG_MODULE_DIAGNOSTIC is not set
# CONFIG_MODULE_DIAGNOSTIC_CREATE_CONFIG is not set
# CONFIG_MODULE_ERROR is not set
return 0xffff # wrong value
-
err = 0
total = 0
for i in range(200):
#!/usr/bin/python
import sys, math
+import matplotlib.pyplot as plt
-if 1:
- RPS = 10.
- TIMER_FREQ = 2000000.
-else:
- RPS = 40.
- TIMER_FREQ = 16000000.
+
+#RPS = 10.
+RPS = 20.
+#RPS = 40.
+#TIMER_FREQ = 2000000.
+TIMER_FREQ = 16000000.
LASER_RADIUS = 25. # mm
STEPS = (1 << 9)
k = math.pow(MAX/MIN, 1./STEPS)
-def mm_to_framedist(mm):
+def mm_to_frame(mm):
d = mm
d -= MIN
d /= (MAX-MIN)
d *= 512
return d
-def framedist_to_mm(d):
+def frame_to_mm(d):
d /= 512.
d *= (MAX-MIN)
d += MIN
return d
# t is in us, result is 9 bits
-def time_to_frame(t):
+def us_to_frame(t):
# process angle from t
a = (t / (1000000./RPS)) * 2. * math.pi
# process d from a (between 20cm and 350cm)
d = LASER_RADIUS / math.sin(a/2)
- frame = int(mm_to_framedist(d))
+ frame = int(mm_to_frame(d))
return frame
-# frame is integer 9 bits, result is laserdiff time
-def frame_to_time(frame):
- d = framedist_to_mm(frame)
+# frame is integer 9 bits, result is laserdiff time in us
+def frame_to_us(frame):
+ d = frame_to_mm(frame)
a = 2 * math.asin(LASER_RADIUS/d)
- t = (a * (TIMER_FREQ/RPS)) / (2. * math.pi)
+ t = (a * (1000000./RPS)) / (2. * math.pi)
return t
-def sample_to_offset(samples, table):
- offsets = samples[:]
- for i in range(len(offsets)):
- o = offsets[i]
- framedist = mm_to_framedist(o[0])
- off = o[1] - table[int(framedist)]
- offsets[i] = framedist, off
- return offsets
-
-def linear_interpolation(offsets, framedist, time):
- if framedist <= offsets[0][0]:
- return time + offsets[0][1]
- if framedist >= offsets[-1][0]:
- return time + offsets[-1][1]
-
- #print (offsets, framedist, time)
- o_prev = offsets[0]
- for o in offsets[1:]:
- if framedist > o[0]:
- o_prev = o
- continue
- x = (framedist - o_prev[0]) / (o[0] - o_prev[0])
- return time + o_prev[1] + (x * (o[1] - o_prev[1]))
- return None
-
-#x = time_to_frame(float(sys.argv[1]))
-#frame_to_distance(x)
-#frame_to_time(int(sys.argv[1]))
-
-
-table = [0] * 512
-for i in range(512):
- table[i] = frame_to_time(i)
+# theorical: laser timediff to robot distance
+def us_to_mm(us):
+ return frame_to_mm(us_to_frame(us))
+
+# theorical: robot distance to laserdiff
+def mm_to_us(mm):
+ return frame_to_us(mm_to_frame(mm))
+
+def time_us_to_tick(us):
+ return (us / 1000000.) * TIMER_FREQ
+
+def time_tick_to_us(t):
+ return (t * 1000000.) / TIMER_FREQ
+
-# linear correction: distance_mm, time
+##################
+
+# linear correction: distance_mm, time_us
+# must be ordered
samples = [
- (250., 7600.),
- (500., 3000.),
- (3000., 400.),
+ (250., 2201.),
+ (450., 701.),
+ (1200., 231.),
+ (3000., 50.),
]
-offsets = sample_to_offset(samples, table)
+dist_mm = map(frame_to_mm, range(512))
+
+# theorical curve
+theorical = [0] * 512
+for i in range(512):
+ theorical[i] = frame_to_us(i)
+
+# find offset and update theorical curve
+off = samples[-1][1] - mm_to_us(3000.)
+print "offset=%f"%(off)
+theo_off = [0] * 512
+for i in range(512):
+ mm = frame_to_mm(i)
+ theo_off[i] = mm_to_us(mm) + off
+
+final = [0] * 512
+for i in range(512):
+ mm = frame_to_mm(i)
+
+ # find between which samples we are
+ smp = 0
+ while smp < (len(samples) - 2):
+ if samples[smp+1][0] >= mm:
+ break
+ smp += 1
+
+ mm_start = us_to_mm(samples[smp][1] - off)
+ mm_end = us_to_mm(samples[smp+1][1] - off)
+
+ # interpolation
+ ratio = (mm - samples[smp][0]) / (samples[smp+1][0] - samples[smp][0])
+ mm_new = mm_start + ratio * (mm_end - mm_start)
+
+ final[i] = mm_to_us(mm_new) + off
+
+sample_idx = 0
+while sample_idx < len(samples):
+ print samples[sample_idx][1],
+ print us_to_mm(samples[sample_idx][1] - off),
+ print mm_to_us(samples[sample_idx][0])
+ sample_idx += 1
+
+
+plt.plot(
+# dist_mm, theorical, "r-",
+# dist_mm, theo_off, "b-",
+ dist_mm, final, "g-",
+ map(lambda x:x[0], samples), map(lambda x:x[1], samples), "g^",
+ )
+plt.show()
+
+
print "#include <aversive.h>"
print "#include <aversive/pgmspace.h>"
print "prog_uint16_t framedist_table[] = {"
for i in range(512):
if (i % 8) == 0:
print " ",
- print "%d,"%(int(linear_interpolation(offsets, i, table[i]))),
+# print "%d,"%(int(linear_interpolation(offsets, i, table[i]))),
+ print "%d,"%(int(table[i])),
if (i % 8 == 7):
print
print "};"
//#define NO_MODULATION
#define WAIT_LASER
-//#define MODUL_455KHZ
+#define MODUL_455KHZ
//#define MODUL_56KHZ
-#define MODUL_38KHZ
+//#define MODUL_38KHZ
//#define SPEED_40RPS
-#define SPEED_10RPS
+#define SPEED_20RPS
+//#define SPEED_10RPS
/* beacon identifier: must be odd, 3 bits */
#define BEACON_ID 0x1
#define FRAME_DATA_SHIFT 3
#if (defined MODUL_455KHZ)
-#define N_PERIODS 10
+#define N_PERIODS 15
#define N_CYCLES_0 17
#define N_CYCLES_1 17
#elif (defined MODUL_56KHZ)
/* FRAME must be odd */
/* #define FRAME 0x0B /\* in little endian 1-1-0-1 *\/ */
/* #define FRAME_LEN 4 */
-#define FRAME 0xAA5B /* in little endian */
+#define FRAME 0x4A5B /* in little endian */
#define FRAME_LEN 16
/* pin returns !0 when nothing, and 0 when laser is on photodiode */
#define MAX_INTER_TIME ((uint16_t)(8000*2)) /* t=8ms dist=10cm */
#define IR_DELAY ((uint16_t)(8000*2))
#define INTER_LASER_TIME 30 /* in ms */
+#elif (defined SPEED_20RPS)
+#define MIN_INTER_TIME ((uint16_t)(40*16)) /* t~=80us dist=350cm */
+#define MAX_INTER_TIME ((uint16_t)(2000*16)) /* t=2ms dist=? >10cm */
+#define IR_DELAY ((uint16_t)(2000*16))
+#define INTER_LASER_TIME 10 /* in ms */
#elif (defined SPEED_40RPS)
#define MIN_INTER_TIME ((uint16_t)(40*16)) /* t~=40us dist=350cm */
#define MAX_INTER_TIME ((uint16_t)(2000*16)) /* t=2ms dist=10cm */
uint16_t time1, time2;
uint16_t diff;
-#ifdef SPEED_40RPS
+#if (defined SPEED_40RPS) || (defined SPEED_20RPS)
/* set timer to 16Mhz, we will use ICP */
TCCR1A = 0;
TCCR1B = _BV(CS10);
while (1);
#endif
-#if 1
+#if 0
/* test freq ~ 400khz */
ICR1 = 38;
OCR1A = 19;
git.droids-corp.org / aversive.git / commitdiff