bool GetupModule::processFrameChild() {
//std::cout << frame_info_->frame_id << " " << getName(state) << std::endl;
// set getting up odometry
if (isGettingUp()){
// which way are we getting up?
odometry_->getting_up_side_ = getUpSide;
} else {
// no getup
odometry_->getting_up_side_ = Getup::NONE;
// set getup side back to unknown for the next getup
getUpSide = Getup::UNKNOWN;
}
bool fallenCountHigh = (abs(walk_request_->roll_fallen_counter_) >= 50) || (abs(walk_request_->tilt_fallen_counter_) >= 50);
switch (state) {
case INITIAL:
if (walk_request_->getup_from_keeper_dive_) {
transitionToState(CROSS); // goalie needs cross
} else {
if (fallenCountHigh)
transitionToState(STIFFNESS_ON);
else
transitionToState(PREPARE_ARMS);
}
break;
case PREPARE_ARMS:
if ((getTimeInState() > PREPARE_ARMS_TIME) || fallenCountHigh) { // if fallen count is high, it's too late to prepare arms
transitionToState(STIFFNESS_OFF);
} else {
if (getTimeInState() < 0.015) {
prepareArms();
}
// otherwise do nothing
}
break;
case STIFFNESS_OFF:
if ((getTimeInState() > STIFFNESS_OFF_TIME) || fallenCountHigh) { // if fallen count is high, it's too late to worry about the stiffness being off
transitionToState(STIFFNESS_ON);
} else {
commands_->setAllStiffness(0.0,10);
}
break;
case STIFFNESS_ON:
if (getTimeInState() > 0.001) {
//if (armsStuckBehindBack()) {
//std::cout << "Trying to free arms" << std::endl;
//transitionToState(FREE_ARMS);
//} else {
transitionToState(EXECUTE);
//}
} else {
commands_->setAllStiffness(1.0,10);
}
break;
case CROSS:
if (getTimeInState() < CROSS_TIME)
cross();
else {
transitionToState(EXECUTE);
numCrosses++;
}
break;
case EXECUTE:
if (currMotion == Getup) {
selectGetup();
if (currMotion == Getup) // didn't choose
return true; // don't do anything else
// yay! we chose a getup
std::cout << "Starting getup: " << getName(currMotion) << std::endl;
stateStartTime = frame_info_->seconds_since_start; // reset the time
}
if (isMotionDoneExecuting()) {
if ((abs(walk_request_->tilt_fallen_counter_) > 2) || (abs(walk_request_->roll_fallen_counter_) > 2)) {
// still fallen
selectGetup();
transitionToState(STIFFNESS_ON);
} else {
transitionToState(STAND);
}
} else {
if (getTimeInState() < 0.015)
commands_->setAllStiffness(1.0,10);
//if (shouldAbortGetup()) { // KG: for now, never abort. Consider adding it back in if needed, but will need to also update method for new getup states
// std::cout << "FAILED GETUP" << std::endl;
// numRestarts++;
// transitionToState(PAUSE_BEFORE_RESTART);
//}
executeMotionSequence();
}
break;
case STAND:
// do nothing, handled in MotionCore
if (getTimeInState() > 0.5)
transitionToState(FINISHED);
break;
case FREE_ARMS:
currMotion = backFreeArms;
if (isMotionDoneExecuting()) {
currMotion = Getup;
transitionToState(EXECUTE);
} else {
if (getTimeInState() < 0.015)
commands_->setAllStiffness(1.0,10);
executeMotionSequence();
}
break;
case PAUSE_BEFORE_RESTART:
if (getTimeInState() > PAUSE_BEFORE_RESTART_TIME) {
transitionToState(STIFFNESS_ON);
} else {
commands_->setAllStiffness(0.0,10);
}
break;
default:
return false;
break;
}
return true;
}
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
}
