void strat_autopos(int16_t x, int16_t y, int16_t a, int16_t epaisseurRobot) { robot.is_aligning = 1; // Pour se recaler, on met le robot en regulation angulaire, on reduit la vitesse et l'acceleration // On diminue la sensibilite on augmente la constante de temps de detection du bloquage bd_set_thresholds(&robot.distance_bd, 5000, 2); trajectory_set_speed(&robot.traj, 100, 100); robot.mode = BOARD_MODE_DISTANCE_ONLY; // On recule jusqu'a� qu'on ait touche un mur trajectory_d_rel(&robot.traj, (double) -2000); wait_traj_end(END_BLOCKING); trajectory_hardstop(&robot.traj); bd_reset(&robot.distance_bd); bd_reset(&robot.angle_bd); robot.mode = BOARD_MODE_ANGLE_DISTANCE; position_set(&robot.pos, epaisseurRobot, 0, COLOR_A(0.)); /* On se mets a la bonne position en x. */ trajectory_d_rel(&robot.traj, 80); wait_traj_end(END_TRAJ); /* On se tourne face a la paroi en Y. */ trajectory_only_a_abs(&robot.traj, COLOR_A(90)); wait_traj_end(END_TRAJ); /* On recule jusqu'a avoir touche le bord. */ trajectory_d_rel(&robot.traj, (double) -2000); wait_traj_end(END_BLOCKING); bd_reset(&robot.distance_bd); bd_reset(&robot.angle_bd); /* On reregle la position. */ robot.pos.pos_d.y = COLOR_Y(epaisseurRobot); robot.pos.pos_s16.y = COLOR_Y(epaisseurRobot); trajectory_d_rel(&robot.traj, 80); wait_traj_end(END_TRAJ); trajectory_goto_forward_xy_abs(&robot.traj, x, COLOR_Y(y)); wait_traj_end(END_TRAJ); /* Pour finir on s'occuppe de l'angle. */ trajectory_a_abs(&robot.traj, COLOR_A((double)a)); wait_traj_end(END_TRAJ); /* On remet le robot dans son etat initial. */ robot.mode = BOARD_MODE_ANGLE_DISTANCE; robot.is_aligning = 0; /* On se met en place a la position demandee. */ trajectory_set_speed(&robot.traj, speed_mm2imp(&robot.traj, 300), speed_rd2imp(&robot.traj, 2.5)); }
void strat_set_speed(enum speed_e speed) { int speed_d, speed_a; int acc_d, acc_a; switch (speed) { case SLOW: speed_d = 400; acc_d = 500; acc_a = 10; speed_a = 10; break; case FUCKING_SLOW: speed_d = 400; acc_d = 50; acc_a = 1; speed_a = 10; break; case FAST: default: speed_d = 800; acc_d = 1000; acc_a = 20; speed_a = 20; break; } trajectory_set_acc(&robot.traj, acc_mm2imp(&robot.traj, acc_d), acc_rd2imp(&robot.traj, acc_a)); trajectory_set_speed(&robot.traj, speed_mm2imp(&robot.traj, speed_d), speed_rd2imp(&robot.traj, speed_a)); }
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); }