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