static bool
test_olc(int n_wind, Contest olc_type)
{
GlidePolar glide_polar(fixed_two);
Waypoints waypoints;
SetupWaypoints(waypoints);
if (verbose)
PrintDistanceCounts();
TaskBehaviour task_behaviour;
task_behaviour.SetDefaults();
task_behaviour.DisableAll();
if (!verbose)
task_behaviour.enable_trace = false;
TaskManager task_manager(task_behaviour, waypoints);
TaskEventsPrint default_events(verbose);
task_manager.SetTaskEvents(default_events);
task_manager.SetGlidePolar(glide_polar);
test_task(task_manager, waypoints, 1);
waypoints.Clear(); // clear waypoints so abort wont do anything
autopilot_parms.goto_target = true;
return run_flight(task_manager, autopilot_parms, n_wind);
}
static bool
test_abort(int n_wind)
{
GlidePolar glide_polar(fixed(2));
Waypoints waypoints;
SetupWaypoints(waypoints);
if (verbose)
PrintDistanceCounts();
TaskBehaviour task_behaviour;
task_behaviour.SetDefaults();
task_behaviour.DisableAll();
task_behaviour.enable_trace = false;
TaskManager task_manager(task_behaviour, waypoints);
TaskEventsPrint default_events(verbose);
task_manager.SetTaskEvents(default_events);
task_manager.SetGlidePolar(glide_polar);
test_task(task_manager, waypoints, 1);
task_manager.Abort();
task_report(task_manager, "abort");
autopilot_parms.goto_target = true;
return run_flight(task_manager, autopilot_parms, n_wind);
}
static bool
test_goto(int n_wind, unsigned id, bool auto_mc)
{
GlidePolar glide_polar(fixed(2));
Waypoints waypoints;
SetupWaypoints(waypoints);
if (verbose)
PrintDistanceCounts();
TaskBehaviour task_behaviour;
task_behaviour.SetDefaults();
task_behaviour.DisableAll();
task_behaviour.auto_mc = auto_mc;
task_behaviour.enable_trace = false;
TaskManager task_manager(task_behaviour, waypoints);
TaskEventsPrint default_events(verbose);
task_manager.SetTaskEvents(default_events);
task_manager.SetGlidePolar(glide_polar);
test_task(task_manager, waypoints, 1);
task_manager.DoGoto(*waypoints.LookupId(id));
task_report(task_manager, "goto");
waypoints.Clear(); // clear waypoints so abort wont do anything
autopilot_parms.goto_target = true;
return run_flight(task_manager, autopilot_parms, n_wind);
}
int main(int argc, char** argv)
{
if (!parse_args(argc,argv)) {
return 0;
}
#define NUM_RANDOM 50
#define NUM_TYPE_MANIPS 50
plan_tests(NUM_TASKS+2+NUM_RANDOM+8+NUM_TYPE_MANIPS);
GlidePolar glide_polar(fixed_two);
Waypoints waypoints;
setup_waypoints(waypoints);
{
TaskManager task_manager(waypoints);
task_manager.SetGlidePolar(glide_polar);
test_task_bad(task_manager,waypoints);
}
{
for (unsigned i = 0; i < NUM_TYPE_MANIPS; i++) {
TaskManager task_manager(waypoints);
ok(test_task_type_manip(task_manager,waypoints, i+2), "construction: task type manip", 0);
}
}
for (int i=0; i<NUM_TASKS+2; i++) {
TaskManager task_manager(waypoints);
task_manager.SetGlidePolar(glide_polar);
ok(test_task(task_manager, waypoints, i),test_name("construction",i,0),0);
}
for (int i=0; i<NUM_RANDOM; i++) {
TaskManager task_manager(waypoints);
task_manager.SetGlidePolar(glide_polar);
ok(test_task(task_manager, waypoints, 7),test_name("construction",7,0),0);
}
return exit_status();
}
void inifinity_function(TaskId task_id, WorkItem& item) {
fprintf(stdout, "inifinity_function(%d) %lx\n", task_id, pthread_self());
fflush(stdout);
usleep(10000);
TaskId test = task_manager->begin_add(test_task());
task_manager->finish_add(test);
task_manager->wait(test);
}
int main(){
test_loop();
test_task();
test_reduction();
test_gmove();
test_bcast();
test_reflect();
test_barrier();
#pragma xmp task on t(1)
printf("PASS\n");
return 0;
}
TestFlightResult
test_flight(TestFlightComponents components, int test_num, int n_wind,
const double speed_factor, const bool auto_mc)
{
// multipurpose flight test
GlidePolar glide_polar(fixed(2));
Waypoints waypoints;
SetupWaypoints(waypoints);
if (verbose)
PrintDistanceCounts();
TaskBehaviour task_behaviour;
task_behaviour.SetDefaults();
task_behaviour.enable_trace = false;
task_behaviour.auto_mc = auto_mc;
task_behaviour.calc_glide_required = false;
if ((test_num == 0) || (test_num == 2))
task_behaviour.optimise_targets_bearing = false;
TaskManager task_manager(task_behaviour, waypoints);
TaskEventsPrint default_events(verbose);
task_manager.SetTaskEvents(default_events);
task_manager.SetGlidePolar(glide_polar);
OrderedTaskSettings otb = task_manager.GetOrderedTask().GetOrderedTaskSettings();
otb.aat_min_time = aat_min_time(test_num);
task_manager.SetOrderedTaskSettings(otb);
bool goto_target = false;
switch (test_num) {
case 0:
case 2:
case 7:
goto_target = true;
break;
};
autopilot_parms.goto_target = goto_target;
test_task(task_manager, waypoints, test_num);
waypoints.Clear(); // clear waypoints so abort wont do anything
return run_flight(components, task_manager, autopilot_parms, n_wind,
speed_factor);
}
int
main(int argc, char* argv[])
{
ind_soc_config_t config = {0};
printf("Init returned %d\n", ind_soc_init(&config));
test_timer_mgmt();
test_periodic_timer();
test_immediate_timer();
test_socket();
test_socket_mgmt();
test_task();
test_priority();
return 0;
}
bool
test_flight(int test_num, int n_wind, const double speed_factor,
const bool auto_mc)
{
// multipurpose flight test
GlidePolar glide_polar(fixed_two);
Waypoints waypoints;
setup_waypoints(waypoints);
if (verbose)
distance_counts();
TaskEventsPrint default_events(verbose);
TaskManager task_manager(default_events, waypoints);
task_manager.set_glide_polar(glide_polar);
task_manager.get_ordered_task_behaviour().aat_min_time = aat_min_time(test_num);
TaskBehaviour task_behaviour = task_manager.get_task_behaviour();
task_behaviour.enable_trace = false;
task_behaviour.auto_mc = auto_mc;
task_behaviour.calc_glide_required = false;
if ((test_num == 0) || (test_num == 2))
task_behaviour.optimise_targets_bearing = false;
task_manager.set_task_behaviour(task_behaviour);
bool goto_target = false;
switch (test_num) {
case 0:
case 2:
case 7:
goto_target = true;
break;
};
autopilot_parms.goto_target = goto_target;
test_task(task_manager, waypoints, test_num);
waypoints.clear(); // clear waypoints so abort wont do anything
return run_flight(task_manager, autopilot_parms, n_wind,
speed_factor);
}
int main(int argc, char** argv) {
// default arguments
verbose=1;
if (!parse_args(argc,argv)) {
return 0;
}
TaskBehaviour task_behaviour;
TaskEventsPrint default_events(verbose);
GlidePolar glide_polar(fixed_two);
Waypoints waypoints;
setup_waypoints(waypoints);
TaskManager task_manager(default_events,
task_behaviour,
waypoints);
task_manager.set_glide_polar(glide_polar);
test_task(task_manager, waypoints, 0);
plan_tests(1);
ok(test_edit(task_manager, task_behaviour),
"edit task", 0);
/*
plan_tests(task_manager.task_size());
// here goes, example, edit all task points
SafeTaskEdit ste(task_manager, waypoints);
for (unsigned i=0; i<task_manager.task_size(); i++) {
ok(ste.edit(i),"edit tp",0);
task_report(task_manager, "edit tp\n");
}
*/
return exit_status();
}
void test_dma_m2m_chan1_burst16(void)
{
assert_true((test_task(1, 16) == TC_PASS), NULL);
}
/* export test cases */
void test_dma_m2m_chan0_burst8(void)
{
assert_true((test_task(0, 8) == TC_PASS), NULL);
}
void ManualOperation::work()
{
#ifndef DEBUG_FRAME_BY_FRAME
// Turn off display by default
if (image_input->can_display()) {
mvWindow::setShowImage(show_raw_images);
}
#else
// Show first image
process_image();
#endif
display_start_message();
// Take keyboard commands
bool loop = true;
while (loop) {
char c = CharacterStreamSingleton::get_instance().wait_key(WAIT_KEY_IN_MS);
// Print yaw and depth unless delayed by another message
if (c == '\0') {
if (count < 0) {
count++;
} else if (mode != VISION) {
static int attitude_counter = 0;
attitude_counter++;
if (attitude_counter == 1000 / WAIT_KEY_IN_MS / REFRESH_RATE_IN_HZ) {
attitude_counter = 0;
char buf[128];
sprintf(buf, "Yaw: %+04d degrees, Depth: %+04d cm, Target Yaw: %+04d degrees, Target Depth: %+04d",
attitude_input->yaw(), attitude_input->depth(), attitude_input->target_yaw(), attitude_input->target_depth());
message(buf);
}
}
}
switch(c) {
case 'q':
loop = false;
break;
case 'z':
dump_images();
break;
case 'y':
if (image_input->can_display()) {
show_raw_images = !show_raw_images;
mvWindow::setShowImage(show_raw_images);
} else {
message_hold("Image stream should already be displayed");
}
break;
case 'u':
use_fwd_img = !use_fwd_img;
break;
case 'i':
actuator_output->special_cmd(SIM_MOVE_FWD);
break;
case 'k':
actuator_output->special_cmd(SIM_MOVE_REV);
break;
case 'j':
actuator_output->special_cmd(SIM_MOVE_LEFT);
break;
case 'l':
actuator_output->special_cmd(SIM_MOVE_RIGHT);
break;
case 'p':
actuator_output->special_cmd(SIM_MOVE_RISE);
break;
case ';':
actuator_output->special_cmd(SIM_MOVE_SINK);
break;
case 'e':
actuator_output->stop();
break;
case 'w':
actuator_output->set_attitude_change(FORWARD, SPEED_CHG);
break;
case 's':
actuator_output->set_attitude_change(REVERSE, SPEED_CHG);
break;
case 'a':
actuator_output->set_attitude_change(LEFT, YAW_CHG_IN_DEG);
break;
case 'd':
actuator_output->set_attitude_change(RIGHT, YAW_CHG_IN_DEG);
break;
case 'r':
actuator_output->set_attitude_change(RISE, DEPTH_CHG_IN_CM);
break;
case 'f':
actuator_output->set_attitude_change(SINK, DEPTH_CHG_IN_CM);
break;
case ' ':
actuator_output->special_cmd(SIM_ACCEL_ZERO);
#ifdef DEBUG_FRAME_BY_FRAME
process_image();
#endif
break;
case '^':
actuator_output->special_cmd(SUB_POWER_ON);
break;
case '%':
actuator_output->stop();
actuator_output->special_cmd(SUB_STARTUP_SEQUENCE);
break;
case '$':
actuator_output->special_cmd(SUB_POWER_OFF);
break;
case '#':
long_input();
break;
case 'm':
endwin();
// Scope mission so that it is destructed before display_start_message
{
Mission m(attitude_input, image_input, actuator_output);
m.work_internal(true);
}
display_start_message();
message_hold("Mission complete!");
break;
case 'M': // same as mission, but force turn off show_image
endwin();
// Scope mission so that it is destructed before display_start_message
{
Mission m(attitude_input, image_input, actuator_output);
m.work();
}
display_start_message();
message_hold("Mission complete!");
break;
case '0':
if (mode != VISION) {
endwin();
MDA_TASK_RETURN_CODE ret_code;
// Scope task so that it is destructed before display_start_message
{
MDA_TASK_TEST test_task(attitude_input, image_input, actuator_output);
ret_code = test_task.run_task();
}
display_start_message();
switch(ret_code) {
case TASK_DONE:
message_hold("Test task completed successfully");
break;
case TASK_QUIT:
message_hold("Test task quit by user");
break;
default:
message_hold("Test task errored out");
break;
}
break;
}
delete vision_module;
message_hold("Selected test vision module\n");
vision_module = new MDA_VISION_MODULE_TEST();
use_fwd_img = true;
break;
case '1':
if (mode != VISION) {
endwin();
MDA_TASK_RETURN_CODE ret_code;
// Scope task so that it is destructed before display_start_message
{
MDA_TASK_GATE gate_task(attitude_input, image_input, actuator_output);
ret_code = gate_task.run_task();
}
display_start_message();
switch(ret_code) {
case TASK_DONE:
message_hold("Gate task completed successfully");
break;
case TASK_QUIT:
message_hold("Gate task quit by user");
break;
default:
message_hold("Gate task errored out");
break;
}
break;
}
delete vision_module;
message_hold("Selected gate vision module\n");
vision_module = new MDA_VISION_MODULE_GATE();
use_fwd_img = true;
break;
case '2':
if (mode != VISION) {
endwin();
MDA_TASK_RETURN_CODE ret_code;
// Scope task so that it is destructed before display_start_message
{
MDA_TASK_PATH path_task(attitude_input, image_input, actuator_output);
ret_code = path_task.run_task();
}
display_start_message();
switch(ret_code) {
case TASK_DONE:
message_hold("Path task completed successfully");
break;
case TASK_QUIT:
message_hold("Path task quit by user");
break;
default:
message_hold("Path task errored out");
break;
}
break;
}
delete vision_module;
message_hold("Selected path vision module\n");
vision_module = new MDA_VISION_MODULE_PATH();
use_fwd_img = false;
break;
case '3':
if (mode != VISION) {
endwin();
MDA_TASK_RETURN_CODE ret_code;
// Scope task so that it is destructed before display_start_message
{
MDA_TASK_BUOY buoy_task(attitude_input, image_input, actuator_output);
ret_code = buoy_task.run_task();
}
display_start_message();
switch(ret_code) {
case TASK_DONE:
message_hold("Buoy task completed successfully");
break;
case TASK_QUIT:
message_hold("Buoy task quit by user");
break;
default:
message_hold("Buoy task errored out");
break;
}
break;
}
delete vision_module;
message_hold("Selected buoy vision module\n");
vision_module = new MDA_VISION_MODULE_BUOY();
use_fwd_img = true;
break;
case '4':
if (mode != VISION) {
endwin();
MDA_TASK_RETURN_CODE ret_code;
// Scope task so that it is destructed before display_start_message
{
MDA_TASK_FRAME frame_task(attitude_input, image_input, actuator_output);
ret_code = frame_task.run_task();
}
display_start_message();
switch(ret_code) {
case TASK_DONE:
message_hold("Frame task completed successfully");
break;
case TASK_QUIT:
message_hold("Frame task quit by user");
break;
default:
message_hold("Frame task errored out");
break;
}
break;
}
delete vision_module;
message_hold("Selected frame vision module\n");
vision_module = new MDA_VISION_MODULE_FRAME();
use_fwd_img = true;
break;
case '5':
if (mode != VISION) {
endwin();
MDA_TASK_RETURN_CODE ret_code;
// Scope task so that it is destructed before display_start_message
{
MDA_TASK_MARKER marker_task(attitude_input, image_input, actuator_output);
ret_code = marker_task.run_task();
}
display_start_message();
switch(ret_code) {
case TASK_DONE:
message_hold("Marker task completed successfully");
break;
case TASK_QUIT:
message_hold("Marker task quit by user");
break;
default:
message_hold("Marker task errored out");
break;
}
break;
}
delete vision_module;
message_hold("Selected marker dropper vision module\n");
vision_module = new MDA_VISION_MODULE_MARKER();
use_fwd_img = false;
break;
case '8':
if (mode != VISION) {
endwin();
MDA_TASK_RETURN_CODE ret_code;
// Scope task so that it is destructed before display_start_message
{
MDA_TASK_PATH_SKIP path_skip(attitude_input, image_input, actuator_output);
ret_code = path_skip.run_task();
}
display_start_message();
switch(ret_code) {
case TASK_DONE:
message_hold("Path skip task completed successfully");
break;
case TASK_QUIT:
message_hold("Path skip task quit by user");
break;
default:
message_hold("Path skip task errored out");
break;
}
}
break;
case '9':
if (mode != VISION) {
endwin();
MDA_TASK_RETURN_CODE ret_code;
// Scope task so that it is destructed before display_start_message
{
MDA_TASK_SURFACE surface_task(attitude_input, image_input, actuator_output);
ret_code = surface_task.run_task();
}
display_start_message();
switch(ret_code) {
case TASK_DONE:
message_hold("Surface task completed successfully");
break;
case TASK_QUIT:
message_hold("Surface task quit by user");
break;
default:
message_hold("Surface task errored out");
break;
}
}
break;
case 'x':
if (mode == NORMAL) {
break;
}
delete vision_module;
vision_module = NULL;
mode = NORMAL;
display_start_message();
break;
case 'v':
if (mode == VISION) {
delete vision_module;
vision_module = NULL;
}
endwin();
mode = VISION;
message(
"Entering Vision Mode:\n"
" 0 - test vision\n"
" 1 - gate vision\n"
" 2 - path vision\n"
" 3 - buoy vision\n"
" 4 - frame vision\n"
" 5 - marker dropper vision\n"
"\n"
" v - cancel current vision selection\n"
" x - exit vision mode\n"
" q - exit simulator\n"
);
break;
case '\0': // timeout
#ifndef DEBUG_FRAME_BY_FRAME
process_image();
#else
char ch = cvWaitKey(3);
if (ch != -1) {
CharacterStreamSingleton::get_instance().write_char(ch);
}
#endif
break;
}
}
// close ncurses
endwin();
// Surface
MDA_TASK_SURFACE surface(attitude_input, image_input, actuator_output);
surface.run_task();
actuator_output->special_cmd(SUB_POWER_OFF);
}
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();
}
