int main(void) { //wait_ms(3000); /// XXX hack to give time to the person that tests the system to take a cofee uart_init(); uart_com_init(); fdevopen(uart0_dev_send, uart0_dev_recv); //-------------------------------------------------------- // Error configuration error_register_emerg(log_event); error_register_error(log_event); error_register_warning(log_event); error_register_notice(log_event); error_register_debug(log_event); log_level = ERROR_SEVERITY_NOTICE; log_level = ERROR_SEVERITY_DEBUG; sei(); printf("%c[2J",0x1B); printf("%c[0;0H",0x1B); printf("Robotter 2011 - Galipeur - R3D2-2K10"); printf("Compiled "__DATE__" at "__TIME__"."); //NOTICE(0,"Initializing r3d2"); r3d2_init(); //NOTICE(0,"Initializing leds"); init_led(); //NOTICE(0,"Initializing scheduler"); scheduler_init(); scheduler_add_periodical_event_priority(&r3d2_monitor, NULL, 300, 50); scheduler_add_periodical_event_priority(&send_periodic_position_msg, NULL, 1000, 60); PORTA = ~(0x55); NOTICE(0,"Strike '?' for help"); while (1) { uart_com_monitor(); } }
/** schedule the trajectory event */ void schedule_event(struct trajectory *traj) { if ( traj->scheduler_task != -1) { DEBUG(E_TRAJECTORY, "Schedule event, already scheduled"); } else { traj->scheduler_task = scheduler_add_periodical_event_priority(&trajectory_manager_event, (void*)traj, TRAJ_EVT_PERIOD, 30); } }
int main(void) { #ifndef HOST_VERSION DDRB=0x18; DDRE=0x0C; #endif scheduler_init(); event_id = scheduler_add_periodical_event_priority(f1, NULL, 50000l/SCHEDULER_UNIT, 200); scheduler_add_periodical_event_priority(f2, NULL, 50000l/SCHEDULER_UNIT, 100); scheduler_add_periodical_event(f3, NULL, 1000000l/SCHEDULER_UNIT); // scheduler_add_single_event(supp,65); #ifndef HOST_VERSION sei(); #endif while(1); return 0; }
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_init(void) { scheduler_add_periodical_event_priority(elevator_cb, NULL, 1000L / SCHEDULER_UNIT, ELEVATOR_PRIO); }
void fessor_init(void) { scheduler_add_periodical_event_priority(fessor_cb, NULL, 1000L / SCHEDULER_UNIT, FESSOR_PRIO); }
int main(void) { #ifndef HOST_VERSION /* brake */ BRAKE_DDR(); BRAKE_OFF(); /* CPLD reset on PG3 */ DDRG |= 1<<3; PORTG &= ~(1<<3); /* implicit */ /* LEDS */ DDRJ |= 0x0c; DDRL = 0xc0; LED1_OFF(); LED2_OFF(); LED3_OFF(); LED4_OFF(); #endif memset(&gen, 0, sizeof(gen)); memset(&mainboard, 0, sizeof(mainboard)); mainboard.flags = DO_ENCODERS | DO_CS | DO_RS | DO_POS | DO_POWER | DO_BD | DO_ERRBLOCKING; ballboard.lcob = I2C_COB_NONE; ballboard.rcob = I2C_COB_NONE; /* UART */ uart_init(); uart_register_rx_event(CMDLINE_UART, emergency); #ifndef HOST_VERSION #if CMDLINE_UART == 3 fdevopen(uart3_dev_send, uart3_dev_recv); #elif CMDLINE_UART == 1 fdevopen(uart1_dev_send, uart1_dev_recv); #endif /* check eeprom to avoid to run the bad program */ if (eeprom_read_byte(EEPROM_MAGIC_ADDRESS) != EEPROM_MAGIC_MAINBOARD) { int c; sei(); printf_P(PSTR("Bad eeprom value ('f' to force)\r\n")); c = uart_recv(CMDLINE_UART); if (c == 'f') eeprom_write_byte(EEPROM_MAGIC_ADDRESS, EEPROM_MAGIC_MAINBOARD); wait_ms(100); bootloader(); } #endif /* ! HOST_VERSION */ /* LOGS */ error_register_emerg(mylog); error_register_error(mylog); error_register_warning(mylog); error_register_notice(mylog); error_register_debug(mylog); #ifndef HOST_VERSION /* SPI + ENCODERS */ encoders_spi_init(); /* this will also init spi hardware */ /* I2C */ i2c_init(I2C_MODE_MASTER, I2C_MAINBOARD_ADDR); i2c_protocol_init(); i2c_register_recv_event(i2c_recvevent); i2c_register_send_event(i2c_sendevent); /* TIMER */ timer_init(); timer0_register_OV_intr(main_timer_interrupt); /* PWM */ PWM_NG_TIMER_16BITS_INIT(1, TIMER_16_MODE_PWM_10, TIMER1_PRESCALER_DIV_1); PWM_NG_TIMER_16BITS_INIT(4, TIMER_16_MODE_PWM_10, TIMER4_PRESCALER_DIV_1); PWM_NG_INIT16(&gen.pwm1_4A, 4, A, 10, PWM_NG_MODE_SIGNED, &PORTD, 4); PWM_NG_INIT16(&gen.pwm2_4B, 4, B, 10, PWM_NG_MODE_SIGNED | PWM_NG_MODE_SIGN_INVERTED, &PORTD, 5); PWM_NG_INIT16(&gen.pwm3_1A, 1, A, 10, PWM_NG_MODE_SIGNED, &PORTD, 6); PWM_NG_INIT16(&gen.pwm4_1B, 1, B, 10, PWM_NG_MODE_SIGNED, &PORTD, 7); /* servos */ PWM_NG_TIMER_16BITS_INIT(3, TIMER_16_MODE_PWM_10, TIMER1_PRESCALER_DIV_256); PWM_NG_INIT16(&gen.servo1, 3, C, 10, PWM_NG_MODE_NORMAL, NULL, 0); PWM_NG_TIMER_16BITS_INIT(5, TIMER_16_MODE_PWM_10, TIMER1_PRESCALER_DIV_256); PWM_NG_INIT16(&gen.servo2, 5, A, 10, PWM_NG_MODE_NORMAL, NULL, 0); PWM_NG_INIT16(&gen.servo3, 5, B, 10, PWM_NG_MODE_NORMAL, NULL, 0); PWM_NG_INIT16(&gen.servo4, 5, C, 10, PWM_NG_MODE_NORMAL, NULL, 0); support_balls_deploy(); /* init pwm for servos */ #endif /* !HOST_VERSION */ /* SCHEDULER */ scheduler_init(); #ifdef HOST_VERSION hostsim_init(); robotsim_init(); #endif #ifndef HOST_VERSION scheduler_add_periodical_event_priority(do_led_blink, NULL, 100000L / SCHEDULER_UNIT, LED_PRIO); #endif /* !HOST_VERSION */ /* all cs management */ microb_cs_init(); /* TIME */ time_init(TIME_PRIO); /* sensors, will also init hardware adc */ sensor_init(); #ifndef HOST_VERSION /* start i2c slave polling */ scheduler_add_periodical_event_priority(i2c_poll_slaves, NULL, 8000L / SCHEDULER_UNIT, I2C_POLL_PRIO); #endif /* !HOST_VERSION */ /* strat */ gen.logs[0] = E_USER_STRAT; gen.log_level = 5; /* strat-related event */ scheduler_add_periodical_event_priority(strat_event, NULL, 25000L / SCHEDULER_UNIT, STRAT_PRIO); #ifndef HOST_VERSION /* eeprom time monitor */ scheduler_add_periodical_event_priority(do_time_monitor, NULL, 1000000L / SCHEDULER_UNIT, EEPROM_TIME_PRIO); #endif /* !HOST_VERSION */ sei(); strat_db_init(); printf_P(PSTR("\r\n")); printf_P(PSTR("Respect et robustesse.\r\n")); #ifndef HOST_VERSION { uint16_t seconds; seconds = eeprom_read_word(EEPROM_TIME_ADDRESS); printf_P(PSTR("Running since %d mn %d\r\n"), seconds/60, seconds%60); } #endif #ifdef HOST_VERSION strat_reset_pos(400, COLOR_Y(400), COLOR_A(-90)); #endif cmdline_interact(); return 0; }
void holonomic_trajectory_manager_event(void * param) { ///@todo : probablement des fonctions de la lib math qui font ça struct h_trajectory *traj = (struct h_trajectory *) param; double x = holonomic_position_get_x_double(traj->position); double y = holonomic_position_get_y_double(traj->position); vect2_cart vector_pos; int32_t s_consign = 0; /**< The speed consign */ int32_t a_consign = 0; /**< The angle consign */ int32_t o_consign = 0; /**< The angular speed (omega) consign */ float target_norm = sqrtf(pow(traj->xy_target.x, 2) + pow(traj->xy_target.y, 2)); // TODO : variable never used float position_norm = sqrtf(pow(x, 2) + pow(y, 2)); // TODO : variable never used int32_t distance2target = sqrtf(pow(x - traj->xy_target.x, 2) + pow(y - traj->xy_target.y, 2)); vector_pos.x = x; vector_pos.y = y; /** @todo : move the following in the right siwtch-case */ vect2_cart vec_target = {.x = traj->xy_target.x - x, .y = traj->xy_target.y - y}; vect2_cart vec_to_center = {.x = traj->circle_center.x - x, // TODO : variable never used .y = traj->circle_center.y - y}; static vect2_cart keyframe = {.x = -1, .y = -1}; /* step 1 : process new commands to quadramps */ switch (traj->moving_state) { case MOVING_STRAIGHT: /* Calcul de la consigne d'angle */ a_consign = TO_DEG(vect2_angle_vec_x_rad_cart(&vec_target)); /* Calcul de la consigne de vitesse */ s_consign = SPEED_ROBOT; if (distance2target < 250) s_consign = 2*distance2target; break; case MOVING_CIRCLE: if (keyframe.x < 0) { keyframe.x = traj->circle_center.x + cos(atan2f(y - traj->circle_center.y, x - traj->circle_center.x) - ANGLE_INC)*traj->radius; keyframe.y = traj->circle_center.y + sin(atan2f(y - traj->circle_center.y, x - traj->circle_center.x)-ANGLE_INC)*traj->radius; } traj->xy_target.x = keyframe.x; traj->xy_target.y = keyframe.y; /* Calcul de la consigne d'angle */ a_consign = TO_DEG(vect2_angle_vec_x_rad_cart(&vec_target)); printf("%d\n", a_consign); s_consign = SPEED_ROBOT/5; //if (distance2target < 250) ///@todo : faire un truc avec holonomic_length_arc_of_circle_pnt(traj, RAD)) plutôt. //s_consign = 2*distance2target; break; case MOVING_IDLE: break; } switch (traj->turning_state) { case TURNING_CAP: if(traj->a_target - holonomic_position_get_a_rad_double(traj->position) < 0 || traj->a_target - holonomic_position_get_a_rad_double(traj->position) > M_PI) o_consign = 50; else o_consign = -50;//holonomic_angle_2_x_rad(traj, traj->a_target);//cs_do_process(traj->csm_omega, holonomic_angle_2_x_rad(traj, ANG)); break; case TURNING_SPEEDOFFSET: o_consign = 1;//cs_do_process(traj->csm_omega, holonomic_angle_2_speed_rad(traj, ANG)); break; case TURNING_FACEPOINT: o_consign = 1;//cs_do_process(traj->csm_omega, holonomic_angle_facepoint_rad(traj, &FP)); break; case TURNING_IDLE: break; } /* step 3 : check the end of the move */ if (traj->turning_state == TURNING_IDLE && holonomic_robot_in_xy_window(traj, traj->d_win)) //@todo : not only distance, angle { if (traj->moving_state == MOVING_CIRCLE) { keyframe.x = traj->circle_center.x + cos(atan2f(y - traj->circle_center.y, x - traj->circle_center.x) - ANGLE_INC) * traj->radius; keyframe.y = traj->circle_center.y + sin(atan2f(y - traj->circle_center.y, x - traj->circle_center.x) - ANGLE_INC) * traj->radius; printf("x: %f y: %f\n", keyframe.x, keyframe.y); // If we have finished the circular trajectory /**@todo do not work beaucause the pos has changed -> take the depaeture of the pos of restart for scratch */ vect2_cart arrival = { .x = traj->circle_center.x + cos(atan2f(y - traj->circle_center.y, x - traj->circle_center.x) - traj->arc_angle) * traj->radius, .y = traj->circle_center.y + sin(atan2f(y - traj->circle_center.y, x - traj->circle_center.x) - traj->arc_angle) * traj->radius}; printf("Arrival : x: %f y: %f\n", arrival.x, arrival.y); if (vect2_dist_cart(&arrival, &vector_pos) < traj->d_win) { holonomic_delete_event(traj); } return; } if (prev_speed < 20) { traj->moving_state = MOVING_IDLE; holonomic_delete_event(traj); return; } else s_consign = 0; } if (traj->moving_state == MOVING_IDLE && holonomic_robot_in_angle_window(traj, traj->a_win)) { traj->turning_state = TURNING_IDLE; holonomic_delete_event(traj); return; } /* step 2 : pass the consign to rsh */ set_consigns_to_rsh(traj, s_consign, a_consign, o_consign); /** @todo : re-init le keyframe !!! */ } /** near the target (dist in x,y) ? */ uint8_t holonomic_robot_in_xy_window(struct h_trajectory *traj, double d_win) { vect2_cart vcp = {.x = holonomic_position_get_x_double(traj->position), .y = holonomic_position_get_y_double(traj->position)}; return (vect2_dist_cart(&vcp, &traj->xy_target) < d_win); } /** returns true if the robot is in an area enclosed by a certain angle */ uint8_t holonomic_robot_in_angle_window(struct h_trajectory *traj, double a_win_rad) { double d_a = traj->a_target - holonomic_position_get_a_rad_double(traj->position); if (ABS(d_a) < M_PI) { return (ABS(d_a) < (a_win_rad/2)); } else { return ((2 * M_PI-ABS(d_a)) < (a_win_rad/2)); } } /** remove event if any @todo */ void holonomic_delete_event(struct h_trajectory *traj) { prev_speed = 0; traj->end_of_traj = 1; set_consigns_to_rsh(traj, 0, holonomic_position_get_theta_v_int(traj->position), 0); /** do not work without this : */ rsh_set_speed(traj->robot,0); if ( traj->scheduler_task != -1) { DEBUG(E_TRAJECTORY, "Delete event"); scheduler_del_event(traj->scheduler_task); traj->scheduler_task = -1; } } /** schedule the trajectory event */ void holonomic_schedule_event(struct h_trajectory *traj) { if ( traj->scheduler_task != -1) { DEBUG(E_TRAJECTORY, "Schedule event, already scheduled"); } else { traj->scheduler_task = scheduler_add_periodical_event_priority(&holonomic_trajectory_manager_event, (void*)traj, TRAJ_EVT_PERIOD, 30); } } /** do a modulo 2.pi -> [-Pi,+Pi], knowing that 'a' is in [-3Pi,+3Pi] */ double holonomic_simple_modulo_2pi(double a) { if (a < -M_PI) { a += M_PI_2; } else if (a > M_PI) { a -= M_PI_2; } return a; } /** do a modulo 2.pi -> [-Pi,+Pi] */ double holonomic_modulo_2pi(double a) { double res = a - (((int32_t) (a/M_PI_2)) * M_PI_2); return holonomic_simple_modulo_2pi(res); }
int main(void) { // ADNS configuration adns6010_configuration_t adns_config; //-------------------------------------------------------------------------- // Booting // Turn interruptions ON sei(); // Initialize UART uart_init(); fdevopen(uart1_dev_send, uart1_dev_recv); //-------------------------------------------------------- // Error configuration error_register_emerg(log_event); error_register_error(log_event); error_register_warning(log_event); error_register_notice(log_event); error_register_debug(log_event); log_level = ERROR_SEVERITY_NOTICE; //log_level = ERROR_SEVERITY_DEBUG; // Clear screen printf("%c[2J",0x1B); printf("%c[0;0H",0x1B); // Some advertisment :p NOTICE(0,"Robotter 2009 - Galipeur - UNIOC-NG ADNS6010 CALIBRATION"); NOTICE(0,"Compiled "__DATE__" at "__TIME__"."); //-------------------------------------------------------- // Initialize scheduler scheduler_init(); //-------------------------------------------------------- // Initialize time time_init(160); //-------------------------------------------------------- // Initialize FPGA fpga_init(); // turn off led _SFR_MEM8(0x1800) = 1; //-------------------------------------------------------- // ADNS6010 //-------------------------------------------------------- NOTICE(0,"Initializing ADNS6010s"); adns6010_init(); #ifndef ADNS_OVERRIDE NOTICE(0,"Checking ADNS6010s firmware"); adns6010_checkFirmware(); // ADNS CONFIGURATION adns_config.res = ADNS6010_RES_2000; adns_config.shutter = ADNS6010_SHUTTER_OFF; adns_config.power = 0x11; NOTICE(0,"Checking ADNS6010s SPI communication"); adns6010_checkSPI(); NOTICE(0,"Booting ADNS6010s"); adns6010_boot(&adns_config); NOTICE(0,"Checking ADNS6010s"); adns6010_checks(); NOTICE(0,"ADNS6010s are GO"); #endif //-------------------------------------------------------- NOTICE(0,"Initializing ADCs"); adc_init(); // For ploting purposes NOTICE(0,"<PLOTMARK>"); // Set ADNS6010 system to automatic adns6010_setMode(ADNS6010_BHVR_MODE_AUTOMATIC); // Safe key event scheduler_add_periodical_event_priority(&safe_key_pressed, NULL, 100, 50); //---------------------------------------------------------------------- NOTICE(0,"Any key to go"); char cal_name = 'x'; uint8_t c; while(1) { c = cli_getkey(); if(c != -1) break; } //---------------------------------------------------------------------- //---------------------------------------------------------------------- int i,k; NOTICE(0,"Go"); // Initialize control systems cs_initialize(); cs_vx=0; cs_vy=0; cs_omega=0; // remove break motor_cs_break(0); event_cs = scheduler_add_periodical_event_priority(&cs_update, NULL, 25, 100); NOTICE(0,"Press 'r' for auto line calibration / 'l' for ADNS line calibration / 'm' for motor encoders line calibration / 'a' for angle calibration / 'n' non-auto angle calibration / 'k' motor non-auto angle calibration"); c = cli_getkey(); //----------------------------------------------------------------------------- // MOTOR ENCODERS LINE CALIBRATION //----------------------------------------------------------------------------- if( c=='m') { NOTICE(0,"Direction : '1' <- 0 ; '2' <- 2Pi/3 ; '3' <- 4Pi/3"); c = cli_getkey(); switch(c) { case '1': cal_name = 'A'; robot_angle = 0; break; case '2': cal_name = 'B'; robot_angle = -2*M_PI/3; break; case '3': cal_name = 'C'; robot_angle = -4*M_PI/3; break; default : cal_name = 'A'; robot_angle = 0; break; } NOTICE(0,"ME ZERO : Place robot in position zero then press a key"); setmotors_course(robot_angle, 0); while(!cli_getkey()); motor_encoders_get_value(&motor_zero); wait_ms(200); NOTICE(0,"ME zero values = (%ld,%ld,%ld)", motor_zero.vectors[0],motor_zero.vectors[1],motor_zero.vectors[2]); NOTICE(0,"P(init), press a key to continue"); while(!cli_getkey()); cs_vx=0; cs_vy=0; cs_omega=0; // perform calibration, positive direction motor_line_calibration(1); NOTICE(0,"ME ZERO : Place robot in position zero then press a key"); setmotors_course(robot_angle, 0); while(!cli_getkey()); motor_encoders_get_value(&motor_zero); wait_ms(200); NOTICE(0,"ME zero values = (%ld,%ld,%ld)", motor_zero.vectors[0],motor_zero.vectors[1],motor_zero.vectors[2]); NOTICE(0,"P(init), press a key to continue"); while(!cli_getkey()); // perform calibration, negative direction motor_line_calibration(-1); // output calibration data NOTICE(0,"CALIBRATION DONE, printint scilab matrix :"); printf("%c=[\n",cal_name); for(i=0;i<12;i++) for(k=0;k<3;k++) { printf("%7.1f",calibration_data[i][k]); if(k==2) printf(";\n"); else printf(" "); } printf("];\n\n"); } //----------------------------------------------------------------------------- // ADNS LINE CALIBRATION //----------------------------------------------------------------------------- if( c=='r') { NOTICE(0,"Direction : '1' <- 0 ; '2' <- 2Pi/3 ; '3' <- 4Pi/3"); c = cli_getkey(); switch(c) { case '1': cal_name = 'A'; robot_angle = 0; break; case '2': cal_name = 'B'; robot_angle = -2*M_PI/3; break; case '3': cal_name = 'C'; robot_angle = -4*M_PI/3; break; default : cal_name = 'A'; robot_angle = 0; break; } for(i=0;i<20;i++) { setmotors_course(robot_angle, 1); wait_ms(1000); setmotors_course(robot_angle, -1); wait_ms(1000); adns6010_encoders_get_value(&adns_zero); } } //----------------------------------------------------------------------------- // ADNS LINE CALIBRATION //----------------------------------------------------------------------------- if( c=='l') { NOTICE(0,"Direction : '1' <- 0 ; '2' <- 2Pi/3 ; '3' <- 4Pi/3"); c = cli_getkey(); switch(c) { case '1': cal_name = 'A'; robot_angle = 0; break; case '2': cal_name = 'B'; robot_angle = -2*M_PI/3; break; case '3': cal_name = 'C'; robot_angle = -4*M_PI/3; break; default : cal_name = 'A'; robot_angle = 0; break; } NOTICE(0,"ADNS ZERO : Place robot in position zero then press a key"); setmotors_course(robot_angle, 0); while(!cli_getkey()); adns6010_encoders_get_value(&adns_zero); wait_ms(200); NOTICE(0,"ADNS zero values = (%ld,%ld,%ld,%ld,%ld,%ld)", adns_zero.vectors[0],adns_zero.vectors[1],adns_zero.vectors[2], adns_zero.vectors[3],adns_zero.vectors[4],adns_zero.vectors[5]); NOTICE(0,"P(init), press a key to continue"); while(!cli_getkey()); cs_vx=0; cs_vy=0; cs_omega=0; // perform calibration, positive direction line_calibration(1); NOTICE(0,"ADNS ZERO : Place robot in position zero then press a key"); setmotors_course(robot_angle, 0); while(!cli_getkey()); adns6010_encoders_get_value(&adns_zero); wait_ms(200); NOTICE(0,"ADNS zero values = (%ld,%ld,%ld,%ld,%ld,%ld)", adns_zero.vectors[0],adns_zero.vectors[1],adns_zero.vectors[2], adns_zero.vectors[3],adns_zero.vectors[4],adns_zero.vectors[5]); NOTICE(0,"P(init), press a key to continue"); while(!cli_getkey()); // perform calibration, negative direction line_calibration(-1); // output calibration data NOTICE(0,"CALIBRATION DONE, printint scilab matrix :"); printf("%c=[\n",cal_name); for(i=0;i<12;i++) for(k=0;k<6;k++) { printf("%7.1f",calibration_data[i][k]); if(k==5) printf(";\n"); else printf(" "); } printf("];\n\n"); } //----------------------------------------------------------------------------- // ANGLE CALIBRATION //----------------------------------------------------------------------------- if( c=='a' ) { NOTICE(0,"Angle calibration is fully automatic, robot will do approx. 3 turns"); NOTICE(0,"Press a key to start, calibration will start ~5s after."); while(!cli_getkey()); NOTICE(0,"Starting in 5s..."); wait_ms(5000); angle_calibration(1); angle_calibration(-1); NOTICE(0,"Angle calibration done, press a key for results matrix : "); while(!cli_getkey()); printf("R=[\n"); for(i=0;i<12;i++) for(k=0;k<6;k++) { printf("%7.1f",calibration_data[i][k]); if(k==5) printf(";\n"); else printf(" "); } printf("];\n\n"); int i,k; for(i=0;i<12;i++) { printf("u_r%d = [0 0 %7.1f];\n",i,calibration_data_compass[i]); } printf("// "); for(i=0;i<12;i++) if(i==11) printf("u_r11];\n"); else printf("u_r%d;",i); } //----------------------------------------------------------------------------- // MOTOR NON-AUTO ANGLE CALIBRATION //----------------------------------------------------------------------------- if( c=='k' ) { NOTICE(0,"key to go"); while(!cli_getkey()); motor_angle_na_calibration(1); motor_angle_na_calibration(-1); NOTICE(0,"Angle calibration done, press a key for results matrix : "); while(!cli_getkey()); printf("R=[\n"); for(i=0;i<12;i++) for(k=0;k<3;k++) { printf("%7.1f",calibration_data[i][k]); if(k==2) printf(";\n"); else printf(" "); } printf("];\n\n"); } //----------------------------------------------------------------------------- // NON-AUTO ANGLE CALIBRATION //----------------------------------------------------------------------------- if( c=='n' ) { NOTICE(0,"key to go"); while(!cli_getkey()); angle_na_calibration(1); angle_na_calibration(-1); NOTICE(0,"Angle calibration done, press a key for results matrix : "); while(!cli_getkey()); printf("R=[\n"); for(i=0;i<12;i++) for(k=0;k<6;k++) { printf("%7.1f",calibration_data[i][k]); if(k==5) printf(";\n"); else printf(" "); } printf("];\n\n"); } EMERG(0,"Program ended"); while(1); return 0; }