mirror of
git://projects.qi-hardware.com/antorcha.git
synced 2024-11-22 08:05:20 +02:00
try cleaner sweep synchronization algorithm
The predictive part doesn't produce useful results so far, so we still use a fixed offset from the trigger.
This commit is contained in:
Werner Almesberger
parent
023c2aa1e9
commit
00180fb287
82
doc/sync.fig
Normal file
82
doc/sync.fig
Normal file
@ -0,0 +1,82 @@
|
||||
#FIG 3.2 Produced by xfig version 3.2.5b
|
||||
Landscape
|
||||
Center
|
||||
Metric
|
||||
A4
|
||||
100.00
|
||||
Single
|
||||
-2
|
||||
1200 2
|
||||
2 1 1 2 0 7 40 -1 -1 6.000 0 0 -1 0 0 2
|
||||
450 7425 11025 7425
|
||||
2 1 1 2 0 7 40 -1 -1 6.000 0 0 -1 0 0 2
|
||||
450 2475 11025 2475
|
||||
2 1 0 2 0 7 35 -1 -1 6.000 0 0 -1 0 0 2
|
||||
1575 3150 10350 3150
|
||||
2 1 0 1 0 7 30 -1 -1 4.000 0 0 -1 0 0 2
|
||||
3375 2250 3375 5175
|
||||
2 1 0 1 0 7 30 -1 -1 4.000 0 0 -1 0 0 2
|
||||
4680 2250 4680 5175
|
||||
2 1 0 1 0 7 30 -1 -1 4.000 0 0 -1 0 0 2
|
||||
6525 2250 6525 5175
|
||||
2 1 0 1 0 7 30 -1 -1 4.000 0 0 -1 0 0 2
|
||||
7830 2250 7830 3825
|
||||
2 1 0 1 0 7 30 -1 -1 4.000 0 0 -1 0 0 2
|
||||
2970 6075 2970 7650
|
||||
2 1 0 1 0 7 30 -1 -1 4.000 0 0 -1 0 0 2
|
||||
1620 6075 1620 7650
|
||||
2 1 0 1 0 7 30 -1 -1 4.000 0 0 -1 0 0 2
|
||||
5040 4725 5040 7650
|
||||
2 1 0 1 0 7 30 -1 -1 4.000 0 0 -1 0 0 2
|
||||
6210 6075 6210 7650
|
||||
2 1 0 1 0 7 30 -1 -1 4.000 0 0 -1 0 0 2
|
||||
8280 6075 8280 7650
|
||||
2 1 0 1 0 7 30 -1 -1 4.000 0 0 -1 0 0 2
|
||||
9630 6075 9630 7650
|
||||
2 1 0 1 0 7 25 -1 -1 0.000 0 0 -1 1 1 2
|
||||
1 1 1.00 60.00 60.00
|
||||
1 1 1.00 60.00 60.00
|
||||
4680 4950 5040 4950
|
||||
2 1 0 2 0 7 35 -1 -1 6.000 0 0 -1 0 0 2
|
||||
1125 6750 10350 6750
|
||||
2 1 0 1 0 7 25 -1 -1 0.000 0 0 -1 1 1 2
|
||||
1 1 1.00 60.00 60.00
|
||||
1 1 1.00 60.00 60.00
|
||||
3375 4950 4680 4950
|
||||
2 1 0 1 0 7 25 -1 -1 0.000 0 0 -1 1 0 2
|
||||
1 1 1.00 60.00 60.00
|
||||
6525 4950 8100 4950
|
||||
2 2 0 1 0 7 20 -1 -1 0.000 0 0 -1 0 0 5
|
||||
7650 4725 8550 4725 8550 5175 7650 5175 7650 4725
|
||||
2 1 0 1 0 7 25 -1 -1 0.000 0 0 -1 1 1 2
|
||||
1 1 1.00 60.00 60.00
|
||||
1 1 1.00 60.00 60.00
|
||||
7650 5400 8550 5400
|
||||
3 2 0 3 0 7 45 -1 -1 0.000 0 0 0 17
|
||||
1350 4725 1620 6705 2250 7425 3015 6660 3330 3195 3960 2475
|
||||
4725 3240 4995 6615 5625 7380 6255 6570 6525 3150 7155 2430
|
||||
7875 3195 8235 6615 9000 7380 9585 6885 9900 4725
|
||||
0.000 -1.000 -1.000 -1.000 -1.000 -1.000 -1.000 -1.000
|
||||
-1.000 -1.000 -1.000 -1.000 -1.000 -1.000 -1.000 -1.000
|
||||
0.000
|
||||
3 2 0 1 0 7 50 -1 -1 0.000 0 0 0 7
|
||||
1350 4725 2475 7785 4050 1575 5625 7875 7155 1575 8775 7920
|
||||
9900 4725
|
||||
0.000 -1.000 -1.000 -1.000 -1.000 -1.000 0.000
|
||||
4 0 0 40 -1 18 12 0.0000 4 150 1140 11115 2565 Sensor limit\001
|
||||
4 0 0 40 -1 18 12 0.0000 4 150 1140 11115 7470 Sensor limit\001
|
||||
4 1 0 40 -1 18 12 0.0000 4 150 1485 4050 1485 Real aceleration\001
|
||||
4 0 0 35 -1 18 12 0.0000 4 195 1380 10440 3240 High threshold\001
|
||||
4 0 0 35 -1 18 12 0.0000 4 150 1335 10440 6795 Low threshold\001
|
||||
4 2 0 30 -1 18 12 0.0000 4 150 450 3060 4770 BWD\001
|
||||
4 1 0 30 -1 18 12 0.0000 4 150 495 4050 2790 LEFT\001
|
||||
4 1 0 30 -1 18 12 0.0000 4 150 600 2340 7200 RIGHT\001
|
||||
4 0 0 30 -1 18 12 0.0000 4 150 435 4995 4185 FWD\001
|
||||
4 0 0 25 -1 18 12 0.0000 4 150 300 3060 7020 tR1\001
|
||||
4 0 0 25 -1 18 12 0.0000 4 150 285 3465 3420 tL0\001
|
||||
4 0 0 25 -1 18 12 0.0000 4 150 285 4770 3060 tL1\001
|
||||
4 0 0 25 -1 18 12 0.0000 4 150 300 5130 6660 tR0\001
|
||||
4 0 0 25 -1 18 12 0.0000 4 150 180 5130 5040 tF\001
|
||||
4 1 0 25 -1 18 12 0.0000 4 150 180 4095 4860 tL\001
|
||||
4 1 0 20 -1 18 12 0.0000 4 150 435 8865 5490 tIMG\001
|
||||
4 1 0 20 -1 18 12 0.0000 4 150 645 7110 4860 tL+tF/2\001
|
||||
116
fw/antorcha.c
116
fw/antorcha.c
@ -25,7 +25,7 @@
|
||||
|
||||
|
||||
static struct sweep sweep = {
|
||||
.pixel_ticks = 1000, /* 1 ms */
|
||||
.pixel_ticks = 1100, /* 1.1 ms */
|
||||
.left = 0,
|
||||
.right = MAX_LINES-1,
|
||||
.forward = 1,
|
||||
@ -33,67 +33,83 @@ static struct sweep sweep = {
|
||||
|
||||
|
||||
static volatile enum sync_state {
|
||||
IDLE,
|
||||
LEFT_DECEL,
|
||||
LEFT_ACCEL,
|
||||
FWD_START_SWEEP,
|
||||
FWD_SWEEP,
|
||||
IDLE, /* undecided */
|
||||
BWD, /* backward sweep */
|
||||
LEFT, /* reversing from backward to forward */
|
||||
FWD, /* forward sweep */
|
||||
RIGHT, /* reversing from forward to backward */
|
||||
} state = IDLE;
|
||||
|
||||
static volatile uint32_t tR0, tR1, tL0, tL1;
|
||||
static volatile uint32_t tL, tR;
|
||||
static volatile bool wake = 0;
|
||||
|
||||
#define LEFT_DECEL_THRESH 850
|
||||
#define LEFT_REVERSE_THRESH 900
|
||||
#define LEFT_ACCEL_THRESH 850
|
||||
#define RIGHT_DECEL_THRESH 200
|
||||
|
||||
//#define THRESH_HIGH 900
|
||||
//#define THRESH_LOW 120
|
||||
#define THRESH_HIGH 850
|
||||
#define THRESH_LOW 170
|
||||
|
||||
|
||||
static void sync_sweep(bool x, uint16_t v)
|
||||
{
|
||||
static uint32_t t;
|
||||
uint32_t t;
|
||||
|
||||
if (!x)
|
||||
return;
|
||||
t = uptime_irq();
|
||||
switch (state) {
|
||||
default:
|
||||
case IDLE:
|
||||
if (v < LEFT_DECEL_THRESH)
|
||||
if (v < THRESH_LOW) {
|
||||
tR0 = t;
|
||||
state = RIGHT;
|
||||
} else if (v > THRESH_HIGH) {
|
||||
tL0 = t;
|
||||
state = LEFT;
|
||||
}
|
||||
break;
|
||||
state = LEFT_DECEL;
|
||||
t = uptime_irq();
|
||||
case RIGHT:
|
||||
if (v < THRESH_LOW)
|
||||
break;
|
||||
case LEFT_DECEL:
|
||||
if (v < LEFT_DECEL_THRESH) {
|
||||
t = uptime_irq();
|
||||
tR1 = t;
|
||||
tR = t-tR0;
|
||||
state = BWD;
|
||||
/* fall through */
|
||||
case BWD:
|
||||
if (v < THRESH_HIGH)
|
||||
break;
|
||||
tL0 = t;
|
||||
state = LEFT;
|
||||
wake = 1;
|
||||
break;
|
||||
case LEFT:
|
||||
if (v > THRESH_HIGH)
|
||||
break;
|
||||
tL1 = t;
|
||||
tL = t-tL0;
|
||||
state = FWD;
|
||||
/* fall through */
|
||||
case FWD:
|
||||
if (v > THRESH_LOW)
|
||||
break;
|
||||
tR0 = t;
|
||||
state = RIGHT;
|
||||
// wake = 1;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
static void submit_fwd_sweep(void)
|
||||
{
|
||||
#if 0
|
||||
if (v > LEFT_REVERSE_THRESH)
|
||||
state = LEFT_ACCEL;
|
||||
break;
|
||||
case LEFT_ACCEL:
|
||||
if (v > LEFT_ACCEL_THRESH)
|
||||
break;
|
||||
#else
|
||||
if (v < LEFT_REVERSE_THRESH)
|
||||
break;
|
||||
uint32_t tIMG;
|
||||
|
||||
tIMG = (sweep.right-sweep.left+1)*(sweep.pixel_ticks)/2;
|
||||
sweep.start_ticks = tL0+110000-tIMG/2;
|
||||
#endif
|
||||
if (sweeping) { /* we're confused */
|
||||
state = IDLE;
|
||||
break;
|
||||
}
|
||||
state = FWD_START_SWEEP;
|
||||
/* t = t1-(t1+t0)/2 = (t1-t0)/2 */
|
||||
t = (uptime_irq()-t) >> 1;
|
||||
// sweep.wait_ticks = kkkkkk
|
||||
break;
|
||||
case FWD_START_SWEEP:
|
||||
break;
|
||||
case FWD_SWEEP:
|
||||
if (v > RIGHT_DECEL_THRESH)
|
||||
break;
|
||||
state = IDLE;
|
||||
break;
|
||||
}
|
||||
sweep.start_ticks = uptime()+70000;
|
||||
sweep_image(&sweep);
|
||||
}
|
||||
|
||||
|
||||
@ -124,12 +140,10 @@ sei();
|
||||
got = rf_recv(buf, sizeof(buf));
|
||||
if (got > 2)
|
||||
dispatch(buf, got-2, protos);
|
||||
#if 1
|
||||
if (state == FWD_START_SWEEP && !sweeping) {
|
||||
state = FWD_SWEEP;
|
||||
sweep.start_ticks = uptime()+60000; /* 60 ms */
|
||||
sweep_image(&sweep);
|
||||
}
|
||||
#endif
|
||||
if (wake && !sweeping) {
|
||||
wake = 0;
|
||||
if (state == LEFT)
|
||||
submit_fwd_sweep();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user