void check_for_qtr_shorts()
{
// Test for shorts
unsigned int sensor_pins[] = {IO_C0,IO_C1,IO_C2,IO_C3,IO_C4};
unsigned char shorts = 1;
clear();
print("qtr");
lcd_goto_xy(0,1);
print("shorts");
while (shorts)
{
unsigned char i;
shorts = 0;
for(i=0;i<5;i++)
{
unsigned char j;
for (j=0;j<5;j++)
{
set_digital_input(sensor_pins[j],PULL_UP_ENABLED);
}
set_digital_output(sensor_pins[i],LOW);
delay_ms(2);
for (j=0;j<5;j++)
{
if (i == j && is_digital_input_high(sensor_pins[j]))
shorts = 1;
if (i != j && !is_digital_input_high(sensor_pins[j]))
shorts = 1;
}
}
}
}
int main()
{
// Make PC1 be an input with its internal pull-up resistor enabled.
// It will read high when nothing is connected to it.
set_digital_input(IO_C1, PULL_UP_ENABLED);
while(1)
{
if(is_digital_input_high(IO_C1)) // Take digital reading of PC1.
{
set_digital_output(IO_D1, HIGH); // PC1 is high, so drive PD1 high.
}
else
{
set_digital_output(IO_D1, LOW); // PC1 is low, so drive PD1 low.
}
}
}
void test_fsm(u08 cmd, u08 *param)
{
//enum states { s_disabled=0, s_tracking_wall=1, s_lost_wall=2, s_turning_corner=3, s_turning_sharp_corner=4 };
static u08 state=0;
static u08 last_state=255;
static u32 t_start;
static s16 bias=0;
static u08 count;
u32 t_delta;
u08 i;
t_config_value v;
static u08 initialized=0;
if(!initialized)
{
initialized=1;
usb_printf("wall_follow_fsm()\n");
}
if(s.behavior_state[TEST_LOGIC_FSM]==1)
{
PUMP_ON();
s.behavior_state[TEST_LOGIC_FSM]=0;
}
if(s.behavior_state[TEST_LOGIC_FSM]==2)
{
PUMP_OFF();
s.behavior_state[TEST_LOGIC_FSM]=0;
}
if(s.behavior_state[TEST_LOGIC_FSM]==3)
{
dbg_printf("start = %d\n",is_digital_input_high(IO_B3));
s.behavior_state[TEST_LOGIC_FSM]=0;
}
if(s.behavior_state[TEST_LOGIC_FSM]==4)
{
switch(state)
{
case 0:
t_start = get_ms();
motor_command(8,0,0,speed_profile[0].l_speed,speed_profile[0].r_speed);
state++;
break;
case 1:
t_delta = get_ms() - t_start;
i=0;
while(speed_profile[i].time < t_delta) i++;
motor_command(8,0,0,speed_profile[i-1].l_speed,speed_profile[i-1].r_speed);
break;
}
}
if(s.behavior_state[TEST_LOGIC_FSM]==5)
{
static s16 ne=0, nw=0;
ne = (ne + (s16) s.inputs.analog[AI_FLAME_NE])/2;
nw = (nw + (s16) s.inputs.analog[AI_FLAME_NW])/2;
bias = 0;
if( (ne>245) && (nw>245) ) bias = 0;
else if( abs(ne-nw) < 10 ) bias = 0;
else if( ne>nw ) bias = -1;
else if( nw>ne ) bias = 1;
v.u16 = cfg_get_u16_by_grp_id(15,6);
v.u16 += bias;
cfg_set_value_by_grp_id(15,6, v);
}
if(s.behavior_state[TEST_LOGIC_FSM]==6)
{
if(s.inputs.analog[AI_FLAME_NW]<80)
{
motor_command(6,3,3,-40,40);
}
else
{
motor_command(2,0,0,0,0);
s.behavior_state[TEST_LOGIC_FSM]=0;
}
}
}
void test_task(u08 cmd, u08 *param)
{
static u16 i;
float time_to_stop=0,distance_to_stop;
static t_scan_result scan_result;
DEFINE_CFG2(s16,accel, 99,1);
DEFINE_CFG2(s16,decel, 99,2);
DEFINE_CFG2(s16,speed, 99,3);
DEFINE_CFG2(s16,distance, 99,4);
DEFINE_CFG2(s16,angle, 99,5);
task_open_1();
//code between _1() and _2() will get executed every time the scheduler resumes this task
if(cmd==0)
{
NOP();
}
else
{
NOP();
return;
}
task_open_2();
//execution below this point will resume wherever it left off when a context switch happens
usb_printf("test_task()\n");
PREPARE_CFG2(accel);
PREPARE_CFG2(decel);
PREPARE_CFG2(speed);
PREPARE_CFG2(distance);
PREPARE_CFG2(angle);
test_task(1,(uint8 *)0x1234);
/*
for(;;)
{
dbg_printf("0123456789\n");
task_wait(100);
}
*/
/*
task_wait(200);
motor_command(2,0,0,0,0);
task_wait(200);
motor_command(7,0,0,100,100);
task_wait(500);
motor_command(6,2,2,0,0);
task_wait(500);
*/
while(1)
{
task_wait(100);
UPDATE_CFG2(accel);
UPDATE_CFG2(decel);
UPDATE_CFG2(speed);
UPDATE_CFG2(distance);
UPDATE_CFG2(angle);
if(s.behavior_state[TEST_LOGIC_FSM]==1)
{
/*
1) gradually ramp up (at specified rate) to target speed
2) when we are <= 30degrees from the target, start ramping down to speed 15
3) when we are <= 10degrees from the target, apply target speed 5 (w/ feed forward) & regulate to maintain 5
3) when we are at the target, hit the brakes - full stop w/out ramping down
*/
/*
odometry_set_checkpoint();
motor_command(6,1,1,(speed),-(speed));
while ( abs(odometry_get_rotation_since_checkpoint()) < 60 ) { task_wait(10); }
motor_command(6,1,1,15,-15);
while ( abs(odometry_get_rotation_since_checkpoint()) < 80 ) { task_wait(10); }
motor_command(7,1,1,5,-5);
while ( abs(odometry_get_rotation_since_checkpoint()) < 90 ) { task_wait(10); }
motor_command(7,1,1,0,0);
*/
/*
MOVE(50,100);
TURN_IN_PLACE( 50, 90);
MOVE(50,100);
TURN_IN_PLACE( 50, 90);
MOVE(50,100);
TURN_IN_PLACE( 50, 90);
MOVE(50,100);
TURN_IN_PLACE( 50, 90);
*/
#if 0
//set_digital_output(IO_D1,0);
set_digital_output(IO_D0,0);
task_wait(2000);
//set_digital_output(IO_D1,1);
set_digital_output(IO_D0,1);
#endif
#if 0
//dbg_printf("Starting scan....\n");
TURN_IN_PLACE_AND_SCAN( 40, 220 );
//dbg_printf("....done\n");
scan_result = find_peak_in_scan(scan_data,360,30);
dbg_printf("scan_result: %d,%d,%d,%d,%d,%d\n",
scan_result.flame_center_value, scan_result.rising_edge_position, scan_result.falling_edge_position,
scan_result.center_angle, scan_result.rising_edge_angle, scan_result.falling_edge_angle);
for(i=0;i<360;i++) {dbg_printf("scan_data[%03d]=%03d,%03d\n",i, scan_data[i].angle, scan_data[i].flame);task_wait(10);}
TURN_IN_PLACE( 40, -(220-scan_result.center_angle) );
#endif
TURN_IN_PLACE_AND_SCAN( 40, 90, 1);
scan_result = find_path_in_scan(scan_data, 100, 300, 0, 1);
dbg_printf("scan_result: %d,%d,%d,%d\n", scan_result.opening, scan_result.center_angle, scan_result.rising_edge_angle, scan_result.falling_edge_angle);
//for(i=0;i<100;i++) {dbg_printf("scan_data[%03d]=%03d,%03d\n",i, scan_data[i].angle, scan_data[i].ir_north);task_wait(10);}
TURN_IN_PLACE(40,-90);
/*
task_wait(2000);
TURN_IN_PLACE_AND_SCAN( 100, -90 );
scan_result = find_peak_in_scan(scan_data,360,3);
task_wait(2000);
TURN_IN_PLACE_AND_SCAN( 100, 180 );
scan_result = find_peak_in_scan(scan_data,360,3);
task_wait(2000);
TURN_IN_PLACE_AND_SCAN( 100, -180 );
scan_result = find_peak_in_scan(scan_data,360,3);
*/
s.behavior_state[TEST_LOGIC_FSM]=0;
}
if(s.behavior_state[TEST_LOGIC_FSM]==2)
{
TURN_IN_PLACE(speed,angle);
s.behavior_state[TEST_LOGIC_FSM]=0;
}
if(s.behavior_state[TEST_LOGIC_FSM]==3)
{
TURN_IN_PLACE(40, -90);
TURN_IN_PLACE_AND_SCAN(40, 180, 4);
scan_result = find_flame_in_scan(scan_data,360,30);
if(scan_result.flame_center_value > 150) //TODO: make the minimum flame value a parameter
{
static int i, i_min;
static u16 min=999;
static float d;
static u08 stop=0;
TURN_IN_PLACE( 40, -(180-scan_result.center_angle+2) );
/*
min=999;
for(i=scan_result.rising_edge_position-10; i<=scan_result.falling_edge_position+10; i++)
{
dbg_printf("scan_data[%3d]: ir_n=%3d, a=%d, f=%d\n",i,scan_data[i].ir_north, scan_data[i].angle, scan_data[i].flame);
task_wait(50);
if(scan_data[i].ir_north < min) { min=scan_data[i].ir_north; i_min=i; }
}
dbg_printf("distance to candle: %d @i=%d,a=%d\n",min,i_min,scan_data[i_min].angle);
if(min<100) min=100;
d = (float) (((min-100)*25)/10);
MOVE2(50,d,60,60);
*/
stop=0;
move_manneuver2(1,30,9999,(80),(90));
while(move_manneuver2(0,30,9999,(70),(70)))
{
OS_SCHEDULE;
if( (s.ir[IR_N] <= 60) ) stop |= 0x01;
if( (s.ir[IR_NE] <= 60)) stop |= 0x02;
if( (s.ir[IR_NW] <= 60)) stop |= 0x04;
if( (s.inputs.sonar[0] <= 100) ) stop |= 0x08;
if(stop != 0)
{
dbg_printf("too close to object/wall! reason: 0x%02x\n",stop);
HARD_STOP();
break;
}
}
}
s.behavior_state[TEST_LOGIC_FSM]=0;
}
if(s.behavior_state[TEST_LOGIC_FSM]==4)
{
PUMP_ON();
task_wait(1000);
PUMP_OFF();
s.behavior_state[TEST_LOGIC_FSM]=0;
}
if(s.behavior_state[TEST_LOGIC_FSM]==5)
{
dbg_printf("start = %d\n",is_digital_input_high(IO_B3));
task_wait(500);
}
/*
while(s.behavior_state[11]==1)
{
time_to_stop = (float)s.inputs.actual_speed[0] / (float)50;
distance_to_stop = ((float)s.inputs.actual_speed[0] * time_to_stop)/2.0;
distance_to_stop *= 50.0; //adjust for seconds
distance_to_stop *= 0.13466716824940938560464; //adjust for mm
if(s.inputs.x + distance_to_stop < distance)
{
motor_command(6,accel,decel,speed,speed);
}
else
{
motor_command(6,accel,decel,0,0);
s.behavior_state[TEST_LOGIC_FSM]=0;
}
task_wait(20);
}
*/
}
task_close();
}
