int Trick::DataRecordGroup::call_function( Trick::JobData * curr_job ) {
int ret = 0 ;
switch (curr_job->id) {
case 1:
ret = init() ;
break ;
case 2:
ret = write_data(false) ;
break ;
case 3:
ret = checkpoint() ;
break ;
case 4:
clear_checkpoint_vars() ;
break ;
case 5:
ret = restart() ;
break ;
case 6:
ret = shutdown() ;
break ;
default:
ret = data_record(exec_get_sim_time()) ;
break ;
}
return(ret) ;
}
void neural_task(void *p)
{
while(1){
detachInterrupt(EXTI_Line0); /*close external interrupt 0*/
detachInterrupt(EXTI_Line1); /*close external interrupt 1*/
detachInterrupt(EXTI_Line2); /*close external interrupt 2*/
detachInterrupt(EXTI_Line3); /*close external interrupt 3*/
if(car_state == CAR_STATE_MOVE_FORWARD){
getMotorData();
rin = move_speed;
neural_update(&n_r, rin, speed_right_counter_1, distance_right_counter);
neural_update(&n_l, rin, speed_left_counter_1, distance_left_counter);
data_record();
float input_l = n_l.u;
float input_r = n_r.u;
proc_cmd("forward", base_pwm_l+(int)input_l, base_pwm_r+(int)input_r);
}
else if (car_state == CAR_STATE_MOVE_BACK)
{
getMotorData();
float err = 0;
rin = 10;
neural_update(&n_r_back, rin, speed_right_counter_1, distance_right_counter);
neural_update(&n_l_back, rin, speed_left_counter_1, distance_left_counter);
float input_l = n_l_back.u_1;
float input_r = n_r_back.u_1;
proc_cmd("forward", base_pwm_l - input_l, base_pwm_r - input_r);
}
else if (car_state == CAR_STATE_MOVE_LEFT){
rin = 5;
getMotorData();
if (distance_right_counter > MOVE_LEFT_RIGHT_PERIOD)
{
rin = 0;
pid_update(&n_r, rin, speed_right_counter_1, distance_right_counter);
}else{
pid_update(&n_r, rin, speed_right_counter_1, distance_right_counter);
}
if (distance_left_counter > MOVE_LEFT_RIGHT_PERIOD)
{
rin = 0;
pid_update(&n_l_back, rin, speed_left_counter_1, distance_left_counter);
}else{
pid_update(&n_l_back, rin, speed_left_counter_1, distance_left_counter);
}
float input_l = n_l_back.u;
float input_r = n_r.u;
proc_cmd("left", base_pwm_l-(int)input_l, base_pwm_r+(int)input_r);
}
else if (car_state == CAR_STATE_MOVE_RIGHT){
rin = 5;
getMotorData();
if (distance_right_counter > MOVE_LEFT_RIGHT_PERIOD)
{
rin = 0;
pid_update(&n_r_back, rin, speed_right_counter_1, distance_right_counter);
}else{
pid_update(&n_r_back, rin, speed_right_counter_1, distance_right_counter);
}
if (distance_left_counter > MOVE_LEFT_RIGHT_PERIOD)
{
rin = 0;
pid_update(&n_l, rin, speed_left_counter_1, distance_left_counter);
}else{
pid_update(&n_l, rin, speed_left_counter_1, distance_left_counter);
}
float input_l = n_l.u;
float input_r = n_r_back.u;
proc_cmd("right", base_pwm_l+(int)input_l, base_pwm_r-(int)input_r);
}
else if(car_state == CAR_STATE_STOPPING){
getMotorData();
if (last_state == CAR_STATE_MOVE_FORWARD)
{
if ((speed_left_counter_1 > 2) && (speed_right_counter_1 > 2))
{
rin = 0;
pid_update(&n_l, rin, speed_left_counter_1, distance_left_counter);
pid_update(&n_r, rin, speed_right_counter_1, distance_right_counter);
float input_l = n_l.u;
float input_r = n_r.u;
proc_cmd("forward", base_pwm_l + input_l, base_pwm_r + input_r);
}
else{
record_controller_base(&n_r);
record_controller_base(&n_l);
controller_reset(&n_r);
controller_reset(&n_l);
car_state = CAR_STATE_IDLE;
proc_cmd("forward", base_pwm_l, base_pwm_r);
data_sending = 1;
}
}
else if (last_state == CAR_STATE_MOVE_BACK)
{
if ((speed_left_counter_1 > 2) && (speed_right_counter_1 > 2))
{
rin = 0;
pid_update(&n_l_back, rin, speed_left_counter_1, distance_left_counter);
pid_update(&n_r_back, rin, speed_right_counter_1, distance_right_counter);
float input_l = n_l_back.u;
float input_r = n_r_back.u;
proc_cmd("forward", base_pwm_l - input_l, base_pwm_r - input_r);
}
else{
record_controller_base(&n_r_back);
record_controller_base(&n_l_back);
controller_reset(&n_r_back);
controller_reset(&n_l_back);
car_state = CAR_STATE_IDLE;
proc_cmd("forward", base_pwm_l, base_pwm_r);
data_sending = 1;
}
}
else if (last_state == CAR_STATE_MOVE_LEFT)
{
if ((speed_left_counter_1 > 2) && (speed_right_counter_1 > 2))
{
rin = 0;
pid_update(&n_l_back, rin, speed_left_counter_1, distance_left_counter);
pid_update(&n_r, rin, speed_right_counter_1, distance_right_counter);
float input_l = n_l_back.u;
float input_r = n_r.u;
proc_cmd("forward", base_pwm_l - input_l, base_pwm_r + input_r);
}
else{
//record_controller_base(&n_r);
//record_controller_base(&n_l_back);
controller_reset(&n_r);
controller_reset(&n_l_back);
car_state = CAR_STATE_IDLE;
proc_cmd("forward", base_pwm_l, base_pwm_r);
data_sending = 1;
}
}
else if (last_state == CAR_STATE_MOVE_RIGHT)
{
if ((speed_left_counter_1 > 2) && (speed_right_counter_1 > 2))
{
rin = 0;
pid_update(&n_l, rin, speed_left_counter_1, distance_left_counter);
pid_update(&n_r_back, rin, speed_right_counter_1, distance_right_counter);
float input_l = n_l.u;
float input_r = n_r_back.u;
proc_cmd("forward", base_pwm_l + input_l, base_pwm_r - input_r);
}
else{
//record_controller_base(&n_r_back);
//record_controller_base(&n_l);
controller_reset(&n_r_back);
controller_reset(&n_l);
car_state = CAR_STATE_IDLE;
proc_cmd("forward", base_pwm_l, base_pwm_r);
data_sending = 1;
}
}
}
else if(car_state == CAR_STATE_IDLE){
proc_cmd("forward", base_pwm_l, base_pwm_r);
speeed_initialize();
distance_initialize();
}
else{
}
attachInterrupt(EXTI_Line0);
attachInterrupt(EXTI_Line1);
attachInterrupt(EXTI_Line2);
attachInterrupt(EXTI_Line3);
vTaskDelay(NEURAL_PERIOD);
}
}
