#include "cmdline.h"
#include "main.h"
+/* beacon identifier: must be odd, 3 bits */
+#define BEACON_ID_MASK 0x7
+#define BEACON_ID_SHIFT 0
+#define FRAME_DATA_MASK 0xFFF8
+#define FRAME_DATA_SHIFT 3
+
/******************* TSOP */
#define EICRx_TSOP EICRB /* EICRA is not ok, cannot do intr on any edge */
#define ISCx0_TSOP ISC60
#define ISCx1_TSOP ISC61
#define SIG_TSOP SIG_INTERRUPT6
-#define TSOP_READ() (PINE & 0x40)
+#define TSOP_READ() (!(PINE & 0x40))
#else
#define INTx_TSOP INT4
#define ISCx0_TSOP ISC40
#define ISCx1_TSOP ISC41
#define SIG_TSOP SIG_INTERRUPT4
-#define TSOP_READ() (PINE & 0x10)
+#define TSOP_READ() (!(PINE & 0x10))
#endif
+//#define MODUL_455KHZ
+#define MODUL_38KHZ
+
+#if (defined MODUL_455KHZ)
#define TSOP_FREQ_MHZ 0.455
-#define TSOP_PERIOD_US (1./TSOP_FREQ_MHZ)
#define N_PERIODS 10.
+#else
+#define TSOP_FREQ_MHZ 0.038
+#define N_PERIODS 15.
+#endif
+
+#define TSOP_PERIOD_US (1./TSOP_FREQ_MHZ)
#define TSOP_TIME_SHORT_US (1.5 * N_PERIODS * TSOP_PERIOD_US)
#define TSOP_TIME_LONG_US (2.5 * N_PERIODS * TSOP_PERIOD_US)
#define TSOP_TIME_LONG ((uint16_t)(TSOP_TIME_LONG_US*2))
#define FRAME_LEN 16
+#define FRAME_MASK ((1UL << FRAME_LEN) - 1)
/* frame */
static uint16_t start_angle_time;
cur_tsop = TSOP_READ();
diff_time = cur_time - prev_time;
+ if (cur_tsop)
+ LED2_ON();
+ else
+ LED2_OFF();
+
/* first rising edge */
- if (cur_tsop && diff_time > TSOP_TIME_LONG) {
+ if (len == 0 && cur_tsop && diff_time > TSOP_TIME_LONG) {
len = 1;
val = 1;
frame = 0;
mask = 1;
}
/* any short edge */
- else if (diff_time < TSOP_TIME_SHORT) {
+ else if (len != 0 && diff_time < TSOP_TIME_SHORT) {
if (len & 1) {
if (val)
frame |= mask;
len ++;
}
/* any long edge */
- else if (diff_time < TSOP_TIME_LONG) {
+ else if (len != 0 && diff_time < TSOP_TIME_LONG) {
val = !val;
if (val)
frame |= mask;
mask <<= 1;
len += 2;
}
+ /* error case, reset */
+ else {
+ len = 0;
+ }
/* end of frame */
if (len == FRAME_LEN*2) {
uint8_t tail_next = (frame_ring_tail+1) & FRAME_RING_MASK;
if (tail_next != frame_ring_head) {
- frame_ring[frame_ring_tail].frame = frame;
+ frame_ring[frame_ring_tail].frame = (frame & FRAME_MASK);
frame_ring[frame_ring_tail].time = start_angle_time;
frame_ring_tail = tail_next;
}
- if (led_cpt & 0x8)
+ if ((led_cpt & 0x7) == 0)
LED3_TOGGLE();
led_cpt ++;
}
sei();
ETIFR = _BV(ICF3);
- LED2_TOGGLE();
+ //LED2_TOGGLE();
diff_icr = (icr - prev_icr);
cpt_icr = cpt;
prev_icr = icr;
/* after CS, check if we have a new frame in ring */
if (frame_ring_head != frame_ring_tail) {
uint8_t head_next;
+ uint32_t frame;
head_next = (frame_ring_head+1) & FRAME_RING_MASK;
- if (beacon_tsop.debug_frame)
- printf("%x %d\n", frame_ring[frame_ring_head].frame,
+ frame = frame_ring[frame_ring_head].frame;
+
+ /* display if needed */
+ if (beacon_tsop.debug_frame) {
+ uint8_t beacon_id;
+ uint16_t data;
+
+ beacon_id = (frame >> BEACON_ID_SHIFT) & BEACON_ID_MASK;
+ data = (frame >> FRAME_DATA_SHIFT) & FRAME_DATA_MASK;
+ printf("ID=%d data=%d time=%d\r\n",
+ beacon_id, data,
frame_ring[frame_ring_head].time);
+ }
frame_ring_head = head_next;
}
-/*
+/*
* Copyright Droids Corporation (2009)
* Olivier Matz <zer0@droids-corp.org>
- *
+ *
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
/*
* hfuse: RSTDISBL=1 WTDON=1 SPIEN=0 CKOPT=0 EESAVE=1 BOOTSZ1=0 BOOTSZ0=0 BOOTRST=1
- * lfuse: BODLEVEL=1 BODEN=1 SUT1=1 SUT0=1 CKSEL3=1 CKSEL2=1 CKSEL1=1 CKSEL0=1
+ * lfuse: BODLEVEL=1 BODEN=1 SUT1=1 SUT0=1 CKSEL3=1 CKSEL2=1 CKSEL1=1 CKSEL0=1
*/
+//#define NO_MODULATION
+#define WAIT_LASER
+//#define MODUL_455KHZ
+//#define MODUL_56KHZ
+#define MODUL_38KHZ
+//#define SPEED_40RPS
+#define SPEED_10RPS
+
+/* beacon identifier: must be odd, 3 bits */
+#define BEACON_ID 0x1
+#define BEACON_ID_MASK 0x7
+#define BEACON_ID_SHIFT 0
+
+/* */
+#define FRAME_DATA_MASK 0xFFF8
+#define FRAME_DATA_SHIFT 3
+
+#if (defined MODUL_455KHZ)
#define N_PERIODS 10
#define N_CYCLES_0 17
#define N_CYCLES_1 17
+#elif (defined MODUL_56KHZ)
+#define N_PERIODS 15
+#define N_CYCLES_0 143
+#define N_CYCLES_1 143
+#elif (defined MODUL_38KHZ)
+#define N_PERIODS 15
+#define N_CYCLES_0 210
+#define N_CYCLES_1 210
+#else
+#error "no freq defined"
+#endif
+
#define N_CYCLES_PERIOD (N_CYCLES_0 + N_CYCLES_1)
#define LED_PORT PORTD
#define LED3_BIT 7
#define LED_TOGGLE(port, bit) do { \
- if (port & _BV(bit)) \
- port &= ~_BV(bit); \
- else \
- port |= _BV(bit); \
- } while(0)
+ if (port & _BV(bit)) \
+ port &= ~_BV(bit); \
+ else \
+ port |= _BV(bit); \
+ } while(0)
#define LED1_ON() sbi(LED_PORT, LED1_BIT)
#define LED1_OFF() cbi(LED_PORT, LED1_BIT)
#define FRAME 0xAA5B /* in little endian */
#define FRAME_LEN 16
-/* pin returns 1 when nothing, and 0 when laser is on photodiode */
-#define PHOTO_PIN PINC
-#define PHOTO1_BIT 0
-#define PHOTO2_BIT 1
-#define READ_PHOTOS() (PHOTO_PIN & (_BV(PHOTO1_BIT) | _BV(PHOTO2_BIT)))
-#define READ_PHOTO1() (PHOTO_PIN & _BV(PHOTO1_BIT))
-#define READ_PHOTO2() (PHOTO_PIN & _BV(PHOTO2_BIT))
-
-#define PHOTOS_ALL_OFF (_BV(PHOTO1_BIT) | _BV(PHOTO2_BIT))
-#define PHOTOS_ALL_ON (0)
-#define PHOTOS_1ON_2OFF (_BV(PHOTO2_BIT))
-#define PHOTOS_1OFF_2ON (_BV(PHOTO1_BIT))
-
-
-/* in cycles/64 (unit is 4 us at 16Mhz) */
-#define MAX_PHOTO_TIME ((uint8_t)25) /* t=100us len=5mm rps=40Hz dist=20cm */
-
-#define MIN_INTER_TIME ((uint8_t)12) /* t=50us len=50mm rps=40Hz dist=350cm */
-#define MAX_INTER_TIME ((uint8_t)250) /* t=1000us len=50mm rps=40Hz dist=20cm */
-
-/* in ms */
-#define INTER_LASER_TIME 10
-
-#define NO_MODULATION
-//#define WAIT_LASER
+/* pin returns !0 when nothing, and 0 when laser is on photodiode */
+#define PHOTO_PIN PINB
+#define PHOTO_BIT 0
+#define READ_PHOTO() (!!(PHOTO_PIN & (_BV(PHOTO_BIT))))
+
+/* IR_DELAY **must** be < 32768 */
+#if (defined SPEED_10RPS)
+#define MIN_INTER_TIME ((uint16_t)(160*2)) /* t~=160us dist=350cm */
+#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_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 */
+#define IR_DELAY ((uint16_t)(2000*16))
+#define INTER_LASER_TIME 10 /* in ms */
+#else
+#error "speed not defined"
+#endif
/* basic functions to transmit on IR */
static inline void xmit_0(void)
{
- uint8_t t = ((N_CYCLES_PERIOD * N_PERIODS) / 3);
+ uint16_t t = ((N_CYCLES_PERIOD * N_PERIODS) / 4);
#ifdef NO_MODULATION
cbi(IR_PORT, IR_BIT);
#else
TCCR1B = 0;
TCCR1A = 0;
#endif
- _delay_loop_1(t); /* 3 cycles per loop */
+ _delay_loop_2(t); /* 4 cycles per loop */
}
static inline void xmit_1(void)
{
- uint8_t t = ((N_CYCLES_PERIOD * N_PERIODS) / 3);
+ uint16_t t = ((N_CYCLES_PERIOD * N_PERIODS) / 4);
#ifdef NO_MODULATION
sbi(IR_PORT, IR_BIT);
#else
TCCR1B = _BV(WGM13) | _BV(WGM12);
TCNT1 = N_CYCLES_PERIOD-1;
TCCR1A = _BV(COM1A1) | _BV(WGM11);
+ ICR1 = N_CYCLES_PERIOD;
TCCR1B = _BV(WGM13) | _BV(WGM12) | _BV(CS10);
#endif
- _delay_loop_1(t); /* 3 cycles per loop */
+ _delay_loop_2(t); /* 4 cycles per loop */
}
/* transmit in manchester code */
xmit_0();
}
-/* */
-static inline int8_t wait_laser(void)
+#ifdef WAIT_LASER
+/* get the frame from laser time difference */
+static uint32_t get_frame(uint16_t laserdiff)
+{
+ uint32_t frame = 0;
+ frame |= ((uint32_t)BEACON_ID << BEACON_ID_SHIFT);
+ frame |= ((uint32_t)(laserdiff & FRAME_DATA_MASK) << FRAME_DATA_SHIFT);
+ return frame;
+}
+
+/* Wait 2 consecutive rising edges on photodiode. Return 0 on success,
+ * in this case, the 'when' pointed area is assigned to the time when
+ * IR signal should be sent. The 'laserdiff' pointer is the time
+ * between the 2 lasers, in timer unit. */
+static inline int8_t wait_laser(uint16_t *when, uint16_t *laserdiff)
{
- uint8_t photos;
- uint8_t time;
- uint8_t diff;
+ uint16_t time1, time2;
+ uint16_t diff;
+
+#ifdef SPEED_40RPS
+ /* set timer to 16Mhz, we will use ICP */
+ TCCR1A = 0;
+ TCCR1B = _BV(CS10);
+#else /* 10 RPS */
+ /* set timer to 2Mhz, we will use ICP */
+ TCCR1A = 0;
+ TCCR1B = _BV(CS11);
+#endif
- /* wait until all is off */
- while (READ_PHOTOS() != PHOTOS_ALL_OFF);
+ /* wait until all is off (inverted logic) */
+ while (READ_PHOTO() == 0);
+ TIFR = _BV(ICF1);
- /* wait an event, it must be photo1 on */
- while ((photos = READ_PHOTOS()) == PHOTOS_ALL_OFF);
+ /* wait falling edge */
+ while ((TIFR & _BV(ICF1)) == 0);
+ time1 = ICR1;
- if (photos != PHOTOS_1ON_2OFF)
- return -1;
- time = TCNT0;
LED1_ON();
- /* wait an event, it must be photo1 off */
- while ((photos = READ_PHOTOS()) == PHOTOS_1ON_2OFF) {
- diff = TCNT0 - time;
- if (diff > MAX_PHOTO_TIME)
- return -1;
- }
- if (photos != PHOTOS_ALL_OFF)
- return -1;
- //time = TCNT0;
+ /* wait a bit to avoid oscillations */
+ while ((uint16_t)(TCNT1-time1) < MIN_INTER_TIME);
+ TIFR = _BV(ICF1);
- /* wait an event, it must be photo2 on */
- while ((photos = READ_PHOTOS()) == PHOTOS_ALL_OFF) {
- diff = TCNT0 - time;
+ /* wait next falling edge + timeout */
+ while ((TIFR & _BV(ICF1)) == 0) {
+ diff = TCNT1 - time1;
if (diff > MAX_INTER_TIME)
return -1;
}
+
LED2_ON();
-
- diff = TCNT0 - time;
- if (diff < MIN_INTER_TIME)
- return -1;
- if (photos != PHOTOS_1OFF_2ON)
- return -1;
-#if 0
- time = TCNT0;
+ /* get time of 2nd laser */
+ time2 = ICR1;
+ TIFR = _BV(ICF1);
- /* wait an event, it must be photo2 off */
- while ((photos = READ_PHOTOS()) == PHOTOS_1OFF_2ON) {
- diff = TCNT0 - time;
- if (diff > MAX_PHOTO_TIME)
- return -1;
- }
- if (photos != PHOTOS_ALL_OFF)
+ /* process time difference */
+ diff = time2 - time1;
+ if (diff < MIN_INTER_TIME)
return -1;
-#endif
+
+ *when = time1 + (diff/2) + IR_DELAY;
+ *laserdiff = diff;
/* laser ok */
return 0;
}
+#endif
/* */
{
/* must be odd */
uint32_t frame = FRAME;
+#ifdef WAIT_LASER
int8_t ret;
- int16_t c;
-
+ uint16_t when = 0;
+ uint16_t diff = 0;
+#endif
/* LEDS */
LED_DDR = _BV(LED1_BIT) | _BV(LED2_BIT) | _BV(LED3_BIT);
IR_DDR |= _BV(IR_BIT);
#if 0
while (1) {
+ int16_t c;
c = uart_recv_nowait(0);
- if (c != -1)
+ if (c != -1)
printf("%c", (char)(c+1));
LED1_ON();
wait_ms(500);
#if 0
while (1) {
- if (READ_PHOTO() & _BV(PHOTO1_BIT))
+ if (READ_PHOTO())
LED1_ON();
else
LED1_OFF();
TCCR1B = _BV(WGM13) | _BV(WGM12);
#endif
+#if 0
+ /* test freq */
+ ICR1 = N_CYCLES_PERIOD;
+ OCR1A = N_CYCLES_1;
+ TCCR1B = _BV(WGM13) | _BV(WGM12);
+ TCNT1 = N_CYCLES_PERIOD-1;
+ TCCR1A = _BV(COM1A1) | _BV(WGM11);
+ TCCR1B = _BV(WGM13) | _BV(WGM12) | _BV(CS10);
+ while (1);
+#endif
+
+
/* configure timer 0, prescaler = 64 */
TCCR0 = _BV(CS01) | _BV(CS00);
while (1) {
#ifdef WAIT_LASER
- ret = wait_laser();
-
+ ret = wait_laser(&when, &diff);
+
LED1_OFF();
LED2_OFF();
if (ret)
continue;
+
+ frame = get_frame(diff);
+
+ /* wait before IR xmit */
+ while ((int16_t)(when-TCNT1) > 0);
#endif
-
-#if 1
+
LED3_ON();
/* ok, transmit frame */
xmit_bits(frame, FRAME_LEN);
-
+
+ LED3_OFF();
+
/* don't watch a laser during this time */
wait_ms(INTER_LASER_TIME);
- LED3_OFF();
-#else
- LED1_ON();
- wait_ms(1);
- LED1_OFF();
-#endif
}
return 0;
}
CONFIG_MODULE_FIXED_POINT=y
CONFIG_MODULE_VECT2=y
CONFIG_MODULE_GEOMETRY=y
+# CONFIG_MODULE_HOSTSIM is not set
CONFIG_MODULE_SCHEDULER=y
CONFIG_MODULE_SCHEDULER_STATS=y
# CONFIG_MODULE_SCHEDULER_CREATE_CONFIG is not set
# CONFIG_MODULE_ENCODERS_SPI_CREATE_CONFIG is not set
#
-# Robot specific modules
+# Robot specific modules (fixed point lib may be needed)
#
CONFIG_MODULE_ROBOT_SYSTEM=y
+# CONFIG_MODULE_ROBOT_SYSTEM_USE_F64 is not set
# CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT is not set
CONFIG_MODULE_POSITION_MANAGER=y
CONFIG_MODULE_COMPENSATE_CENTRIFUGAL_FORCE=y
#include <pwm_ng.h>
#include <timer.h>
#include <scheduler.h>
-#include <time.h>
+#include <clock_time.h>
#include <pid.h>
#include <quadramp.h>
static volatile uint8_t fessor_state = OFF;
static volatile uint32_t fessor_pos_up = 35000;
-static volatile uint32_t fessor_pos_down = 63500;
+static volatile uint32_t fessor_pos_down = 0;
static volatile uint32_t fessor_delay_up = 500;
static volatile uint32_t fessor_delay_down = 2000;
static volatile uint32_t delay = 0;
#include <rdline.h>
#include <uart.h>
#include <pwm_ng.h>
-#include <time.h>
+#include <clock_time.h>
#include <pid.h>
#include <quadramp.h>
#include <ax12.h>
#include <uart.h>
#include <pwm_ng.h>
-#include <time.h>
+#include <clock_time.h>
#include <pid.h>
#include <quadramp.h>
#include <uart.h>
#include <pwm_ng.h>
-#include <time.h>
+#include <clock_time.h>
#include <pid.h>
#include <quadramp.h>
#include <uart.h>
#include <pwm_ng.h>
-#include <time.h>
+#include <clock_time.h>
#include <encoders_microb.h>
#include <adc.h>
#include <uart.h>
#include <pwm_ng.h>
-#include <time.h>
+#include <clock_time.h>
#include <pid.h>
#include <quadramp.h>
#include <uart.h>
#include <pwm_ng.h>
-#include <time.h>
+#include <clock_time.h>
#include <encoders_microb.h>
#include <pid.h>
#include <pwm_ng.h>
#include <timer.h>
#include <scheduler.h>
-#include <time.h>
+#include <clock_time.h>
#include <adc.h>
#include <pid.h>
cs_manage(&mainboard.angle.cs);
if (mainboard.distance.on)
cs_manage(&mainboard.distance.cs);
- if (mainboard.fessor.on)
+ if (mainboard.fessor.on) {
cs_manage(&mainboard.fessor.cs);
+ fessor_manage();
+ }
if (mainboard.wheel.on)
cs_manage(&mainboard.wheel.cs);
- if (mainboard.elevator.on)
+ if (mainboard.elevator.on) {
cs_manage(&mainboard.elevator.cs);
-
- fessor_manage();
- elevator_manage();
+ elevator_manage();
+ }
}
if ((cpt & 1) && (mainboard.flags & DO_POS)) {
/* about 1.5ms (worst case without centrifugal force
mainboard.angle.on = 0;
mainboard.distance.on = 0;
mainboard.fessor.on = 1;
- mainboard.elevator.on = 1;
+ mainboard.elevator.on = 0;
mainboard.wheel.on = 1;
mainboard.flags |= DO_CS;
#include <pwm_ng.h>
#include <timer.h>
#include <scheduler.h>
-#include <time.h>
+#include <clock_time.h>
#include <adc.h>
#include <pid.h>
#include <uart.h>
#include <pwm_ng.h>
-#include <time.h>
+#include <clock_time.h>
#include <pid.h>
#include <quadramp.h>
#include <uart.h>
#include <pwm_ng.h>
-#include <time.h>
+#include <clock_time.h>
#include <pid.h>
#include <quadramp.h>
return a;
}
-/** do a modulo 2.pi -> [-Pi,+Pi], knowing that 'a' is in [-3Pi,+3Pi] */
-double simple_modulo_2pi(double a)
-{
- if (a < -M_PI) {
- a += M_2PI;
- }
- else if (a > M_PI) {
- a -= M_2PI;
- }
- return a;
-}
+/* /\** do a modulo 2.pi -> [-Pi,+Pi], knowing that 'a' is in [-3Pi,+3Pi] *\/ */
+/* double simple_modulo_2pi(double a) */
+/* { */
+/* if (a < -M_PI) { */
+/* a += M_2PI; */
+/* } */
+/* else if (a > M_PI) { */
+/* a -= M_2PI; */
+/* } */
+/* return a; */
+/* } */
/* return the distance to a point in the area */
int16_t angle_abs_to_rel(int16_t a_abs)
a1,ea1 = get_angle((real_x, real_y), beacons[1])
a0 -= real_a
a1 -= real_a
- text = "a0 = %2.2f (%+2.2f deg)\n"%(a0, ea0*(180./math.pi))
+ text = "real_pt = %2.2f, %2.2f, %2.2f\n"%(real_x, real_y, real_a)
+ text += "a0 = %2.2f (%+2.2f deg)\n"%(a0, ea0*(180./math.pi))
text += "a1 = %2.2f (%+2.2f deg)\n"%(a1, ea1*(180./math.pi))
d0,ed0 = get_distance((real_x, real_y), beacons[0])
d1,ed1 = get_distance((real_x, real_y), beacons[1])
ax.plot(x, y, 'g-')
result_pt = (-1, -1)
+ result_x, result_y, result_a = -1, -1, -1
patches = []
for l in s.split("\n"):
m = re.match("circle: x=%s y=%s r=%s"%(FLOAT, FLOAT, FLOAT), l)
float(m.groups()[2]), float(m.groups()[3]))
if (n == 0):
patches += [ Circle((x, y), 20, alpha=0.4, facecolor="yellow") ]
- result_pt = (x, y)
+ result_x, result_y = (x, y)
+ result_pt = (x,y)
+ result_a = a
text += l + "\n"
pcol.append(PatchCollection(patches, facecolor="none", alpha = 0.6))
(2200., 1800.), (2200., 500.)]
l.sort(cmp=lambda p1,p2: (dist(p1,real_pt)<dist(p2,real_pt)) and 1 or -1)
x,y = l[0]
- text += "real_pt: x=%2.2f, y=%2.2f\n"%(real_x, real_y)
- text += "error = %2.2f mm"%(dist(real_pt, result_pt))
+ text += "result_pt: x=%2.2f, y=%2.2f, a=%2.2f\n"%(result_x, result_y, result_a)
+ error_dist = dist(real_pt, result_pt)
+ error_a = result_a - real_a
+ if error_a > math.pi:
+ error_a -= 2*math.pi
+ if error_a < -math.pi:
+ error_a += 2*math.pi
+ text += "error = %2.2f mm, %2.2f deg"%(error_dist, error_a * 180. / math.pi)
matplotlib.pyplot.text(x, y, text, size=8,
ha="center", va="center",
bbox = dict(boxstyle="round",
print "---- random %d"%i
x = random.randint(0, 3000)
y = random.randint(0, 2100)
- a = random.random()*2*math.pi
+ a = random.random()*2*math.pi - math.pi
graph("angle/test%d.png"%i, x, y, a)
graph_da("da/test_da%d.png"%i, x, y, a)
fig.savefig(filename)
def get_data(cmd, sat=0):
- data = np.array([[0.]*210]*300)
+ data = np.array([[50.]*210]*300)
oo,ii = popen2.popen2(cmd)
ii.close()
while True:
do_graph_2d(data, "simple_error/error_a%d_%s.png"%(i,j), title)
def do_graph_2d_ad_error():
- for d in ["0.0", "0.1", "0.5"]:
- for a in ["0.0", "0.1", "0.5"]:
+ for d in ["0.0", "0.1", "0.5", "1.0"]:
+ for a in ["0.0", "0.1", "0.5", "1.0"]:
for i in ["0", "1", "2"]:
print "do_graph_2d_ad_error %s %s %s"%(i, d, a)
- data = get_data("./main da_error %s %s %s"%(i, d, a))
+ data = get_data("./main da_error %s %s -%s"%(i, d, a))
title = 'Erreur de position en mm, pour une erreur\n'
title += "d'angle de %s deg et dist de %s %% (algo %s)"%(a, d, i)
do_graph_2d(data, "da_error/error_da_%s_%s_%s.png"%(i, d, a), title)
+def do_graph_2d_ad_error_mm():
+ for d in ["5", "10", "20"]:
+ for a in ["0.0", "0.1", "0.5", "1.0"]:
+ print "do_graph_2d_ad_error_mm 0 %s %s"%(d, a)
+ data = get_data("./main da_error_mm 0 %s -%s"%(d, a))
+ title = 'Erreur de position en mm, pour une erreur\n'
+ title += "d'angle de %s deg et dist de %s mm"%(a, d)
+ do_graph_2d(data, "da_error_mm/error_da_%smm_%s.png"%(d, a), title)
+
def do_graph_2d_move_error():
i = 0
for period in [ 20, 40 ]:
"En rouge, l'erreur de mesure est > 2cm (pour un deplacement\n"
'a %2.2f m/s vers %d deg et une periode tourelle = %d ms)'%(speed, angle_deg, period))
-do_random_test()
-do_graph_2d_simple_error()
-do_graph_2d_move_error()
-do_graph_2d_ad_error()
+#do_random_test()
+#do_graph_2d_simple_error()
+#do_graph_2d_move_error()
+#do_graph_2d_ad_error()
+do_graph_2d_ad_error_mm()
tmp_a01_p1 = atan2(b1->y-p1.y, b1->x-p1.x) -
atan2(b0->y-p1.y, b0->x-p1.x);
+ if (tmp_a01_p1 > M_PI)
+ tmp_a01_p1 -= 2*M_PI;
+ if (tmp_a01_p1 < -M_PI)
+ tmp_a01_p1 += 2*M_PI;
dprintf("a01-1: %2.2f\n", tmp_a01_p1);
tmp_a01_p2 = atan2(b1->y-p2.y, b1->x-p2.x) -
atan2(b0->y-p2.y, b0->x-p2.x);
+ if (tmp_a01_p2 > M_PI)
+ tmp_a01_p2 -= 2*M_PI;
+ if (tmp_a01_p2 < -M_PI)
+ tmp_a01_p2 += 2*M_PI;
dprintf("a01-2: %2.2f\n", tmp_a01_p2);
/* in some conditions, we already know the position of the
return -1;
}
+static int8_t
+ad_to_posxy_algo(point_t *pos, int algo,
+ const point_t *b0, const point_t *b1, /* beacon position */
+ const circle_t *cd0, const circle_t *cd1, const circle_t *c01, /* circles to intersect */
+ double a01, /* seen angle of beacons */
+ double d0, double d1 /* distance to beacons */ )
-/* get the position and angle of the robot from the angle of the 2
- * beacons, and the distance of 2 beacons */
-static int8_t ad_to_posxya(point_t *pos, double *a, int algo,
- const point_t *b0, const point_t *b1, /* beacon position */
- double a0, double a1, /* seen angle of beacons */
- double d0, double d1 /* distance to beacons */ )
{
- circle_t cd0, cd1, c01;
- double a01;
-
- dprintf("algo=%d a0=%2.2f a1=%2.2f d0=%2.2f d1=%2.2f\n",
- algo, a0, a1, d0, d1);
-
- /* the first 2 circles depends on distance between robot and
- * beacons */
- cd0.x = b0->x;
- cd0.y = b0->y;
- cd0.r = d0;
- dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", cd0.x, cd0.y, cd0.r);
- cd1.x = b1->x;
- cd1.y = b1->y;
- cd1.r = d1;
- dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", cd1.x, cd1.y, cd1.r);
-
- /* the third circle depends on angle between b0 and b1 */
- a01 = a1-a0;
- if (angle_to_circles(&c01, NULL, b0, b1, a01))
- return -1;
- dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", c01.x, c01.y, c01.r);
-
switch (algo) {
/* take the closer beacon first */
case 0:
if (d0 < d1) {
- if (ad_to_posxy_one(pos, b0, b1, &cd0, &c01, a01) == 0)
+ if (ad_to_posxy_one(pos, b0, b1, cd0, c01, a01) == 0)
return 0;
- if (ad_to_posxy_one(pos, b0, b1, &cd1, &c01, a01) == 0)
+ if (ad_to_posxy_one(pos, b0, b1, cd1, c01, a01) == 0)
return 0;
return -1;
}
else {
- if (ad_to_posxy_one(pos, b0, b1, &cd1, &c01, a01) == 0)
+ if (ad_to_posxy_one(pos, b0, b1, cd1, c01, a01) == 0)
return 0;
- if (ad_to_posxy_one(pos, b0, b1, &cd0, &c01, a01) == 0)
+ if (ad_to_posxy_one(pos, b0, b1, cd0, c01, a01) == 0)
return 0;
return -1;
}
break;
case 1:
/* b0 only */
- if (ad_to_posxy_one(pos, b0, b1, &cd0, &c01, a01) == 0)
+ if (ad_to_posxy_one(pos, b0, b1, cd0, c01, a01) == 0)
return 0;
break;
case 2:
/* b0 only */
- if (ad_to_posxy_one(pos, b0, b1, &cd1, &c01, a01) == 0)
+ if (ad_to_posxy_one(pos, b0, b1, cd1, c01, a01) == 0)
return 0;
break;
default:
return -1;
}
+/* get the position and angle of the robot from the angle of the 2
+ * beacons, and the distance of 2 beacons */
+static int8_t
+ad_to_posxya(point_t *pos, double *a, int algo,
+ const point_t *b0, const point_t *b1, /* beacon position */
+ double a0, double a1, /* seen angle of beacons */
+ double d0, double d1 /* distance to beacons */ )
+{
+ circle_t cd0, cd1, c01;
+ double a01;
+
+ dprintf("algo=%d a0=%2.2f a1=%2.2f d0=%2.2f d1=%2.2f\n",
+ algo, a0, a1, d0, d1);
+
+ /* the first 2 circles depends on distance between robot and
+ * beacons */
+ cd0.x = b0->x;
+ cd0.y = b0->y;
+ cd0.r = d0;
+ dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", cd0.x, cd0.y, cd0.r);
+ cd1.x = b1->x;
+ cd1.y = b1->y;
+ cd1.r = d1;
+ dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", cd1.x, cd1.y, cd1.r);
+
+ /* the third circle depends on angle between b0 and b1 */
+ a01 = a1-a0;
+ if (a01 < -M_PI)
+ a01 += 2*M_PI;
+ if (a01 > M_PI)
+ a01 -= 2*M_PI;
+
+ if (angle_to_circles(&c01, NULL, b0, b1, a01))
+ return -1;
+ dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", c01.x, c01.y, c01.r);
+
+ /* get the xy position, depending on algo */
+ if (ad_to_posxy_algo(pos, algo, b0, b1, &cd0, &cd1, &c01, a01, d0, d1) < 0)
+ return -1;
+
+ /* now, we have the xy position, we can get angle thanks to
+ * the angles a0 and a1. Take the far beacon. */
+ if (d0 < d1)
+ *a = atan2(beacon1.y - pos->y, beacon1.x - pos->x) - a1;
+ else
+ *a = atan2(beacon0.y - pos->y, beacon0.x - pos->x) - a0;
+ if (*a < -M_PI)
+ *a += 2*M_PI;
+ if (*a > M_PI)
+ *a -= 2*M_PI;
+ return 0;
+}
+
/* get the angles of beacons from xy pos */
int8_t posxy_to_abs_angles(point_t pos, double *a0, double *a1,
double *a2, int err_num, float err_val)
for (x=0; x<300; x++) {
for (y=0; y<210; y++) {
+
pos.x = x*10;
pos.y = y*10;
posxy_to_angles(pos, &a01, &a12, &a20,
}
/* da_error algo errpercent errdeg */
- if (argc == 5 && strcmp(mode, "da_error") == 0) {
+ if ((argc == 5 && strcmp(mode, "da_error") == 0) ||
+ (argc == 5 && strcmp(mode, "da_error_mm") == 0)) {
int x, y, algo;
double err_val_deg;
double err_val_percent;
+ double err_val_mm;
double err, d0, d1, a;
algo = atoi(argv[2]);
err_val_percent = atof(argv[3]); /* how many % of error for dist */
+ err_val_mm = atof(argv[3]); /* how many mm of error for dist */
err_val_deg = atof(argv[4]); /* how many degrees of error */
for (x=0; x<300; x++) {
posxy_to_abs_angles(pos, &a0, &a1, &a2,
0, err_val_deg);
d0 = pt_norm(&pos, &beacon0);
- d0 += d0 * err_val_percent / 100.;
d1 = pt_norm(&pos, &beacon1);
- d1 += d1 * err_val_percent / 100.;
+ if (strcmp(mode, "da_error") == 0) {
+ d0 += d0 * err_val_percent / 100.;
+ d1 += d1 * err_val_percent / 100.;
+ }
+ else {
+ d0 += err_val_mm;
+ d1 += err_val_mm;
+ }
if (ad_to_posxya(&tmp, &a, algo, &beacon0, &beacon1,
a0, a1, d0, d1) < 0)
git.droids-corp.org / aversive.git / commitdiff