/* This function is called once, as soon as the button is pressed * for the robot to begin walking. It is used for initialization. */ void walkControl_entry(void) { disable_motors(); // Clear old motor commands; // Always start with the outer feet in stance, and the inner feet tracking a scissor gait WALK_FSM_MODE = Glide_Out; WALK_FSM_MODE_PREV = Glide_Out; }
int main(){ initialize_board(); enable_motors(); // bring H-bridges of of standby set_led(GREEN,ON); set_led(RED,OFF); // Forward set_motor_all(SPEED); printf("\nAll Motors Forward\n"); sleep(2); // Reverse set_motor_all(-SPEED); printf("All Motors Reverse\n"); sleep(2); // Stop set_motor_all(0); disable_motors(); //put H-bridges into standby printf("All Motors Off\n\n"); cleanup_board(); return 0; }
/*********************************************************************** * drive_stack * This is the medium between the user_interface struct and the * physical servos and motors ************************************************************************/ void* drive_stack(void* ptr){ int i; // general purpose counter for for loops float net_drive, net_turn, net_torque_split; // exiting condition is checked inside the switch case instead while(1){ switch (get_state()){ case EXITING: return NULL; case PAUSED: disable_motors(); // not much to do if paused! break; // when running, drive_stack checks if an input mode // like mavlink, DSM2, or bluetooth is enabled // and moves the servos and motors corresponding to // user input and current controller mode case RUNNING: if(user_interface.input_mode == NONE){ cstate.servos[0]=config.serv1_center-config.turn_straight; cstate.servos[1]=config.serv2_center+config.turn_straight; cstate.servos[2]=config.serv3_center-config.turn_straight; cstate.servos[3]=config.serv4_center+config.turn_straight; for (i=1; i<=4; i++){ send_servo_pulse_normalized(i,cstate.servos[i-1]); } disable_motors(); break; } enable_motors(); // now send input to servos and motors based on drive mode and UI // motors 2 and 3 have negative polarity in the config.txt file so that positive sign in this file = // clockwise spin with a forward input. Eg. all of the net_drive values are positive in spin mode. // 1 3 0 2 New version is 1 2 0 1 to match the orientation of motor wire // 2 4 1 3 4 3 3 2 connectors on cape switch(user_interface.drive_mode){ case LANECHANGE: // lane change maneuver net_drive = user_interface.drive_stick*config.motor_max; net_turn = user_interface.turn_stick*(0.5-config.turn_straight); cstate.motors[0]=net_drive*config.mot1_polarity; cstate.motors[1]=net_drive*config.mot2_polarity; cstate.motors[2]=net_drive*config.mot3_polarity; cstate.motors[3]=net_drive*config.mot4_polarity; cstate.servos[0]=config.serv1_center-config.turn_straight+net_turn; cstate.servos[1]=config.serv2_center+config.turn_straight+net_turn; cstate.servos[2]=config.serv3_center-config.turn_straight+net_turn; cstate.servos[3]=config.serv4_center+config.turn_straight+net_turn; break; case NORMAL_4W: // Normal 4W Steering net_drive = user_interface.drive_stick*config.motor_max; net_turn = user_interface.turn_stick*config.normal_turn_range; net_torque_split = user_interface.turn_stick*config.torque_vec_const*net_drive; cstate.motors[0]=(net_drive+net_torque_split)*config.mot1_polarity; cstate.motors[1]=(net_drive+net_torque_split)*config.mot2_polarity; cstate.motors[2]=(net_drive-net_torque_split)*config.mot3_polarity; cstate.motors[3]=(net_drive-net_torque_split)*config.mot4_polarity; cstate.servos[0]=config.serv1_center-config.turn_straight+net_turn; cstate.servos[1]=config.serv2_center+config.turn_straight+net_turn; cstate.servos[2]=config.serv3_center-config.turn_straight-net_turn; cstate.servos[3]=config.serv4_center+config.turn_straight-net_turn; break; // crab, turn all wheels sideways and drive case CRAB: net_drive = user_interface.drive_stick*config.motor_max; net_turn = user_interface.turn_stick*config.crab_turn_const\ *(net_drive+0.5); cstate.motors[0]=(net_drive+net_turn)*config.mot1_polarity; cstate.motors[1]=-(net_drive+net_turn)*config.mot2_polarity; cstate.motors[2]=-(net_drive-net_turn)*config.mot3_polarity; cstate.motors[3]=(net_drive-net_turn)*config.mot4_polarity; cstate.servos[0]=config.serv1_center+config.turn_crab; cstate.servos[1]=config.serv2_center-config.turn_crab; cstate.servos[2]=config.serv3_center+config.turn_crab; cstate.servos[3]=config.serv4_center-config.turn_crab; break; case SPIN: net_drive = user_interface.turn_stick*config.motor_max; cstate.motors[0]=net_drive*config.mot1_polarity; cstate.motors[1]=-net_drive*config.mot2_polarity; cstate.motors[2]=-net_drive*config.mot3_polarity; cstate.motors[3]=net_drive*config.mot4_polarity; cstate.servos[0]=config.serv1_center+config.turn_spin; cstate.servos[1]=config.serv2_center-config.turn_spin; cstate.servos[2]=config.serv3_center+config.turn_spin; cstate.servos[3]=config.serv4_center-config.turn_spin; break; default: printf("unknown drive_mode\n"); disable_motors(); for (i=1; i<=4; i++){ cstate.motors[i-1]=0; cstate.servos[i-1]=0.5; } break; }// end of switch(drive_mode) // send pulses to servos and drive motors for (i=1; i<=4; i++){ saturate_number(&cstate.servos[i-1],config.turn_min,config.turn_max); saturate_number(&cstate.motors[i-1],-config.motor_max,config.motor_max); set_motor(i,cstate.motors[i-1]); send_servo_pulse_normalized(i,cstate.servos[i-1]); } default: break; } // end of switch get_state() // run about as fast as the core itself usleep(1000000/CONTROL_HZ); } return NULL; }
/****************************************************************** * disarm_controller() * - disable motors * - set the setpoint.core_mode to DISARMED to stop the * controller *******************************************************************/ int disarm_controller(){ disable_motors(); setpoint.arm_state = DISARMED; return 0; }
/****************************************************************** * balance_core() * discrete-time balance controller operated off IMU interrupt * Called at SAMPLE_RATE_HZ *******************************************************************/ int balance_core(){ // local variables only in memory scope of balance_core static int D1_saturation_counter = 0; float compensated_D1_output = 0; float dutyL = 0; float dutyR = 0; static log_entry_t new_log_entry; float output_scale; //battery voltage/nominal voltage // if an IMU packet read failed, ignore and just return // the mpu9150_read function may print it's own warnings if (mpu9150_read(&mpu) != 0){ return -1; } /************************************************************** * STATE_ESTIMATION * read sensors and compute the state regardless of if the * controller is ARMED or DISARMED ***************************************************************/ // angle theta is positive in the direction of forward tip // add mounting angle of BBB cstate.theta[2] = cstate.theta[1]; cstate.theta[1] = cstate.theta[0]; cstate.theta[0] = mpu.fusedEuler[VEC3_X] + config.bb_mount_angle; cstate.current_theta = cstate.theta[0]; cstate.d_theta = (cstate.theta[0]-cstate.theta[1])/DT; // collect encoder positions cstate.wheelAngleR = -(get_encoder_pos(config.encoder_channel_R) * 2*PI) \ /(config.gearbox * config.encoder_res); cstate.wheelAngleL = (get_encoder_pos(config.encoder_channel_L) * 2*PI) \ /(config.gearbox * config.encoder_res); // log phi estimate // wheel angle is relative to body, // add theta body angle to get absolute wheel position cstate.phi[2] = cstate.phi[1]; cstate.phi[1] = cstate.phi[0]; cstate.phi[0] = ((cstate.wheelAngleL + cstate.wheelAngleR)/2) +cstate.current_theta; cstate.current_phi = cstate.phi[0]; cstate.d_phi = (cstate.phi[0]-cstate.phi[1])/DT; // body turning estimation cstate.gamma[2] = cstate.gamma[1]; cstate.gamma[1] = cstate.phi[0]; cstate.gamma[0]=(cstate.wheelAngleL-cstate.wheelAngleR) \ * (config.wheel_radius/config.track_width); cstate.d_gamma = (cstate.gamma[0]-cstate.gamma[1])/DT; cstate.current_gamma = cstate.gamma[0]; // output scaling output_scale = cstate.vBatt/config.v_nominal; /************************************************************* * Control based on the robotics_library defined state variable * PAUSED: make sure the controller stays DISARMED * RUNNING: Normal operation of controller. * - check for tipover * - wait for MiP to be within config.start_angle of * upright * - choose mode from setpoint.core_mode * - evaluate difference equation and check saturation * - actuate motors ***************************************************************/ switch (get_state()){ // make sure things are off if program is closing case EXITING: disable_motors(); return 0; // if the controller is somehow still ARMED, disarm it case PAUSED: if(setpoint.arm_state==ARMED){ disarm_controller(); } break; // normal operating mode case RUNNING: // exit if the controller was not armed properly if(setpoint.arm_state==DISARMED){ return 0; } // check for a tipover before anything else if(fabs(cstate.current_theta)>config.tip_angle){ disarm_controller(); printf("tip detected \n"); break; } /********************************************************** * POSITION Phi controller * feedback control of wheel angle Phi by outputting a * reference theta body angle. This is controller D2 in * config ***********************************************************/ if(setpoint.core_mode == POSITION){ // move the position set points based on user input if(setpoint.phi_dot != 0.0){ //setpoint.phi == cstate.current_phi + setpoint.phi_dot*DT; setpoint.phi += setpoint.phi_dot * DT; } // march the different equation terms for the input Phi Error // and the output theta reference angle cstate.ePhi[2] = cstate.ePhi[1]; cstate.ePhi[1] = cstate.ePhi[0]; cstate.ePhi[0] = setpoint.phi-cstate.current_phi; cstate.theta_ref[2] = cstate.theta_ref[1]; cstate.theta_ref[1] = cstate.theta_ref[0]; // evaluate D2 difference equation cstate.theta_ref[0] = config.K_D2*( \ config.numD2_2 * cstate.ePhi[2] \ + config.numD2_1 * cstate.ePhi[1] \ + config.numD2_0 * cstate.ePhi[0]) \ -(config.denD2_2 * cstate.theta_ref[2] \ + config.denD2_1 * cstate.theta_ref[1]); //check saturation of outer loop theta reference output signal saturate_float(&cstate.theta_ref[0],-config.theta_ref_max,config.theta_ref_max); setpoint.theta = cstate.theta_ref[0]; } // evaluate inner loop controller D1z cstate.eTheta[2] = cstate.eTheta[1]; cstate.eTheta[1] = cstate.eTheta[0]; cstate.eTheta[0] = setpoint.theta - cstate.current_theta; cstate.u[2] = cstate.u[1]; cstate.u[1] = cstate.u[0]; cstate.u[0] = \ config.K_D1 * (config.numD1_0 * cstate.eTheta[0] \ + config.numD1_1 * cstate.eTheta[1] \ + config.numD1_2 * cstate.eTheta[2]) \ - (config.denD1_1 * cstate.u[1] \ + config.denD1_2 * cstate.u[2]); // check saturation of inner loop knowing that right after // this control will be scaled by battery voltage if(saturate_float(&cstate.u[0], -output_scale, output_scale)){ D1_saturation_counter ++; if(D1_saturation_counter > SAMPLE_RATE_HZ*config.pickup_detection_time){ printf("inner loop controller saturated\n"); disarm_controller(); D1_saturation_counter = 0; break; } } else{ D1_saturation_counter = 0; } cstate.current_u = cstate.u[0]; // scale output to compensate for battery charge level compensated_D1_output = cstate.u[0] / output_scale; // // integrate the reference theta to correct for imbalance or sensor // // only if standing relatively still with zero phi reference // // to-do, wait for stillness for a time period before integrating // if(setpoint.phi==0 && fabs(cstate.phi_dot)<2){ // state.thetaTrim += (config.kTrim*cstate.theta_ref[0]) * DT; // } //steering controller // move the controller set points based on user input setpoint.gamma += setpoint.gamma_dot * DT; cstate.egamma[1] = cstate.egamma[0]; cstate.egamma[0] = setpoint.gamma - cstate.current_gamma; cstate.duty_split = config.KP_steer*(cstate.egamma[0] \ +config.KD_steer*(cstate.egamma[0]-cstate.egamma[1])); // if the steering input would saturate a motor, reduce // the steering input to prevent compromising balance input if(fabs(compensated_D1_output)+fabs(cstate.duty_split) > 1){ if(cstate.duty_split > 0){ cstate.duty_split = 1-fabs(compensated_D1_output); } else cstate.duty_split = -(1-fabs(compensated_D1_output)); } // add D1 balance controller and steering control dutyL = compensated_D1_output - cstate.duty_split; dutyR = compensated_D1_output + cstate.duty_split; // send to motors // one motor is flipped on chassis so reverse duty to L set_motor(config.motor_channel_L,-dutyL); set_motor(config.motor_channel_R,dutyR); cstate.time_us = microsSinceEpoch(); // pass new information to the log with add_to_buffer // this only puts information in memory, doesn't // write to disk immediately if(config.enable_logging){ new_log_entry.time_us = cstate.time_us-core_start_time_us; new_log_entry.theta = cstate.current_theta; new_log_entry.theta_ref = setpoint.theta; new_log_entry.phi = cstate.current_phi; new_log_entry.u = cstate.current_u; add_to_buffer(new_log_entry); } // end of normal balancing routine // last_state will be updated beginning of next interrupt break; default: break; // nothing to do if UNINITIALIZED } return 0; }
int initialize_cape(){ FILE *fd; // opened and closed for each file char path[128]; // buffer to store file path string int i = 0; // general use counter printf("\n"); // check if another project was using resources // kill that process cleanly with sigint if so fd = fopen(LOCKFILE, "r"); if (fd != NULL) { int old_pid; fscanf(fd,"%d", &old_pid); if(old_pid != 0){ printf("warning, shutting down existing robotics project\n"); kill((pid_t)old_pid, SIGINT); sleep(1); } // close and delete the old file fclose(fd); remove(LOCKFILE); } // create new lock file with process id fd = fopen(LOCKFILE, "ab+"); if (fd < 0) { printf("\n error opening LOCKFILE for writing\n"); return -1; } pid_t current_pid = getpid(); printf("Current Process ID: %d\n", (int)current_pid); fprintf(fd,"%d",(int)current_pid); fflush(fd); fclose(fd); // ensure gpios are exported printf("Initializing GPIO\n"); for(i=0; i<NUM_OUT_PINS; i++){ if(gpio_export(out_gpio_pins[i])){ printf("failed to export gpio %d", out_gpio_pins[i]); return -1; }; gpio_set_dir(out_gpio_pins[i], OUTPUT_PIN); } // set up default values for some gpio disable_motors(); deselect_spi1_slave(1); deselect_spi1_slave(2); //Set up PWM printf("Initializing PWM\n"); i=0; for(i=0; i<4; i++){ strcpy(path, pwm_files[i]); strcat(path, "polarity"); fd = fopen(path, "a"); if(fd<0){ printf("PWM polarity not available in /sys/class/devices/ocp.3\n"); return -1; } //set correct polarity such that 'duty' is time spent HIGH fprintf(fd,"%c",'0'); fflush(fd); fclose(fd); } //leave duty cycle file open for future writes for(i=0; i<4; i++){ strcpy(path, pwm_files[i]); strcat(path, "duty"); pwm_duty_pointers[i] = fopen(path, "a"); } //read in the pwm period defined in device tree overlay .dts strcpy(path, pwm_files[0]); strcat(path, "period"); fd = fopen(path, "r"); if(fd<0){ printf("PWM period not available in /sys/class/devices/ocp.3\n"); return -1; } fscanf(fd,"%i", &pwm_period_ns); fclose(fd); // mmap pwm modules to get fast access to eQep encoder position // see mmap_eqep example program for more mmap and encoder info printf("Initializing eQep Encoders\n"); int dev_mem; if ((dev_mem = open("/dev/mem", O_RDWR | O_SYNC))==-1){ printf("Could not open /dev/mem \n"); return -1; } pwm_map_base[0] = mmap(0,getpagesize(),PROT_READ|PROT_WRITE,MAP_SHARED,dev_mem,PWM0_BASE); pwm_map_base[1] = mmap(0,getpagesize(),PROT_READ|PROT_WRITE,MAP_SHARED,dev_mem,PWM1_BASE); pwm_map_base[2] = mmap(0,getpagesize(),PROT_READ|PROT_WRITE,MAP_SHARED,dev_mem,PWM2_BASE); if(pwm_map_base[0] == (void *) -1) { printf("Unable to mmap pwm \n"); return(-1); } close(dev_mem); // Test eqep and reset position for(i=1;i<3;i++){ if(set_encoder_pos(i,0)){ printf("failed to access eQep register\n"); printf("eQep driver not loaded\n"); return -1; } } //set up function pointers for button press events printf("starting button interrupts\n"); set_pause_pressed_func(&null_func); set_pause_unpressed_func(&null_func); set_mode_pressed_func(&null_func); set_mode_unpressed_func(&null_func); initialize_button_handlers(); // Load binary into PRU printf("Starting PRU servo controller\n"); if(initialize_pru_servos()){ printf("WARNING: PRU init FAILED"); } // Print current battery voltage printf("Battery Voltage = %fV\n", getBattVoltage()); // Start Signal Handler printf("Enabling exit signal handler\n"); signal(SIGINT, ctrl_c); // all done set_state(PAUSED); printf("\nRobotics Cape Initialized\n"); return 0; }
/***************************************************************** * int initialize_cape() * sets up necessary hardware and software * should be the first thing your program calls *****************************************************************/ int initialize_cape(){ FILE *fd; printf("\n"); // check if another project was using resources // kill that process cleanly with sigint if so #ifdef DEBUG printf("checking for existing PID_FILE\n"); #endif fd = fopen(PID_FILE, "r"); if (fd != NULL) { int old_pid; fscanf(fd,"%d", &old_pid); if(old_pid != 0){ printf("warning, shutting down existing robotics project\n"); kill((pid_t)old_pid, SIGINT); sleep(1); } // close and delete the old file fclose(fd); remove(PID_FILE); } // create new pid file with process id #ifdef DEBUG printf("opening PID_FILE\n"); #endif fd = fopen(PID_FILE, "ab+"); if (fd < 0) { printf("\n error opening PID_FILE for writing\n"); return -1; } pid_t current_pid = getpid(); printf("Current Process ID: %d\n", (int)current_pid); fprintf(fd,"%d",(int)current_pid); fflush(fd); fclose(fd); // check the device tree overlay is actually loaded if (is_cape_loaded() != 1){ printf("ERROR: Device tree overlay not loaded by cape manager\n"); return -1; } // initialize mmap io libs printf("Initializing GPIO\n"); if(initialize_gpio()){ printf("mmap_gpio_adc.c failed to initialize gpio\n"); return -1; } //export all GPIO output pins gpio_export(RED_LED); gpio_set_dir(RED_LED, OUTPUT_PIN); gpio_export(GRN_LED); gpio_set_dir(GRN_LED, OUTPUT_PIN); gpio_export(MDIR1A); gpio_set_dir(MDIR1A, OUTPUT_PIN); gpio_export(MDIR1B); gpio_set_dir(MDIR1B, OUTPUT_PIN); gpio_export(MDIR2A); gpio_set_dir(MDIR2A, OUTPUT_PIN); gpio_export(MDIR2B); gpio_set_dir(MDIR2B, OUTPUT_PIN); gpio_export(MDIR3A); gpio_set_dir(MDIR3A, OUTPUT_PIN); gpio_export(MDIR3B); gpio_set_dir(MDIR3B, OUTPUT_PIN); gpio_export(MDIR4A); gpio_set_dir(MDIR4A, OUTPUT_PIN); gpio_export(MDIR4B); gpio_set_dir(MDIR4B, OUTPUT_PIN); gpio_export(MOT_STBY); gpio_set_dir(MOT_STBY, OUTPUT_PIN); gpio_export(PAIRING_PIN); gpio_set_dir(PAIRING_PIN, OUTPUT_PIN); gpio_export(SPI1_SS1_GPIO_PIN); gpio_set_dir(SPI1_SS1_GPIO_PIN, OUTPUT_PIN); gpio_export(SPI1_SS2_GPIO_PIN); gpio_set_dir(SPI1_SS2_GPIO_PIN, OUTPUT_PIN); gpio_export(INTERRUPT_PIN); gpio_set_dir(INTERRUPT_PIN, INPUT_PIN); gpio_export(SERVO_PWR); gpio_set_dir(SERVO_PWR, OUTPUT_PIN); printf("Initializing ADC\n"); if(initialize_adc()){ printf("mmap_gpio_adc.c failed to initialize adc\n"); return -1; } printf("Initializing eQEP\n"); if(init_eqep(0)){ printf("mmap_pwmss.c failed to initialize eQEP\n"); return -1; } if(init_eqep(1)){ printf("mmap_pwmss.c failed to initialize eQEP\n"); return -1; } if(init_eqep(2)){ printf("mmap_pwmss.c failed to initialize eQEP\n"); return -1; } // setup pwm driver printf("Initializing PWM\n"); if(simple_init_pwm(1,PWM_FREQ)){ printf("simple_pwm.c failed to initialize PWMSS 0\n"); return -1; } if(simple_init_pwm(2,PWM_FREQ)){ printf("simple_pwm.c failed to initialize PWMSS 1\n"); return -1; } // start some gpio pins at defaults deselect_spi1_slave(1); deselect_spi1_slave(2); disable_motors(); //set up function pointers for button press events printf("Initializing button interrupts\n"); initialize_button_handlers(); // Load binary into PRU printf("Initializing PRU\n"); if(initialize_pru()){ printf("ERROR: PRU init FAILED\n"); return -1; } // Start Signal Handler printf("Initializing exit signal handler\n"); signal(SIGINT, ctrl_c); signal(SIGTERM, ctrl_c); // Print current battery voltage printf("Battery Voltage: %2.2fV\n", get_battery_voltage()); // all done set_state(PAUSED); printf("\nRobotics Cape Initialized\n"); return 0; }
/****************************************************************** * int cleanup_cape() * shuts down library and hardware functions cleanly * you should call this before your main() function returns *******************************************************************/ int cleanup_cape(){ set_state(EXITING); // allow up to 3 seconds for thread cleanup struct timespec thread_timeout; clock_gettime(CLOCK_REALTIME, &thread_timeout); thread_timeout.tv_sec += 3; int thread_err = 0; thread_err = pthread_timedjoin_np(pause_pressed_thread, NULL, &thread_timeout); if(thread_err == ETIMEDOUT){ printf("WARNING: pause_pressed_thread exit timeout\n"); } thread_err = 0; thread_err = pthread_timedjoin_np(pause_unpressed_thread, NULL, &thread_timeout); if(thread_err == ETIMEDOUT){ printf("WARNING: pause_unpressed_thread exit timeout\n"); } thread_err = 0; thread_err = pthread_timedjoin_np(mode_pressed_thread, NULL, &thread_timeout); if(thread_err == ETIMEDOUT){ printf("WARNING: mode_pressed_thread exit timeout\n"); } thread_err = 0; thread_err = pthread_timedjoin_np(mode_unpressed_thread, NULL, &thread_timeout); if(thread_err == ETIMEDOUT){ printf("WARNING: mode_unpressed_thread exit timeout\n"); } thread_err = 0; thread_err = pthread_timedjoin_np(imu_interrupt_thread, NULL, &thread_timeout); if(thread_err == ETIMEDOUT){ printf("WARNING: imu_interrupt_thread exit timeout\n"); } #ifdef DEBUG printf("turning off GPIOs & PWM\n"); #endif set_led(GREEN,LOW); set_led(RED,LOW); disable_motors(); deselect_spi1_slave(1); deselect_spi1_slave(2); disable_servo_power_rail(); #ifdef DEBUG printf("turning off PRU\n"); #endif prussdrv_pru_disable(0); prussdrv_pru_disable(1); prussdrv_exit(); #ifdef DEBUG printf("deleting PID file\n"); #endif FILE* fd; // clean up the pid_file if it still exists fd = fopen(PID_FILE, "r"); if (fd != NULL) { // close and delete the old file fclose(fd); remove(PID_FILE); } printf("\nExiting Cleanly\n"); return 0; }