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