void firmware_init(){
// Initialize firmware
dxl_initialize(0,1); //initialize dynamixel communication
serial_initialize(57600); //initialize serial communication
//initialize sensors
sensor_init(SENSOR_FRONT,SENSOR_DISTANCE);
sensor_init(SENSOR_FRONTLEFT,SENSOR_IR);
sensor_init(SENSOR_FRONTRIGHT,SENSOR_IR);
sensor_init(SENSOR_BACKLEFT,SENSOR_IR);
sensor_init(SENSOR_BACKRIGHT,SENSOR_IR);
//initialize I/O
io_init();
io_set_interrupt(BTN_START, &reset_state); //assign callback function when start button is pressed
// Activate general interrupts
sei();
// Set motors to wheel mode
motor_set_mode(254, MOTOR_WHEEL_MODE);
// Set serial communication through ZigBee
serial_set_zigbee();
}
/**
* main program
*/
void main(void)
{
uint16_t task_cnt_led;
uint16_t task_cnt_motor;
uint16_t task_cnt_pwm;
uint16_t task_drv;
uint16_t task_cnt_timeout;
uint16_t task_pid;
uint16_t task_cnt_sound;
init();
task_cnt_led = TASK_INIT_LED;
task_cnt_motor = TASK_INIT_MOTOR;
task_cnt_pwm = TASK_INIT_PWM;
task_drv = TASK_DRV;
task_cnt_timeout = TASK_INIT_TIMEOUT;
task_pid = TASK_PID;
task_cnt_sound = TASK_INIT_SOUND;
for(;;)
{
if(rtc_get_clear_flag() != RTC_NONE) {
/* Switch load measurement LED on */
//led_y_on();
#if LED_LOAD
led_load_on();
#endif
/* Task to toggle LED0 */
if(task_cnt_led == 0) {
task_cnt_led = TASK_PERIOD_LED; /* Prepare scheduler for next period */
led_r_toggle();
}
else {
task_cnt_led--;
}
/* Task to handle the DRV-Chip */
if(task_drv == 0) {
task_drv = TASK_DRV; /* Prepare scheduler for next period */
handleDrv();
}
else {
task_drv--;
}
/* Task to handle the PID */
if(task_pid == 0) {
task_pid = TASK_PID; /* Prepare scheduler for next period */
pid_task();
}
else {
task_pid--;
}
/* Task to control commutation frequency */
if(task_cnt_motor == 0) {
task_cnt_motor = TASK_PERIOD_MOTOR; /* Prepare scheduler for next period */
motor_task(); /* Perform motor task */
}
else {
task_cnt_motor--;
}
/* Task to control timeout for protecting motor windings from overheating */
if(task_cnt_timeout == 0) {
task_cnt_timeout = TASK_PERIOD_TIMEOUT; /* Prepare scheduler for next period */
motor_task_timeout();
}
else {
task_cnt_timeout--;
}
/* Task to toggle PWM */
if(task_cnt_pwm == 0) {
static uint8_t motor_pid_flag = 0;
task_cnt_pwm = TASK_PERIOD_PWM; /* Prepare scheduler for next period */
if( (motor_get_status() == MOTOR_MODE_BRAKE) || (motor_get_status() == MOTOR_MODE_OFF))
motor_pid_flag = 0;
if (motor_pid_flag == 1) {
motor_set_mode(MOTOR_MODE_RUN_PID);
motor_pid_flag = 2;
}
if (motor_pid_flag == 0) {
if (motor_get_status() == MOTOR_STATUS_AUTO_FREE) {
motor_pid_flag = 1;
}
}
}
else {
task_cnt_pwm--;
}
/* Task to generate Sound */
if (task_cnt_sound == 0) {
sound_task();
task_cnt_sound = sound_get_time();
}
else {
task_cnt_sound--;
}
/* Other tasks come here */
/* Switch load measurement led off */
//led_y_off();
#if LED_LOAD
led_load_off();
#endif
}
__RESET_WATCHDOG(); /* feeds the dog */
}
/* please make sure that you never leave main */
}
