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); }