void main(void)
{
struct uart uart_pc;
/* Place your initialization/startup code here (e.g. MyInst_Start()) */
CYGlobalIntEnable; /* Uncomment this line to enable global interrupts. */
color_sign = RED;
timer_asserv_Start();
isr_positionning_Start();
uart_init(&uart_pc, 16, &uart_pc_Start, &uart_pc_Stop, &uart_pc_GetChar, &uart_pc_ClearRxBuffer, &uart_pc_ClearTxBuffer, &uart_pc_PutString, &uart_pc_PutStringConst);
uart_set_command(&uart_pc, 0, "value", &quad_dec_value);
uart_set_command(&uart_pc, 1, "readxy", &read_xy);
uart_set_command(&uart_pc, 2, "exec", &time_exec_counter);
uart_set_command(&uart_pc, 3, "reset", &reset);
uart_set_command(&uart_pc, 4, "asserv", &set_consigne_asserv);
uart_set_command(&uart_pc, 5, "setcoeff", &set_PID);
uart_set_command(&uart_pc, 6, "pwmr", &pwmR);
uart_set_command(&uart_pc, 7, "pwml", &pwmL);
uart_set_command(&uart_pc, 8, "demuxr", &demuxR);
uart_set_command(&uart_pc, 9, "demuxl", &demuxL);
uart_set_command(&uart_pc, 10, "stop", &stop);
uart_set_command(&uart_pc, 11, "gotoxy", &gotoxy);
uart_set_command(&uart_pc, 12, "gotoa", &gotoa);
uart_set_command(&uart_pc, 13, "gotod", &gotod);
uart_set_command(&uart_pc, 14, "resetcoeff", &resetCoef);
uart_set_command(&uart_pc, 15, "setxy", &set_multiple_xy);
uart_pc.put_string_const("Asserv\n\r>");
trajectory_init(&rsT);
pid_set_gains(&rsT.csm_angle.correct_filter_params, PIDA_P, PIDA_I, PIDA_D);
pid_set_out_shift(&rsT.csm_angle.correct_filter_params, PIDA_SH);
pid_set_gains(&rsT.csm_distance.correct_filter_params, PIDD_P, PIDD_I, PIDD_D);
pid_set_out_shift(&rsT.csm_distance.correct_filter_params, PIDD_SH);
while(1)
{
uart_check(&uart_pc);
if(rsT.t == TIME_ASSERV)
{
rsT.t = TIME_IDLE;
++rsT.time;
trajectory_update(&rsT);
}
}
}
void pid_init(pid_ctrl_t *pid)
{
pid_set_gains(pid, 1., 0., 0.);
pid->integrator = 0.;
pid->previous_error = 0.;
pid->integrator_limit = INFINITY;
pid->frequency = 1.;
}
/** Transfer configuration to PID (thread-safe) */
void pidcfg_apply(pid_config_t *cfg)
{
CRITICAL_SECTION_ALLOC();
// check if something to apply
if (!cfg->has_update) {
return;
}
CRITICAL_SECTION_ENTER();
pid_set_gains(cfg->target_pid, cfg->kp, cfg->ki, cfg->kd);
pid_set_integral_limit(cfg->target_pid, cfg->integrator_limit);
pid_set_frequency(cfg->target_pid, cfg->frequency);
cfg->has_update = false;
CRITICAL_SECTION_EXIT();
}
void robot_cs_init(robot_cs_t* rcs)
{
// zeroing structures
rcs->hrs = NULL;
rcs->hpm = NULL;
// set CS on
rcs->active = 1;
// CS not reactivated since last tick
rcs->reactivated = 0;
// setup PIDs
pid_init(&pid_x);
pid_init(&pid_y);
pid_init(&pid_angle);
pid_set_gains(&pid_x, SETTING_PID_X_GAIN_P,
SETTING_PID_X_GAIN_I,
SETTING_PID_X_GAIN_D);
pid_set_maximums(&pid_x, SETTING_PID_X_MAX_IN,
SETTING_PID_X_MAX_I,
SETTING_PID_X_MAX_OUT);
pid_set_out_shift(&pid_x, SETTING_PID_X_SHIFT);
pid_set_gains(&pid_y, SETTING_PID_Y_GAIN_P,
SETTING_PID_Y_GAIN_I,
SETTING_PID_Y_GAIN_D);
pid_set_maximums(&pid_y, SETTING_PID_Y_MAX_IN,
SETTING_PID_Y_MAX_I,
SETTING_PID_Y_MAX_OUT);
pid_set_out_shift(&pid_y, SETTING_PID_Y_SHIFT);
pid_set_gains(&pid_angle, SETTING_PID_A_GAIN_P,
SETTING_PID_A_GAIN_I,
SETTING_PID_A_GAIN_D);
pid_set_maximums(&pid_angle, SETTING_PID_A_MAX_IN,
SETTING_PID_A_MAX_I,
SETTING_PID_A_MAX_OUT);
pid_set_out_shift(&pid_angle, SETTING_PID_A_SHIFT);
// quadramp
quadramp_set_1st_order_vars(&qramp_angle,
SETTING_QRAMP_A_SPEED, SETTING_QRAMP_A_SPEED);
quadramp_set_2nd_order_vars(&qramp_angle,
SETTING_QRAMP_A_ACC, SETTING_QRAMP_A_ACC);
// setup CSMs
cs_init(&csm_x);
cs_init(&csm_y);
cs_init(&csm_angle);
cs_set_consign_filter(&csm_x, NULL, NULL);
cs_set_consign_filter(&csm_y, NULL, NULL);
cs_set_consign_filter(&csm_angle, &quadramp_do_filter, &qramp_angle);
cs_set_correct_filter(&csm_x, &pid_do_filter, &pid_x);
cs_set_correct_filter(&csm_y, &pid_do_filter, &pid_y);
cs_set_correct_filter(&csm_angle, &pid_do_filter, &pid_angle);
cs_set_feedback_filter(&csm_x, NULL, NULL);
cs_set_feedback_filter(&csm_y, NULL, NULL);
cs_set_feedback_filter(&csm_angle, NULL, NULL);
cs_set_process_out(&csm_x, &get_robot_x, rcs);
cs_set_process_out(&csm_y, &get_robot_y, rcs);
cs_set_process_out(&csm_angle, &get_robot_a, rcs);
cs_set_process_in(&csm_x, NULL, NULL);
cs_set_process_in(&csm_y, NULL, NULL);
cs_set_process_in(&csm_angle, NULL, NULL);
}
int main(void)
{
// init motors and PWMs
brushless_init();
// enable power bridges
sbi(DDRG, 1);
sbi(PORTG, 1);
// init uart
uart_init();
fdevopen((void *)uart0_send,NULL,0);
//printf_P(PSTR("\nbonjour\n"));
/** replaces the scheduler. This is directly derived from the interrupt which runs the brushless motors, for convenience */
brushless_0_register_periodic_event(asserv_rapide_manage); // 10 ms
/** speed PID stuff */
// PID
pid_init(&speed_pid);
pid_set_gains(&speed_pid, 180, 70, 40); // sur alim
pid_set_maximums(&speed_pid, 0, 80, PWM_MAX*4/5);
pid_set_out_shift(&speed_pid, 0);
// derivation (This could alternatively be skipped if we use the brushless peed info directly)
biquad_init(&speed_derivation);
biquad_set_numerator_coeffs(&speed_derivation, 1,-1,0); // this is a discrete derivation : O(n) = I(n) - I(n-1)
// no need to initialize denominator coeffs to 0, init has done it
// control system speed
cs_init(&speed);
cs_set_correct_filter(&speed, pid_do_filter, &speed_pid);
cs_set_process_in(&speed, brushless_set_torque, (void *)0 );
cs_set_process_out(&speed,brushless_get_pos , (void *)0 );
cs_set_feedback_filter(&speed, biquad_do_filter, &speed_derivation);
cs_set_consign(&speed, 0);
/** ramp generator parameters */
quadramp_derivate_init(&position_quadr_derv);
quadramp_derivate_set_gain_anticipation(&position_quadr_derv, 256 *3);// some anticipation : 3.0 (this is a fixed point value *1/256)
quadramp_derivate_set_goal_window(&position_quadr_derv, 5); // algorithm shut down window
quadramp_derivate_set_2nd_order_vars(&position_quadr_derv, 1 , 1); // max acceleration : 1
quadramp_derivate_set_1st_order_vars(&position_quadr_derv, 12, 12); // max speed is 12
quadramp_derivate_set_divisor(&position_quadr_derv, 2); // divisor, for precision
// control system position
cs_init(&position);
cs_set_correct_filter(&position, quadramp_derivate_do_filter, &position_quadr_derv);
cs_set_process_in(&position, (void *)cs_set_consign, &speed );
cs_set_process_out(&position,brushless_get_pos , (void *)0 );
cs_set_consign(&position, 0);
/** begin */
brushless_set_speed((void *)0 , BRUSHLESS_MAX_SPEED); // init speed
sei();
// some simple trajectories (enable one )
while(1)
{
wait_ms(3500);
cs_set_consign(&position, 400);
wait_ms(500);
cs_set_consign(&position, 0);
}
/*
while(1)
{
wait_ms(2500);
cs_set_consign(&position, 2000);
wait_ms(2500);
cs_set_consign(&position, 0);
}
// test of speed pid only, deactivate the position.
while(1)
{
wait_ms(300);
cs_set_consign(&speed, 10);
wait_ms(300);
cs_set_consign(&speed, -10);
} */
while(1);
return 0;
}
void set_gains_a(short p, short i, short d)
{
pid_set_gains(&robot.pid_a, p, i, d);
}
void microb_cs_init(void)
{
/* ROBOT_SYSTEM */
rs_init(&mainboard.rs);
rs_set_left_pwm(&mainboard.rs, pwm_set_and_save, LEFT_PWM);
rs_set_right_pwm(&mainboard.rs, pwm_set_and_save, RIGHT_PWM);
/* increase gain to decrease dist, increase left and it will turn more left */
rs_set_left_ext_encoder(&mainboard.rs, encoders_microb_get_value,
LEFT_ENCODER, IMP_COEF * -1.0000);
rs_set_right_ext_encoder(&mainboard.rs, encoders_microb_get_value,
RIGHT_ENCODER, IMP_COEF * 1.0000);
/* rs will use external encoders */
rs_set_flags(&mainboard.rs, RS_USE_EXT);
/* POSITION MANAGER */
position_init(&mainboard.pos);
position_set_physical_params(&mainboard.pos, VIRTUAL_TRACK_MM, DIST_IMP_MM);
position_set_related_robot_system(&mainboard.pos, &mainboard.rs);
//position_set_centrifugal_coef(&mainboard.pos, 0.000016);
position_use_ext(&mainboard.pos);
/* TRAJECTORY MANAGER */
trajectory_init(&mainboard.traj);
trajectory_set_cs(&mainboard.traj, &mainboard.distance.cs,
&mainboard.angle.cs);
trajectory_set_robot_params(&mainboard.traj, &mainboard.rs, &mainboard.pos);
trajectory_set_speed(&mainboard.traj, 1500, 1500); /* d, a */
/* distance window, angle window, angle start */
trajectory_set_windows(&mainboard.traj, 200., 5.0, 30.);
/* ---- CS angle */
/* PID */
pid_init(&mainboard.angle.pid);
pid_set_gains(&mainboard.angle.pid, 500, 10, 7000);
pid_set_maximums(&mainboard.angle.pid, 0, 20000, 4095);
pid_set_out_shift(&mainboard.angle.pid, 10);
pid_set_derivate_filter(&mainboard.angle.pid, 4);
/* QUADRAMP */
quadramp_init(&mainboard.angle.qr);
quadramp_set_1st_order_vars(&mainboard.angle.qr, 2000, 2000); /* set speed */
quadramp_set_2nd_order_vars(&mainboard.angle.qr, 13, 13); /* set accel */
/* CS */
cs_init(&mainboard.angle.cs);
cs_set_consign_filter(&mainboard.angle.cs, quadramp_do_filter, &mainboard.angle.qr);
cs_set_correct_filter(&mainboard.angle.cs, pid_do_filter, &mainboard.angle.pid);
cs_set_process_in(&mainboard.angle.cs, rs_set_angle, &mainboard.rs);
cs_set_process_out(&mainboard.angle.cs, rs_get_angle, &mainboard.rs);
cs_set_consign(&mainboard.angle.cs, 0);
/* Blocking detection */
bd_init(&mainboard.angle.bd);
bd_set_speed_threshold(&mainboard.angle.bd, 80);
bd_set_current_thresholds(&mainboard.angle.bd, 500, 8000, 1000000, 50);
/* ---- CS distance */
/* PID */
pid_init(&mainboard.distance.pid);
pid_set_gains(&mainboard.distance.pid, 500, 10, 7000);
pid_set_maximums(&mainboard.distance.pid, 0, 2000, 4095);
pid_set_out_shift(&mainboard.distance.pid, 10);
pid_set_derivate_filter(&mainboard.distance.pid, 6);
/* QUADRAMP */
quadramp_init(&mainboard.distance.qr);
quadramp_set_1st_order_vars(&mainboard.distance.qr, 2000, 2000); /* set speed */
quadramp_set_2nd_order_vars(&mainboard.distance.qr, 17, 17); /* set accel */
/* CS */
cs_init(&mainboard.distance.cs);
cs_set_consign_filter(&mainboard.distance.cs, quadramp_do_filter, &mainboard.distance.qr);
cs_set_correct_filter(&mainboard.distance.cs, pid_do_filter, &mainboard.distance.pid);
cs_set_process_in(&mainboard.distance.cs, rs_set_distance, &mainboard.rs);
cs_set_process_out(&mainboard.distance.cs, rs_get_distance, &mainboard.rs);
cs_set_consign(&mainboard.distance.cs, 0);
/* Blocking detection */
bd_init(&mainboard.distance.bd);
bd_set_speed_threshold(&mainboard.distance.bd, 60);
bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 1000000, 50);
/* ---- CS fessor */
fessor_autopos();
/* PID */
pid_init(&mainboard.fessor.pid);
pid_set_gains(&mainboard.fessor.pid, 300, 10, 150);
pid_set_maximums(&mainboard.fessor.pid, 0, 10000, 4095);
pid_set_out_shift(&mainboard.fessor.pid, 10);
pid_set_derivate_filter(&mainboard.fessor.pid, 4);
/* CS */
cs_init(&mainboard.fessor.cs);
cs_set_correct_filter(&mainboard.fessor.cs, pid_do_filter, &mainboard.fessor.pid);
cs_set_process_in(&mainboard.fessor.cs, fessor_set, NULL);
cs_set_process_out(&mainboard.fessor.cs, encoders_microb_get_value, FESSOR_ENC);
fessor_up();
/* ---- CS elevator */
elevator_autopos();
/* PID */
pid_init(&mainboard.elevator.pid);
pid_set_gains(&mainboard.elevator.pid, 300, 10, 150);
pid_set_maximums(&mainboard.elevator.pid, 0, 10000, 4095);
pid_set_out_shift(&mainboard.elevator.pid, 10);
pid_set_derivate_filter(&mainboard.elevator.pid, 4);
/* CS */
cs_init(&mainboard.elevator.cs);
cs_set_correct_filter(&mainboard.elevator.cs, pid_do_filter, &mainboard.elevator.pid);
cs_set_process_in(&mainboard.elevator.cs, elevator_set, NULL);
cs_set_process_out(&mainboard.elevator.cs, encoders_microb_get_value, ELEVATOR_ENC);
elevator_down();
/* ---- CS wheel */
/* PID */
pid_init(&mainboard.wheel.pid);
pid_set_gains(&mainboard.wheel.pid, 100, 100, 0);
pid_set_maximums(&mainboard.wheel.pid, 0, 30000, 4095);
pid_set_out_shift(&mainboard.wheel.pid, 5);
pid_set_derivate_filter(&mainboard.wheel.pid, 4);
/* CS */
cs_init(&mainboard.wheel.cs);
cs_set_correct_filter(&mainboard.wheel.cs, pid_do_filter, &mainboard.wheel.pid);
cs_set_process_in(&mainboard.wheel.cs, wheel_set, NULL);
cs_set_process_out(&mainboard.wheel.cs, wheel_get_value, NULL);
cs_set_consign(&mainboard.wheel.cs, 1000);
/* set them on !! */
mainboard.angle.on = 0;
mainboard.distance.on = 0;
mainboard.fessor.on = 1;
mainboard.elevator.on = 0;
mainboard.wheel.on = 1;
mainboard.flags |= DO_CS;
scheduler_add_periodical_event_priority(do_cs, NULL,
5000L / SCHEDULER_UNIT,
CS_PRIO);
}
void motor_cs_init()
{
// initialize pwms
pwm_init();
// setup pwms dirs
sbi(MOTOR_CS_PWM1_DDR,MOTOR_CS_PWM1_PIN);
cbi(MOTOR_CS_PWM1_PORT,MOTOR_CS_PWM1_PIN);
sbi(MOTOR_CS_PWM2_DDR,MOTOR_CS_PWM2_PIN);
cbi(MOTOR_CS_PWM2_PORT,MOTOR_CS_PWM2_PIN);
sbi(MOTOR_CS_PWM3_DDR,MOTOR_CS_PWM3_PIN);
cbi(MOTOR_CS_PWM3_PORT,MOTOR_CS_PWM3_PIN);
motor1_sign = 0;
motor2_sign = 0;
motor3_sign = 0;
pwm_set_1A(0);
pwm_set_1B(0);
pwm_set_1C(0);
// setup brake
sbi(MOTOR_CS_BREAK_DDR, MOTOR_CS_BREAK_PIN);
cbi(MOTOR_CS_BREAK_PORT, MOTOR_CS_BREAK_PIN);
// activate interrupts
sbi(TIMSK,TOIE1);
// setup PIDs
pid_init(&pid_motor1);
pid_init(&pid_motor2);
pid_init(&pid_motor3);
pid_set_gains(&pid_motor1, 500, 0, 10) ;
pid_set_maximums(&pid_motor1, 0, 0, 0);
pid_set_out_shift(&pid_motor1, 10);
pid_set_gains(&pid_motor2, 500, 0, 10) ;
pid_set_maximums(&pid_motor2, 0, 0, 0);
pid_set_out_shift(&pid_motor2, 10);
pid_set_gains(&pid_motor3, 500, 0, 10);
pid_set_maximums(&pid_motor3, 0, 0, 0);
pid_set_out_shift(&pid_motor3, 10);
// setup CSMs
cs_init(&csm_motor1);
cs_init(&csm_motor2);
cs_init(&csm_motor3);
cs_set_consign_filter(&csm_motor1, NULL, NULL);
cs_set_consign_filter(&csm_motor2, NULL, NULL);
cs_set_consign_filter(&csm_motor3, NULL, NULL);
cs_set_correct_filter(&csm_motor1, &pid_do_filter, &pid_motor1);
cs_set_correct_filter(&csm_motor2, &pid_do_filter, &pid_motor2);
cs_set_correct_filter(&csm_motor3, &pid_do_filter, &pid_motor3);
cs_set_feedback_filter(&csm_motor1, NULL, NULL);
cs_set_feedback_filter(&csm_motor2, NULL, NULL);
cs_set_feedback_filter(&csm_motor3, NULL, NULL);
cs_set_process_out( &csm_motor1, &get_encoder_motor1, NULL);
cs_set_process_out( &csm_motor2, &get_encoder_motor2, NULL);
cs_set_process_out( &csm_motor3, &get_encoder_motor3, NULL);
cs_set_process_in( &csm_motor1, &set_pwm_motor1, NULL);
cs_set_process_in( &csm_motor2, &set_pwm_motor2, NULL);
cs_set_process_in( &csm_motor3, &set_pwm_motor3, NULL);
return;
}
