// initialize for walking - assume walk ready pose void walk_init() { int commStatus = 0; // reset walk state and command walk_state = 0; walk_command = 0; // and get ready for walking! current_motion_page = COMMAND_WALK_READY_MP; executeMotion(current_motion_page); // experimental - increase punch for walking commStatus = dxl_write_word(BROADCAST_ID, DXL_PUNCH_L, 100); if(commStatus != COMM_RXSUCCESS) { printf("\nDXL_PUNCH Broadcast - "); dxl_printCommStatus(dxl_get_result()); } }
int main(void) { // local variables int sensor_flag, command_flag, comm_status, sensor_process_flag, obstacle_flag, wait_flag; unsigned long wait_timer = 0; unsigned long wait_time = 0; uint8 motion_flag = 0; // TIMING: unsigned long timer1, timer2, timer3, timer4; // Initialization Routines led_init(); // switches all 6 LEDs on serial_init(57600); // serial port at 57600 baud buzzer_init(); // enable buzzer melodies button_init(); // enable push buttons on CM-510 delay_ms(200); // wait 0.2s led_off(ALL_LED); // and switch them back off // initialize the clock clock_init(); wait_flag = 0; // enable interrupts sei(); // print welcome message printf("\nBioloid C Control V0.8\n"); printf("Press the START button on the CM-510 to continue.\n"); // reset the start button variable, something triggers the interrupt on start-up start_button_pressed = FALSE; // initialize motion pages motionPageInit(); // Wait for the START Button before going any further while(!start_button_pressed) { // PLAY LED is flashing at 5Hz led_toggle(LED_PLAY); delay_ms(200); } // Now turn LED solid led_on(LED_PLAY); // and reset the start button variable start_button_pressed = FALSE; // perform high level initialization of Dynamixel bus and servos dxl_init(DEFAULT_BAUDNUMBER); // assume initial pose executeMotion(COMMAND_BALANCE_MP); // set the walk state walk_setWalkState(0); obstacle_flag = 0; // initialize the PID controller for balancing #ifdef ACCEL_AND_ULTRASONIC pid_init(); setupGyroKalman(); #endif // initialize the ADC and take default readings delay_ms(4000); // wait 4s for gyros to stabilize adc_init(); sensor_process_flag = 0; sensor_flag = 0; // print out default sensor values #ifdef GYRO_AND_DMS_ONLY printf("\nBattery = %imV, Gyro X, Y Center = %i %i ", adc_battery_val, adc_gyrox_center, adc_gyroy_center); #endif #ifdef ACCEL_AND_ULTRASONIC printf("\nBattery, Gyro X, Y Accel X, Y Center = %imV %i %i %i %i", adc_battery_val, adc_gyrox_center, adc_gyroy_center, adc_accelx_center, adc_accely_center); #endif // write out the command prompt printf( "\nReady for command.\n> "); // TIMING: timer4 = micros(); // main command loop (takes 28us when idle) // keeps executing unless we encounter a major alarm while( !major_alarm ) { // Check if we received a new command command_flag = serialReceiveCommand(); // takes 4ms if new command (largely because of printf) // TIMING: timer1 = micros() - timer4; // see if we are in a wait command if ( bioloid_command == COMMAND_WAIT_MILLISECONDS || bioloid_command == COMMAND_WAIT_SECONDS ) { // first look if we should continue waiting if ( wait_flag == 1 ) { // check timer if ( millis() - wait_timer > wait_time ) { // wait time is finished - reset the wait flag, timer and time wait_flag = 0; wait_timer = 0; wait_time = 0; // read next command from command sequence if ( flag_motion_sequence == 1 ) { bioloid_command = command_sequence_buffer[command_sequence_counter][0]; next_motion_page = command_sequence_buffer[command_sequence_counter][1]; // update pointer if there are more commands left if ( command_sequence_counter < command_sequence_length ) { command_sequence_counter++; } else { // sequence is finished, reset all sequence related variables flag_motion_sequence = 0; command_sequence_counter = 0; command_sequence_length = 0; bioloid_command = COMMAND_STOP; } } } } if ( command_flag == 1 && wait_flag == 0 ) { // wait command has only just been received, calculate wait time wait_timer = millis(); command_flag = 0; wait_flag = 1; wait_time = next_motion_page; } else if ( command_flag == 1 && wait_flag == 1 ) { // wait command is still in progress but new command has been received // check for STOP, otherwise ignore if ( bioloid_command == COMMAND_STOP ) { // reset the wait flag, timer and time wait_flag = 0; wait_timer = 0; wait_time = 0; } } } // check if start button has been pressed and we need to do emergency stop if ( start_button_pressed && bioloid_command != COMMAND_STOP ) { // disable torque & reset current command comm_status = dxl_write_byte(BROADCAST_ID, DXL_TORQUE_ENABLE, 0); last_bioloid_command = bioloid_command; bioloid_command = COMMAND_STOP; command_flag = 1; // and reset the start button variable start_button_pressed = FALSE; } else if ( start_button_pressed && bioloid_command == COMMAND_STOP ) { // we are resuming from an emergency stop, restore last command bioloid_command = last_bioloid_command; last_bioloid_command = COMMAND_STOP; command_flag = 1; // and reset the start button variable start_button_pressed = FALSE; } // Check if we need to read the sensors sensor_flag = adc_readSensors(); // takes 0.6ms for gyro/accel and 0.9ms including DMS/ultrasonic (156us per channel) if ( sensor_flag == 1 ) { // new sensor data - process and update command flag if necessary sensor_process_flag = adc_processSensorData(); if ( command_flag == 0 && sensor_process_flag == 1 ) { // robot has slipped, front/back getup command has been issued command_flag = 1; } else if ( sensor_process_flag == 2 ) { // if the sensor process flag = 2 it means low voltage emergency stop major_alarm = TRUE; } } // obstacle avoidance for walking if ( walk_getWalkState() != 0 ) { // currently very basic - turn left until path is clear obstacle_flag = walk_avoidObstacle(obstacle_flag); if ( command_flag == 0 && (obstacle_flag == 1 || obstacle_flag == -1) ) { command_flag = 1; } } // set the new command global variable if( command_flag == 1 ) { new_command = TRUE; command_flag = 0; // if we are coming out of BAL command, reset joint offsets if( last_bioloid_command == COMMAND_BALANCE && bioloid_command != COMMAND_BALANCE ) { for (uint8 i=0; i<NUM_AX12_SERVOS; i++) { joint_offset[i] = 0; } } } // TEST printf("\n Command %i, New %i, MP %i, Next MP %i ", bioloid_command, new_command, current_motion_page, next_motion_page); // TIMING: timer2 = micros() - timer4 - timer1; #ifdef ACCEL_AND_ULTRASONIC // static balancing if ( bioloid_command == COMMAND_BALANCE && major_alarm != TRUE ) { // static balancing uses Kalman Filter or PID controller depending on availability of accelerometer // first make sure the PID is turned on if ( pid_getMode() != AUTOMATIC ) { pid_setMode(AUTOMATIC); } staticRobotBalance(); } else if ( pid_getMode() == 1 ) { pid_setMode(MANUAL); } #endif // execute motion steps if ( major_alarm != TRUE ) { motion_flag = executeMotionSequence(); // takes 2.1ms when executing a step during walking or 3.3ms if unpacking a new motion page } // TIMING: timer3 = micros() - timer4 - timer1 - timer2; // TIMING: printf("Timer 1 = %lu, 2 = %lu, 3 = %lu, SFlag = %i\n", timer1, timer2, timer3, sensor_flag); // TIMING: timer4 = micros(); } // end of main command loop }