void EXTI1_IRQHandler(){
if(EXTI_GetITStatus(EXTI_Line1) != RESET)
{
speed_right_counter++ ;
distance_right_counter ++;
if(car_state==CAR_STATE_MOVE_LEFT){
if (distance_right_counter >= MOVE_LEFT_RIGHT_PERIOD && distance_left_counter >= MOVE_LEFT_RIGHT_PERIOD) //2000 == 4 cycle
{
controller_reset(&n_r);
controller_reset(&n_l_back);
car_state = CAR_STATE_IDLE;
last_state = CAR_STATE_MOVE_LEFT;
}
}
else if(car_state==CAR_STATE_MOVE_RIGHT){
if(distance_right_counter >= MOVE_LEFT_RIGHT_PERIOD && distance_left_counter >= MOVE_LEFT_RIGHT_PERIOD)
{
controller_reset(&n_r_back);
controller_reset(&n_l);
car_state = CAR_STATE_IDLE;
last_state = CAR_STATE_MOVE_RIGHT;
}
}
EXTI_ClearITPendingBit(EXTI_Line1);
}
}
void controller_handler(const double xk, double *yk)
{
double ek;
double p_term, i_term, d_term;
ek = p_param.t_target - xk; /* error */
if (ek >= 3.0) {
syslog(LOG_DEBUG, "PID: %3.1f Below set point %3.1f, stop\n",
xk, p_param.t_target);
controller_reset();
*yk = 0.0;
return;
}
/* compute intermediate PID terms */
p_term = -p_param.kp * (xk - xk_1);
i_term = p_param.kp * p_param.ki * p_param.ts * ek;
d_term = -p_param.kp * p_param.kd * (xk - 2 * xk_1 + xk_2) / p_param.ts;
/* compute output */
*yk += p_term + i_term + d_term;
/* update sample data */
xk_1 = xk;
xk_2 = xk_1;
/* clamp output adjustment range */
if (*yk < -LIMIT_HIGH)
*yk = -LIMIT_HIGH;
else if (*yk > -LIMIT_LOW)
*yk = -LIMIT_LOW;
p_param.y_k = *yk;
set_ctrl_state(lround(fabs(p_param.y_k)));
}
static void setup_pathfin(TinBot* tinbot) {
hal_print("Tin Bot Setup: Path Finder (20,30), somewhat");
controller_reset(&tinbot->controller, &tinbot->sens);
/* First, persuade the Moderator that we're already rescuing the victim
* all along, and that this is fine. */
tinbot->controller.moderator.locals.state = 4;
tinbot->rx_buffer.moderate.owning_xy_p = 1;
/* Call ProxFilter, for fun.
* Also, disable TCE's interrupt. */
tinbot->controller.moderator.found_victim_xy = 1;
/* approx_step can run freely, as approx_reset was called by
* controller_reset. */
/* Next, Blind Cop has lots of internal state. */
tinbot->controller.blind.locals.state_big = 1;
tinbot->controller.blind.locals.state_leaf = 5;
/* Finally, tell the controller where to go. */
tinbot->controller.first_iter = 0;
tinbot->controller.origin.x = 20.0;
tinbot->controller.origin.y = 30.0;
/* The rest should be propagated automatically. */
}
static bool state_machine_filter(enum state_function *state)
{
switch (*state)
{
case STATE_FCT_IDLE:
if (new_state_fct)
{
gui_set_selection(GUI_FILTER_ID);
gui_text_print(GUI_COMMENT_ID, TEXT_FILTER);
}
else
{
if (controller_wheel_right(2))
switch_state(STATE_OUTPUT);
else if (controller_wheel_left(2))
switch_state(STATE_INPUT);
return false;
}
break;
// FFT
case STATE_FCT_FUNCTION1:
if (new_state_fct)
filter_set_active(0);
break;
case STATE_FCT_FUNCTION2:
if (new_state_fct)
filter_set_active(1);
break;
case STATE_FCT_FUNCTION3:
if (new_state_fct)
filter_set_active(2);
break;
case STATE_FCT_ZOOM:
break;
}
if (new_state_fct)
gui_text_print(GUI_FILTER_ID, filter_active_get_description());
if (controller_wheel_right(0) || controller_wheel_left(0))
{
*state = STATE_FCT_IDLE;
controller_reset();
}
return true;
}
int main( void )
{
controller_reset( "localhost", 2002 );
controller_set( 0, 0, -5, 0 );
while( 1 )
{
double north;
double east;
double down;
double roll;
double pitch;
double yaw;
int rc;
rc = controller_step(
&north,
&east,
&down,
&roll,
&pitch,
&yaw
);
if( rc == 0 )
continue;
if( rc < 0 )
break;
fprintf( stderr,
"(%2.3f,%2.3f,%2.3f,%2.3f)\r",
north,
east,
down,
yaw
);
}
fprintf( stderr, "\n" );
return 0;
}
static void state_machine_task(void)
{
bool is_reset;
// Set function state
if (controller_key_fct3_pressed())
{
state_fct = STATE_FCT_ZOOM;
new_state_fct = true;
}
else if (controller_key_fct1())
{
state_fct = STATE_FCT_FUNCTION1;
new_state_fct = true;
}
else if (controller_key_fct2())
{
state_fct = STATE_FCT_FUNCTION2;
new_state_fct = true;
}
else if (controller_key_fct3())
{
state_fct = STATE_FCT_FUNCTION3;
new_state_fct = true;
}
// Clear Zoom state if on and a key is pressed
if (zoom_view && !controller_key_fct3_pressed())
{
zoom_view = false;
gui_clear_view();
gui_text_print(GUI_FILTER_ID, filter_active_get_description());
new_state_fct = true;
state_fct = STATE_FCT_IDLE;
controller_key_reset();
}
// Reset or back modes
is_reset = controller_key_reset();
if ((controller_key_back() && !zoom_view) || is_reset)
{
if (is_reset)
{
// SW resetof the application
Reset_CPU();
while(1);
}
if (state_fct == STATE_FCT_IDLE)
state = STATE_IDLE;
state_fct = STATE_FCT_IDLE;
// Reset the controller (delete saved key states)
controller_reset();
new_state_fct = true;
}
switch (state)
{
case STATE_IDLE:
if (!state_machine_idle(&state_fct))
return;
break;
case STATE_SOURCE1:
if (!state_machine_source(1, &state_fct))
return;
break;
case STATE_SOURCE2:
if (!state_machine_source(2, &state_fct))
return;
break;
case STATE_INPUT:
if (!state_machine_signal(STATE_INPUT, &state_fct))
return;
break;
case STATE_FILTER:
if (!state_machine_filter(&state_fct))
return;
break;
case STATE_OUTPUT:
if (!state_machine_signal(STATE_OUTPUT, &state_fct))
return;
break;
}
new_state_fct = false;
}
static bool state_machine_signal(enum state_master id, enum state_function *state)
{
switch (*state)
{
case STATE_FCT_IDLE:
if (id == STATE_INPUT)
{
if (new_state_fct)
{
gui_set_selection(GUI_INPUT_ID);
gui_text_print(GUI_COMMENT_ID, TEXT_INPUT);
}
else
{
if (controller_wheel_right(2))
switch_state(STATE_FILTER);
else if (controller_wheel_left(2))
switch_state(STATE_SOURCE2);
return false;
}
}
else if (id == STATE_OUTPUT)
{
if (new_state_fct)
{
gui_set_selection(GUI_OUTPUT_ID);
gui_text_print(GUI_COMMENT_ID, TEXT_OUTPUT);
}
else
{
if (controller_wheel_left(2))
switch_state(STATE_FILTER);
return false;
}
}
break;
// FFT
case STATE_FCT_FUNCTION1:
if (new_state_fct)
{
if (id == STATE_INPUT)
{
input_fft_view = !input_fft_view;
if (input_fft_view)
gui_text_print(GUI_COMMENT_ID, "Input - " TEXT_FUNC1 "\nFrequency domain\n\n\n\n(Use " TEXT_FUNC1 ")");
else
gui_text_print(GUI_COMMENT_ID, "Input - " TEXT_FUNC1 "\nTemporal domain\n\n\n\n(Use " TEXT_FUNC1 ")");
}
else if (id == STATE_OUTPUT)
{
output_fft_view = !output_fft_view;
if (output_fft_view)
gui_text_print(GUI_COMMENT_ID, "Output - " TEXT_FUNC1 "\nFrequency domain\n\n\n\n(Use " TEXT_FUNC1 ")");
else
gui_text_print(GUI_COMMENT_ID, "Output - " TEXT_FUNC1 "\nTemporal domain\n\n\n\n(Use " TEXT_FUNC1 ")");
}
}
break;
case STATE_FCT_FUNCTION2:
case STATE_FCT_FUNCTION3:
if (new_state_fct)
gui_text_print(GUI_COMMENT_ID, TEXT_FUNC_NOT_IMPLEMENTED);
break;
// Zoom
case STATE_FCT_ZOOM:
if (new_state_fct)
{
zoom_view = true;
if (id == STATE_INPUT)
zoom_view_id = GUI_INPUT_ID;
else if (id == STATE_OUTPUT)
zoom_view_id = GUI_OUTPUT_ID;
// controller_reset();
}
break;
}
if (controller_wheel_right(0) || controller_wheel_left(0))
{
*state = STATE_FCT_IDLE;
controller_reset();
}
return true;
}
static bool state_machine_source(int source_id, enum state_function *state)
{
static dsp16_t volume;
static unsigned int frequency;
struct signal_source *source = NULL;
if (source_id == 1)
source = &signal1_generator;
else if (source_id == 2)
source = &signal2_generator;
switch (*state)
{
case STATE_FCT_IDLE:
if (source_id == 1)
{
if (new_state_fct)
{
gui_set_selection(GUI_SOURCE1_ID);
gui_text_print(GUI_COMMENT_ID, TEXT_SOURCE1);
}
else
{
if (controller_wheel_right(2))
switch_state(STATE_SOURCE2);
return false;
}
}
else if (source_id == 2)
{
if (new_state_fct)
{
gui_set_selection(GUI_SOURCE2_ID);
gui_text_print(GUI_COMMENT_ID, TEXT_SOURCE2);
}
else
{
if (controller_wheel_left(2))
switch_state(STATE_SOURCE1);
else if (controller_wheel_right(2))
switch_state(STATE_INPUT);
return false;
}
}
break;
// Amplitude
case STATE_FCT_FUNCTION1:
volume = signal_source_get_volume(source);
if (controller_wheel_right(1) && volume < DSP16_Q(1.))
{
if (volume < DSP16_Q(1.) - DSP16_Q(1./16))
volume += DSP16_Q(1./16);
else
volume = DSP16_Q(1.);
new_state_fct = true;
}
else if (controller_wheel_left(1))
{
if (volume > DSP16_Q(1./16))
volume -= DSP16_Q(1./16);
else
volume = 0;
new_state_fct = true;
}
if (new_state_fct)
{
signal_source_set_volume(source, volume);
gui_text_printf(GUI_COMMENT_ID, "Source%i - " TEXT_FUNC1 "\nAmplitude %f\n\n\n\n" TEXT_WHEEL, source_id, volume);
}
break;
// Frequency
case STATE_FCT_FUNCTION2:
frequency = signal_source_get_freq(source);
if (controller_wheel_right(1) && frequency < 10000)
{
frequency *= 1.1;
new_state_fct = true;
}
else if (controller_wheel_left(1) && frequency > 100)
{
frequency *= 0.9;
new_state_fct = true;
}
if (new_state_fct)
{
signal_source_set_freq(source, frequency);
gui_text_printf(GUI_COMMENT_ID, "Source%i - " TEXT_FUNC2 "\nFrequency %iHz\n\n\n\n" TEXT_WHEEL, source_id, frequency);
}
break;
case STATE_FCT_FUNCTION3:
break;
// Zoom
case STATE_FCT_ZOOM:
if (new_state_fct)
{
zoom_view = true;
if (source_id == 1)
zoom_view_id = GUI_SOURCE1_ID;
else if (source_id == 2)
zoom_view_id = GUI_SOURCE2_ID;
controller_reset();
}
break;
}
return true;
}
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);
}
}
/* Mode - Full */
static void setup_full(TinBot* tinbot) {
hal_print("Tin Bot Setup: FULL");
controller_reset(&tinbot->controller, &tinbot->sens);
}
