qi-hardware/antorcha
1
0
Fork 0
mirror of git://projects.qi-hardware.com/antorcha.git synced 2024-11-01 09:24:05 +02:00
Code

try cleaner sweep synchronization algorithm

Browse Source 
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 2012-06-25 10:48:19 -03:00
parent 023c2aa1e9
commit 00180fb287
2 changed files with 144 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

82
doc/sync.fig Normal file
View 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
View File

@ -25,7 +25,7 @@
static struct sweep sweep = { static struct sweep sweep = {
.pixel_ticks = 1000, /* 1 ms */ .pixel_ticks = 1100, /* 1.1 ms */
.left = 0, .left = 0,
.right = MAX_LINES-1, .right = MAX_LINES-1,
.forward = 1, .forward = 1,
@ -33,67 +33,83 @@ static struct sweep sweep = {
static volatile enum sync_state { static volatile enum sync_state {
IDLE, IDLE, /* undecided */
LEFT_DECEL, BWD, /* backward sweep */
LEFT_ACCEL, LEFT, /* reversing from backward to forward */
FWD_START_SWEEP, FWD, /* forward sweep */
FWD_SWEEP, RIGHT, /* reversing from forward to backward */
} state = IDLE; } 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 THRESH_HIGH 900
#define LEFT_ACCEL_THRESH 850 //#define THRESH_LOW 120
#define RIGHT_DECEL_THRESH 200 #define THRESH_HIGH 850
#define THRESH_LOW 170
static void sync_sweep(bool x, uint16_t v) static void sync_sweep(bool x, uint16_t v)
{ {
static uint32_t t; uint32_t t;
if (!x) if (!x)
return; return;
t = uptime_irq();
switch (state) { switch (state) {
default:
case IDLE: 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; break;
state = LEFT_DECEL; case RIGHT:
t = uptime_irq(); if (v < THRESH_LOW)
break; break;
case LEFT_DECEL: tR1 = t;
if (v < LEFT_DECEL_THRESH) { tR = t-tR0;
t = uptime_irq(); 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; break;
} }
}
static void submit_fwd_sweep(void)
{
#if 0 #if 0
if (v > LEFT_REVERSE_THRESH) uint32_t tIMG;
state = LEFT_ACCEL;
break; tIMG = (sweep.right-sweep.left+1)*(sweep.pixel_ticks)/2;
case LEFT_ACCEL: sweep.start_ticks = tL0+110000-tIMG/2;
if (v > LEFT_ACCEL_THRESH)
break;
#else
if (v < LEFT_REVERSE_THRESH)
break;
#endif #endif
if (sweeping) { /* we're confused */ sweep.start_ticks = uptime()+70000;
state = IDLE; sweep_image(&sweep);
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;
}
} }
@ -124,12 +140,10 @@ sei();
got = rf_recv(buf, sizeof(buf)); got = rf_recv(buf, sizeof(buf));
if (got > 2) if (got > 2)
dispatch(buf, got-2, protos); dispatch(buf, got-2, protos);
#if 1 if (wake && !sweeping) {
if (state == FWD_START_SWEEP && !sweeping) { wake = 0;
state = FWD_SWEEP; if (state == LEFT)
sweep.start_ticks = uptime()+60000; /* 60 ms */ submit_fwd_sweep();
sweep_image(&sweep); }
}
#endif
} }
} }