/* for elevator auto mode */ static void elevator_cb(void *dummy) { static uint8_t prev = 0; uint8_t val; val = !sensor_get(0); switch (elevator_state) { case OFF: break; case WAIT_SENSOR: if (val && !prev) { delay = elevator_delay_up; elevator_state = SENSOR_OK; } break; case SENSOR_OK: if (delay-- == 0) { cs_set_consign(&mainboard.elevator.cs, elevator_pos_up); elevator_state = WAIT_DOWN; delay = elevator_delay_down; } break; case WAIT_DOWN: if (delay-- == 0) { cs_set_consign(&mainboard.elevator.cs, elevator_pos_down); elevator_state = WAIT_SENSOR; } break; default: break; } prev = val; }
void robot_cs_set_xy_consigns( robot_cs_t* rcs, int32_t x, int32_t y) { uint8_t flags; IRQ_LOCK(flags); cs_set_consign(&csm_x,x); cs_set_consign(&csm_y,y); IRQ_UNLOCK(flags); }
void motor_cs_update(void* dummy, int32_t m1, int32_t m2, int32_t m3) { uint8_t flags; // set consigns for motors CS cs_set_consign(&csm_motor1, m1); cs_set_consign(&csm_motor2, m2); cs_set_consign(&csm_motor3, m3); // update motors CS cs_manage(&csm_motor1); cs_manage(&csm_motor2); cs_manage(&csm_motor3); return; }
void robot_cs_set_a_consign( robot_cs_t* rcs, int32_t angle) { uint8_t flags; IRQ_LOCK(flags); cs_set_consign(&csm_angle,angle); IRQ_UNLOCK(flags); }
void rsh_update(struct robot_system_holonomic *rs) { if(rs->pos == NULL) return; /** @todo add real physical parameters. */ double theta_r = holonomic_position_get_a_rad_double(rs->pos); int i; for(i = 0; i < 3; i++) { /** omega = omega_r + omega_t = speed of the wheel i */ /** < the part for translation */ const double omega_t = - rs->speed / rs->pos->geometry.wheel_radius[i] * cos(theta_r - rs->direction + rs->pos->geometry.beta[i] - M_PI_2); /** < the part for rotation */ const double omega_r = - rs->rotation_speed * rs->pos->geometry.wheel_distance[i] / rs->pos->geometry.wheel_radius[i]; /** @bug @todo le facteur 10 : on ne le met pas et on a des vitesse 10 x trop haute */ cs_set_consign(rs->motors[i],((omega_t + omega_r) / M_2_PI * (double)rs->pos->geometry.encoder_resolution / (double)rs->pos->update_frequency)/10); //DEBUG(E_ROBOT_SYSTEM, "wheel %d : omega_t=%.1f omega_r=%.1f", i, omega_t, omega_r); } }
void beacon_calibre_pos(void) { sensorboard.flags &= ~DO_CS; /* init beacon pos */ pwm_ng_set(BEACON_PWM, 100); /* find rising edge of the mirror*/ wait_ms(100); while (sensor_get(BEACON_POS_SENSOR)); wait_ms(100); while (!sensor_get(BEACON_POS_SENSOR)); pwm_ng_set(BEACON_PWM, 0); beacon_reset_pos(); pid_reset(&sensorboard.beacon.pid); encoders_spi_set_value_beacon(BEACON_ENCODER, BEACON_OFFSET_CALIBRE); cs_set_consign(&sensorboard.beacon.cs, 0); sensorboard.flags |= DO_CS; }
void roller_reverse(void) { cs_set_consign(&ballboard.roller.cs, ROLLER_REVERSE); }
void roller_on(void) { cs_set_consign(&ballboard.roller.cs, ROLLER_ON); }
void roller_off(void) { cs_set_consign(&ballboard.roller.cs, ROLLER_OFF); }
void elevator_up(void) { elevator_state = 0; cs_set_consign(&mainboard.elevator.cs, elevator_pos_up); }
void fessor_auto(void) { fessor_state = WAIT_SENSOR; cs_set_consign(&mainboard.fessor.cs, fessor_pos_up); }
void fessor_down(void) { fessor_state = 0; cs_set_consign(&mainboard.fessor.cs, fessor_pos_down); }
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 fork_mid2(void) { cs_set_consign(&ballboard.forkrot.cs, FORKROT_MID2); }
void fork_deploy(void) { cs_set_consign(&ballboard.forkrot.cs, FORKROT_DEPLOYED); }
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 elevator_down(void) { elevator_state = 0; cs_set_consign(&mainboard.elevator.cs, elevator_pos_down); }
void elevator_auto(void) { elevator_state = WAIT_SENSOR; cs_set_consign(&mainboard.elevator.cs, elevator_pos_down); }
void fork_pack(void) { cs_set_consign(&ballboard.forkrot.cs, FORKROT_PACKED); }
void beacon_start(void) { beacon_reset_pos(); sensorboard.beacon.on = 1; cs_set_consign(&sensorboard.beacon.cs, 600); }
void fork_eject(void) { cs_set_consign(&ballboard.forkrot.cs, FORKROT_EJECT); }
void fessor_up(void) { fessor_state = 0; cs_set_consign(&mainboard.fessor.cs, fessor_pos_up); }