int
invert_run_xml (xmlNode * node, Air * a, Trajectory * t, Jet * j, char *result)
{
gsl_rng *rng;
rng = gsl_rng_alloc (gsl_rng_taus);
gsl_rng_set (rng, RANDOM_SEED);
node = node->children;
if (!node)
{
invert_error (gettext ("No air"));
goto exit_on_error;
}
if (!air_open_xml (a, node))
goto exit_on_error;
node = node->next;
if (!jet_open_xml (j, node))
goto exit_on_error;
for (node = node->next; node; node = node->next)
{
if (!trajectory_open_xml (t, a, node, result))
goto exit_on_error;
trajectory_init (t, a, rng);
air_wind_uncertainty (a, rng);
//drop_print_parabolic (t->drop);
trajectory_invert_with_jet (t, a, j);
//drop_print_ballistic (t->drop);
}
gsl_rng_free (rng);
return 1;
exit_on_error:
return 0;
}
/**
* \fn int trajectory_run_xml (Trajectory * t, Air * a, xmlNode * node, \
* char *result)
* \brief function to run the simulation open all data in a XML file.
* \param t.
* \brief Trajectory struct.
* \param a
* \brief Air struct.
* \param node
* \brief XML node.
* \param result
* \brief result file name.
* \return 1 on success, 0 on error.
*/
int
trajectory_run_xml (Trajectory * t, Air * a, xmlNode * node, char *result)
{
gsl_rng *rng;
#if DEBUG_TRAJECTORY
fprintf (stderr, "trajectory_run_xml: start\n");
#endif
rng = gsl_rng_alloc (gsl_rng_taus);
gsl_rng_set (rng, RANDOM_SEED);
node = node->children;
if (!air_open_xml (a, node))
goto exit_on_error;
for (node = node->next; node; node = node->next)
{
if (!trajectory_open_xml (t, a, node, result))
goto exit_on_error;
trajectory_init (t, a, rng);
air_wind_uncertainty (a, rng);
trajectory_calculate (t, a, NULL, 0, t->file);
}
gsl_rng_free (rng);
#if DEBUG_TRAJECTORY
fprintf (stderr, "trajectory_run_xml: end\n");
#endif
return 1;
exit_on_error:
show_error ();
gsl_rng_free (rng);
#if DEBUG_TRAJECTORY
fprintf (stderr, "trajectory_run_xml: end\n");
#endif
return 0;
}
void motor_driver_update_trajectory(motor_driver_t *d, trajectory_chunk_t *traj)
{
chBSemWait(&d->lock);
if (d->control_mode != MOTOR_CONTROL_MODE_TRAJECTORY) {
d->setpt.trajectory = chPoolAlloc(d->traj_buffer_pool);
float *traj_mem = chPoolAlloc(d->traj_buffer_points_pool);
if (d->setpt.trajectory == NULL || traj_mem == NULL) {
chSysHalt("motor driver out of memory (trajectory buffer allocation)");
}
trajectory_init(d->setpt.trajectory, traj_mem, d->traj_buffer_nb_points, 4, traj->sampling_time_us);
d->control_mode = MOTOR_CONTROL_MODE_TRAJECTORY;
}
int ret = trajectory_apply_chunk(d->setpt.trajectory, traj);
switch (ret) {
case TRAJECTORY_ERROR_TIMESTEP_MISMATCH:
chSysHalt("TRAJECTORY_ERROR_TIMESTEP_MISMATCH");
break;
case TRAJECTORY_ERROR_CHUNK_TOO_OLD:
chSysHalt("TRAJECTORY_ERROR_CHUNK_TOO_OLD");
break;
case TRAJECTORY_ERROR_DIMENSION_MISMATCH:
chSysHalt("TRAJECTORY_ERROR_DIMENSION_MISMATCH");
break;
case TRAJECTORY_ERROR_CHUNK_OUT_OF_ORER:
log_message("TRAJECTORY_ERROR_CHUNK_OUT_OF_ORER");
// chSysHalt("TRAJECTORY_ERROR_CHUNK_OUT_OF_ORER");
break;
}
d->update_period = MOTOR_CONTROL_UPDATE_PERIOD_TRAJECTORY;
chBSemSignal(&d->lock);
}
TEST(TrajectoryInitTestGroup, CanInitTraj)
{
trajectory_t traj;
trajectory_init(&traj, (float *)buffer, 3, 2, 1000);
POINTERS_EQUAL((float *)buffer, traj.buffer);
CHECK_EQUAL(3, traj.length);
CHECK_EQUAL(2, traj.dimension);
CHECK_EQUAL(1000, (int)traj.sampling_time_us);
CHECK_EQUAL(0, (int)traj.read_index);
}
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);
}
}
}
//////////////////////////////////////
// Main
/////////////////////////////////////
int main(void)
{
//int a = 0;
/////////////////////////////////////
///TODO//////////////////////////////
/////////////////////////////////////
//
//participer a la coupe
/////////////////////////////////////
/////////////////////////////////////
//Initialisations////////////////////
/////////////////////////////////////
uart_init();
time_init(128);
printf("fdevopen \n");
fdevopen(uart0_send,uart0_recv, 1);
// Affichage uart: tip -s 9600 -l /dev/ttyS0
// prog usb :
// avarice -j /dev/ttyUSB0 --erase --program -f ./main.hex
adc_init();
DDRE |= 0x00;
PORTE |= 0x10;//active le pull up
//initialisation du fpga
sbi(XMCRA,SRW11);
sbi(XMCRA,SRW00);
sbi(MCUCR,SRE);
wait_ms(1000);
//on reset le fpga
U_RESET = 0x42;
sbi(PORTB,0);
wait_ms(100);
//on relache le reset
cbi(PORTB,0);
sbi(PORTE,3);
wait_ms(100);
//printf("stack begin :%d \n", &a);
//init des interruptions
printf("init i2c \n");
scheduler_init();
position_init(&pos);
asserv_init(&asserv,&pos);
trajectory_init(&traj,&pos,&asserv);
i2cm_init();
//autorise les interruptions
sei();
//infinite loop!!!
//get_enc_value();
//init communication
#ifdef ENABLE_MECA
printf("init com \n");
initCom();
printf("end init com \n");
#endif
//init A*
initObstacle();
//init méca
#ifdef ENABLE_MECA
mecaCom(TIROIR_FERMER);
mecaCom(PEIGNE_FERMER);
#endif
team = getTeam();
while(MAXIME);//boucle anti_maxime
// while(1)
// {
// printf("gp2r = %d ,gp2l = %d ,gp2m = %d \n",adc_get_value(ADC_REF_AVCC | MUX_ADC0),adc_get_value(ADC_REF_AVCC | MUX_ADC1),adc_get_value(ADC_REF_AVCC | MUX_ADC2));
// }
//avoidance_init();
//antipatinage_init();
// homologation();
// while(1);
// test_evitement();
// while(1);
if(team == RED)
{
disableSpinning();
findPosition(team);
enableSpinning();
trajectory_goto_d(&traj, END, 20);
trajectory_goto_a(&traj, END, 55);
while(!trajectory_is_ended(&traj));
//
// trajectory_goto_a(&traj, END, 90);
//
// trajectory_goto_a(&traj, END, 0);
// while(!trajectory_is_ended(&traj));
asserv_set_vitesse_normal(&asserv);
initTaskManager(&tkm);
//addTask(&tkm, &returnFire, LOW_PRIORITY, R1);
addTask(&tkm, &throwSpears, LOW_PRIORITY, RED);
addTask(&tkm, &putPaint, HIGH_PRIORITY, team);
// addTask(&tkm, &returnFire, LOW_PRIORITY, R3);
// addTask(&tkm, &returnFire, HIGH_PRIORITY, R2);
addTask(&tkm, &takeFruitRed, HIGH_PRIORITY, team);
// addTask(&tkm, &takeFruitYellow, HIGH_PRIORITY, team);
// addTask(&tkm, &findPosition, HIGH_PRIORITY, REDPAINT);
// addTask(&tkm, &returnFire, HIGH_PRIORITY, Y1);
//addTask(&tkm, &throwSpears, HIGH_PRIORITY, YELLOW);
addTask(&tkm, &putFruit, HIGH_PRIORITY, team);
// addTask(&tkm, &returnFire, HIGH_PRIORITY, Y3);
// addTask(&tkm, &takeFruitYellow, HIGH_PRIORITY, team);
// addTask(&tkm, &putFruit, HIGH_PRIORITY, team);
// addTask(&tkm, &returnFire, HIGH_PRIORITY, Y2);
while (TIRETTE);
printf("begin match \n");
avoidance_init();
while(doNextTask(&tkm));
printf("finish \n");
}
else // Team jaune
{
disableSpinning();
findPosition(team);
enableSpinning();
trajectory_goto_d(&traj, END, 20);
trajectory_goto_a(&traj, END, -55);
while(!trajectory_is_ended(&traj));
// addTask(&tkm, &returnFire, LOW_PRIORITY, R1);
addTask(&tkm, &returnFire, LOW_PRIORITY, Y2);
addTask(&tkm, &throwSpears, LOW_PRIORITY, YELLOW);
addTask(&tkm, &putPaint, HIGH_PRIORITY, team);
// addTask(&tkm, &returnFire, HIGH_PRIORITY, Y3);
// addTask(&tkm, &takeFruitRed, HIGH_PRIORITY, team);
addTask(&tkm, &takeFruitYellow, HIGH_PRIORITY, team);
// addTask(&tkm, &returnFire, LOW_PRIORITY, R1);
addTask(&tkm, &putFruit, HIGH_PRIORITY, team);
// addTask(&tkm, &findPosition, HIGH_PRIORITY, YELLOWPAINT);
//addTask(&tkm, &returnFire, HIGH_PRIORITY, R1);
//addTask(&tkm, &throwSpears, HIGH_PRIORITY, RED);
// addTask(&tkm, &returnFire, HIGH_PRIORITY, R3);
// addTask(&tkm, &takeFruitRed, HIGH_PRIORITY, team);
// addTask(&tkm, &putFruit, HIGH_PRIORITY, team);
// addTask(&tkm, &returnFire, HIGH_PRIORITY, R2);
while (TIRETTE);
printf("begin match \n");
avoidance_init();
while(doNextTask(&tkm));
printf("finish \n");
// // attend que la tirette soit retirée
// printf("begin match \n");
// printf("start pos: x %lf pos y %lf \n",position_get_x_cm(&pos),position_get_y_cm(&pos));
}
printf("time: %ld \n",time_get_s());
while(1);//attention boucle infinie
}
//////////////////////////////////////
// Main
/////////////////////////////////////
int main(void)
{
/////////////////////////////////////
//Initialisations////////////////////
/////////////////////////////////////
uart_init();
time_init(128);
printf("fdevopen \n");
fdevopen(uart0_send,uart0_recv, 1);
printf("test uart \n");
adc_init();
DDRE |= 0x00;
PORTE |= 0x10; //active le pull up
//initialisation du fpga
sbi(XMCRA,SRW11);
sbi(XMCRA,SRW00);
sbi(MCUCR,SRE);
wait_ms(1000);
//on reset le fpga
//U_RESET = 0x42;
RESET_FPGA = 1 ;
DDRB |= (1<<0);
sbi(PORTB,0);
wait_ms(100);
//on relache le reset
cbi(PORTB,0);
sbi(PORTE,3);
wait_ms(100);
RESET = (1<<1) | (1<<4) |(1<<7);
wait_ms(10);
RESET = 0 ;
RELATION = 15750;
MOT_R=0;
MOT_L=0;
//init des interruptions
printf("init i2c \n");
scheduler_init();
position_init(&pos);
asserv_init(&asserv,&pos);
trajectory_init(&traj,&pos,&asserv);
//autorise les interruptions
sei();
// while(1)
// {
// // printf("CLICKERS = %d \t %d \t %d \t \n",CLICKER_LEFT,CLICKER_MIDDLE,CLICKER_RIGHT);
// printf("GP2 = %hu \t%hu \t%hu \t%hu\n", read_adc0(),read_adc1(),read_adc2(),read_adc3());
// }
asserv_set_vitesse_fast(&asserv);
find_position_from(GREENSTART);
// SERVO1 = 1000;
// SERVO2 = 1000;
// SERVO3 = 1000;
// SERVO4 = 1000;
// SERVO5 = 1000;
// SERVO6 = 1000;
// SERVO7 = 1000;
// SERVO8 = 1000;
// SERVO9 = 1000;
// SERVO10 = 1000;
// SERVO11 = 1000;
// SERVO12 = 1000;
// SERVO13 = 1000;
// SERVO14 = 1000;
// SERVO15 = 1000;
// SERVO16 = 1000;
// while(1){
// // printf("PORTE=%d, PINE=%d \n", PORTE, PINE);
// //printf("%hu\t%hu\t%hu\t%hu\n", read_adc0(),read_adc1(),read_adc2(),read_adc3());
// printf("%d=PINE & mask=%d %d %d %d \n", PINE, PINE &(1<<PE4),PINE &(1<<PE5), PINE &(1<<PE6), PINE &(1<<PE7));
// // printf("%d=PORTE & mask=%d %d %d %d \n \n \n", PORTE, PORTE &(1<<PE1),PORTE &(1<<PE2), PORTE &(1<<PE3), PORTE &(1<<PE4) );
// // printf("PINE >> pin=%d %d %d %d \n\n\n", PORTE >>PE1, PORTE >>PE2, PORTE >>PE3, PORTE >>PE4 );
// //printf("PINE >> PEx=%d %d %d %d \n", PINE & 0x10, PINE & 0x20, PINE & 0x30, PINE & 0x40);
// wait_ms(1000);
// }
while(1);
}
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);
}
