Robot Control Library
rc_balance.c
/**
* @example rc_balance
*
* Reference solution for balancing EduMiP
**/
#include <stdio.h>
#include <unistd.h> // for isatty()
#include <stdlib.h> // for strtof()
#include <math.h> // for M_PI
#include <robotcontrol.h>
#include "rc_balance_defs.h"
/**
* NOVICE: Drive rate and turn rate are limited to make driving easier.
* ADVANCED: Faster drive and turn rate for more fun.
*/
typedef enum drive_mode_t{
NOVICE,
ADVANCED
}drive_mode_t;
/**
* ARMED or DISARMED to indicate if the controller is running
*/
typedef enum arm_state_t{
ARMED,
DISARMED
}arm_state_t;
/**
* Feedback controller setpoint written to by setpoint_manager and read by the
* controller.
*/
typedef struct setpoint_t{
arm_state_t arm_state; ///< see arm_state_t declaration
drive_mode_t drive_mode;///< NOVICE or ADVANCED
double theta; ///< body lean angle (rad)
double phi; ///< wheel position (rad)
double phi_dot; ///< rate at which phi reference updates (rad/s)
double gamma; ///< body turn angle (rad)
double gamma_dot; ///< rate at which gamma setpoint updates (rad/s)
}setpoint_t;
/**
* This is the system state written to by the balance controller.
*/
typedef struct core_state_t{
double wheelAngleR; ///< wheel rotation relative to body
double wheelAngleL;
double theta; ///< body angle radians
double phi; ///< average wheel angle in global frame
double gamma; ///< body turn (yaw) angle radians
double vBatt; ///< battery voltage
double d1_u; ///< output of balance controller D1 to motors
double d2_u; ///< output of position controller D2 (theta_ref)
double d3_u; ///< output of steering controller D3 to motors
double mot_drive; ///< u compensated for battery voltage
} core_state_t;
// possible modes, user selected with command line arguments
typedef enum m_input_mode_t{
NONE,
DSM,
STDIN
} m_input_mode_t;
static void __print_usage(void);
static void __balance_controller(void); ///< mpu interrupt routine
static void* __setpoint_manager(void* ptr); ///< background thread
static void* __battery_checker(void* ptr); ///< background thread
static void* __printf_loop(void* ptr); ///< background thread
static int __zero_out_controller(void);
static int __disarm_controller(void);
static int __arm_controller(void);
static int __wait_for_starting_condition(void);
static void __on_pause_press(void);
static void __on_mode_release(void);
// global variables
// Must be declared static so they get zero initalized
static core_state_t cstate;
static setpoint_t setpoint;
static rc_mpu_data_t mpu_data;
static m_input_mode_t m_input_mode = DSM;
/*
* printed if some invalid argument was given
*/
static void __print_usage(void)
{
printf("\n");
printf("-i {dsm|stdin|none} specify input\n");
printf("-q Don't print diagnostic info\n");
printf("-h print this help message\n");
printf("\n");
}
/**
* Initialize the filters, mpu, threads, & wait until shut down
*
* @return 0 on success, -1 on failure
*/
int main(int argc, char *argv[])
{
int c;
pthread_t setpoint_thread = 0;
pthread_t battery_thread = 0;
pthread_t printf_thread = 0;
bool adc_ok = true;
bool quiet = false;
// parse arguments
opterr = 0;
while ((c = getopt(argc, argv, "i:qh")) != -1){
switch (c){
case 'i': // input option
if(!strcmp("dsm", optarg)) {
m_input_mode = DSM;
} else if(!strcmp("stdin", optarg)) {
m_input_mode = STDIN;
} else if(!strcmp("none", optarg)){
m_input_mode = NONE;
} else {
__print_usage();
return -1;
}
break;
case 'q':
quiet = true;
break;
case 'h':
__print_usage();
return -1;
break;
default:
__print_usage();
return -1;
break;
}
}
// make sure another instance isn't running
// if return value is -3 then a background process is running with
// higher privaledges and we couldn't kill it, in which case we should
// not continue or there may be hardware conflicts. If it returned -4
// then there was an invalid argument that needs to be fixed.
if(rc_kill_existing_process(2.0)<-2) return -1;
// start signal handler so we can exit cleanly
fprintf(stderr,"ERROR: failed to start signal handler\n");
return -1;
}
// initialize buttons
fprintf(stderr,"ERROR: failed to initialize pause button\n");
return -1;
}
fprintf(stderr,"ERROR: failed to initialize mode button\n");
return -1;
}
// Assign functions to be called when button events occur
rc_button_set_callbacks(RC_BTN_PIN_PAUSE,__on_pause_press,NULL);
rc_button_set_callbacks(RC_BTN_PIN_MODE,NULL,__on_mode_release);
// initialize enocders
fprintf(stderr,"ERROR: failed to initialize eqep encoders\n");
return -1;
}
// initialize motors
if(rc_motor_init()==-1){
fprintf(stderr,"ERROR: failed to initialize motors\n");
return -1;
}
rc_motor_standby(1); // start with motors in standby
// start dsm listener
if(m_input_mode == DSM){
if(rc_dsm_init()==-1){
fprintf(stderr,"failed to start initialize DSM\n");
return -1;
}
}
// initialize adc
if(rc_adc_init()==-1){
fprintf(stderr, "failed to initialize adc\n");
adc_ok = false;
}
// make PID file to indicate your project is running
// due to the check made on the call to rc_kill_existing_process() above
// we can be fairly confident there is no PID file already and we can
// make our own safely.
printf("\nPress and release MODE button to toggle DSM drive mode\n");
printf("Press and release PAUSE button to pause/start the motors\n");
printf("hold pause button down for 2 seconds to exit\n");
if(rc_led_set(RC_LED_GREEN, 0)==-1){
fprintf(stderr, "ERROR in rc_balance, failed to set RC_LED_GREEN\n");
return -1;
}
if(rc_led_set(RC_LED_RED, 1)==-1){
fprintf(stderr, "ERROR in rc_balance, failed to set RC_LED_RED\n");
return -1;
}
// set up mpu configuration
mpu_config.dmp_sample_rate = SAMPLE_RATE_HZ;
mpu_config.orient = ORIENTATION_Y_UP;
// if gyro isn't calibrated, run the calibration routine
printf("Gyro not calibrated, automatically starting calibration routine\n");
printf("Let your MiP sit still on a firm surface\n");
}
// make sure setpoint starts at normal values
setpoint.arm_state = DISARMED;
setpoint.drive_mode = NOVICE;
// set up D1 Theta controller
double D1_num[] = D1_NUM;
double D1_den[] = D1_DEN;
if(rc_filter_alloc_from_arrays(&D1, DT, D1_num, D1_NUM_LEN, D1_den, D1_DEN_LEN)){
fprintf(stderr,"ERROR in rc_balance, failed to make filter D1\n");
return -1;
}
D1.gain = D1_GAIN;
rc_filter_enable_saturation(&D1, -1.0, 1.0);
rc_filter_enable_soft_start(&D1, SOFT_START_SEC);
// set up D2 Phi controller
double D2_num[] = D2_NUM;
double D2_den[] = D2_DEN;
if(rc_filter_alloc_from_arrays(&D2, DT, D2_num, D2_NUM_LEN, D2_den, D2_DEN_LEN)){
fprintf(stderr,"ERROR in rc_balance, failed to make filter D2\n");
return -1;
}
D2.gain = D2_GAIN;
rc_filter_enable_saturation(&D2, -THETA_REF_MAX, THETA_REF_MAX);
rc_filter_enable_soft_start(&D2, SOFT_START_SEC);
printf("Inner Loop controller D1:\n");
printf("\nOuter Loop controller D2:\n");
// set up D3 gamma (steering) controller
if(rc_filter_pid(&D3, D3_KP, D3_KI, D3_KD, 4*DT, DT)){
fprintf(stderr,"ERROR in rc_balance, failed to make steering controller\n");
return -1;
}
rc_filter_enable_saturation(&D3, -STEERING_INPUT_MAX, STEERING_INPUT_MAX);
// start a thread to slowly sample battery
if (adc_ok) {
if(rc_pthread_create(&battery_thread, __battery_checker, (void*) NULL, SCHED_OTHER, 0)){
fprintf(stderr, "failed to start battery thread\n");
return -1;
}
}
else { // If we can't get the battery voltage
cstate.vBatt = V_NOMINAL; // Set to a nominal value
}
// wait for the battery thread to make the first read
while(cstate.vBatt<1.0 && rc_get_state()!=EXITING) rc_usleep(10000);
// start printf_thread if running from a terminal
// if it was started as a background process then don't bother
if(isatty(fileno(stdout)) && (quiet == false)){
if(rc_pthread_create(&printf_thread, __printf_loop, (void*) NULL, SCHED_OTHER, 0)){
fprintf(stderr, "failed to start printf thread\n");
return -1;
}
}
// start mpu
if(rc_mpu_initialize_dmp(&mpu_data, mpu_config)){
fprintf(stderr,"ERROR: can't talk to IMU, all hope is lost\n");
return -1;
}
// start balance stack to control setpoints
if(rc_pthread_create(&setpoint_thread, __setpoint_manager, (void*) NULL, SCHED_OTHER, 0)){
fprintf(stderr, "failed to start setpoint thread\n");
return -1;
}
// this should be the last step in initialization
// to make sure other setup functions don't interfere
rc_mpu_set_dmp_callback(&__balance_controller);
// start in the RUNNING state, pressing the pause button will swap to
// the PAUSED state then back again.
printf("\nHold your MIP upright to begin balancing\n");
// chill until something exits the program
while(rc_get_state()!=EXITING){
rc_usleep(200000);
}
// join threads
rc_pthread_timed_join(setpoint_thread, NULL, 1.5);
if (battery_thread) rc_pthread_timed_join(battery_thread, NULL, 1.5);
if (printf_thread) rc_pthread_timed_join(printf_thread, NULL, 1.5);
// cleanup
rc_button_cleanup(); // stop button handlers
rc_remove_pid_file(); // remove pid file LAST
return 0;
}
/**
* This thread is in charge of adjusting the controller setpoint based on user
* inputs from dsm radio control. Also detects pickup to control arming the
* controller.
*
* @param ptr The pointer
*
* @return { description_of_the_return_value }
*/
void* __setpoint_manager(__attribute__ ((unused)) void* ptr)
{
double drive_stick, turn_stick; // input sticks
int i, ch, chan, stdin_timeout = 0; // for stdin input
char in_str[11];
// wait for mpu to settle
__disarm_controller();
rc_usleep(2500000);
while(rc_get_state()!=EXITING){
// clear out input of old data before waiting for new data
if(m_input_mode == STDIN) fseek(stdin,0,SEEK_END);
// sleep at beginning of loop so we can use the 'continue' statement
rc_usleep(1000000/SETPOINT_MANAGER_HZ);
// nothing to do if paused, go back to beginning of loop
if(rc_get_state() != RUNNING || m_input_mode == NONE) continue;
// if we got here the state is RUNNING, but controller is not
// necessarily armed. If DISARMED, wait for the user to pick MIP up
// which will we detected by wait_for_starting_condition()
if(setpoint.arm_state == DISARMED){
if(__wait_for_starting_condition()==0){
__zero_out_controller();
__arm_controller();
}
else continue;
}
// if dsm is active, update the setpoint rates
switch(m_input_mode){
case NONE:
continue;
case DSM:
// Read normalized (+-1) inputs from RC radio stick and multiply by
// polarity setting so positive stick means positive setpoint
turn_stick = rc_dsm_ch_normalized(DSM_TURN_CH) * DSM_TURN_POL;
drive_stick = rc_dsm_ch_normalized(DSM_DRIVE_CH)* DSM_DRIVE_POL;
// saturate the inputs to avoid possible erratic behavior
rc_saturate_double(&drive_stick,-1,1);
rc_saturate_double(&turn_stick,-1,1);
// use a small deadzone to prevent slow drifts in position
if(fabs(drive_stick)<DSM_DEAD_ZONE) drive_stick = 0.0;
if(fabs(turn_stick)<DSM_DEAD_ZONE) turn_stick = 0.0;
// translate normalized user input to real setpoint values
switch(setpoint.drive_mode){
case NOVICE:
setpoint.phi_dot = DRIVE_RATE_NOVICE * drive_stick;
setpoint.gamma_dot = TURN_RATE_NOVICE * turn_stick;
break;
case ADVANCED:
setpoint.phi_dot = DRIVE_RATE_ADVANCED * drive_stick;
setpoint.gamma_dot = TURN_RATE_ADVANCED * turn_stick;
break;
default: break;
}
}
// if dsm had timed out, put setpoint rates back to 0
setpoint.theta = 0;
setpoint.phi_dot = 0;
setpoint.gamma_dot = 0;
continue;
}
break;
case STDIN:
i = 0;
while ((ch = getchar()) != EOF && i < 10){
stdin_timeout = 0;
if(ch == 'n' || ch == '\n'){
if(i > 2){
in_str[i-2] = '\0'; // Null terminate string in case this command is shorter than last one
if(chan == DSM_TURN_CH){
turn_stick = strtof(in_str, NULL) * DSM_TURN_POL;
setpoint.gamma_dot = turn_stick;
}
else if(chan == DSM_DRIVE_CH){
drive_stick = strtof(in_str, NULL) * DSM_DRIVE_POL;
setpoint.phi_dot = drive_stick;
}
}
if(ch == 'n') i = 1;
else i = 0;
}
else if(i == 1){
chan = ch - 0x30;
i = 2;
}
else{
in_str[i-2] = ch;
i++;
}
}
// if it has been more than 1 second since getting data
if(stdin_timeout >= SETPOINT_MANAGER_HZ){
setpoint.theta = 0;
setpoint.phi_dot = 0;
setpoint.gamma_dot = 0;
}
else{
stdin_timeout++;
}
continue;
break;
default:
fprintf(stderr,"ERROR in setpoint manager, invalid input mode\n");
break;
}
}
// if state becomes EXITING the above loop exists and we disarm here
__disarm_controller();
return NULL;
}
/**
* discrete-time balance controller operated off mpu interrupt Called at
* SAMPLE_RATE_HZ
*/
static void __balance_controller(void)
{
static int inner_saturation_counter = 0;
double dutyL, dutyR;
/******************************************************************
* STATE_ESTIMATION
* read sensors and compute the state when either ARMED or DISARMED
******************************************************************/
// angle theta is positive in the direction of forward tip around X axis
cstate.theta = mpu_data.dmp_TaitBryan[TB_PITCH_X] + BOARD_MOUNT_ANGLE;
// collect encoder positions, right wheel is reversed
cstate.wheelAngleR = (rc_encoder_eqep_read(ENCODER_CHANNEL_R) * 2.0 * M_PI) \
/(ENCODER_POLARITY_R * GEARBOX * ENCODER_RES);
cstate.wheelAngleL = (rc_encoder_eqep_read(ENCODER_CHANNEL_L) * 2.0 * M_PI) \
/(ENCODER_POLARITY_L * GEARBOX * ENCODER_RES);
// Phi is average wheel rotation also add theta body angle to get absolute
// wheel position in global frame since encoders are attached to the body
cstate.phi = ((cstate.wheelAngleL+cstate.wheelAngleR)/2) + cstate.theta;
// steering angle gamma estimate
cstate.gamma = (cstate.wheelAngleR-cstate.wheelAngleL) \
* (WHEEL_RADIUS_M/TRACK_WIDTH_M);
/*************************************************************
* check for various exit conditions AFTER state estimate
***************************************************************/
rc_motor_set(0,0.0);
return;
}
// if controller is still ARMED while state is PAUSED, disarm it
if(rc_get_state()!=RUNNING && setpoint.arm_state==ARMED){
__disarm_controller();
return;
}
// exit if the controller is disarmed
if(setpoint.arm_state==DISARMED){
return;
}
// check for a tipover
if(fabs(cstate.theta) > TIP_ANGLE){
__disarm_controller();
printf("tip detected \n");
return;
}
/************************************************************
* OUTER LOOP PHI controller D2
* Move the position setpoint based on phi_dot.
* Input to the controller is phi error (setpoint-state).
*************************************************************/
if(ENABLE_POSITION_HOLD){
if(fabs(setpoint.phi_dot) > 0.001) setpoint.phi += setpoint.phi_dot*DT;
cstate.d2_u = rc_filter_march(&D2,setpoint.phi-cstate.phi);
setpoint.theta = cstate.d2_u;
}
else setpoint.theta = 0.0;
/************************************************************
* INNER LOOP ANGLE Theta controller D1
* Input to D1 is theta error (setpoint-state). Then scale the
* output u to compensate for changing battery voltage.
*************************************************************/
D1.gain = D1_GAIN * V_NOMINAL/cstate.vBatt;
cstate.d1_u = rc_filter_march(&D1,(setpoint.theta-cstate.theta));
/*************************************************************
* Check if the inner loop saturated. If it saturates for over
* a second disarm the controller to prevent stalling motors.
*************************************************************/
if(fabs(cstate.d1_u)>0.95) inner_saturation_counter++;
else inner_saturation_counter = 0;
// if saturate for a second, disarm for safety
if(inner_saturation_counter > (SAMPLE_RATE_HZ*D1_SATURATION_TIMEOUT)){
printf("inner loop controller saturated\n");
__disarm_controller();
inner_saturation_counter = 0;
return;
}
/**********************************************************
* gama (steering) controller D3
* move the setpoint gamma based on user input like phi
***********************************************************/
if(fabs(setpoint.gamma_dot)>0.0001) setpoint.gamma += setpoint.gamma_dot * DT;
cstate.d3_u = rc_filter_march(&D3,setpoint.gamma - cstate.gamma);
/**********************************************************
* Send signal to motors
* add D1 balance control u and D3 steering control also
* multiply by polarity to make sure direction is correct.
***********************************************************/
dutyL = cstate.d1_u - cstate.d3_u;
dutyR = cstate.d1_u + cstate.d3_u;
rc_motor_set(MOTOR_CHANNEL_L, MOTOR_POLARITY_L * dutyL);
rc_motor_set(MOTOR_CHANNEL_R, MOTOR_POLARITY_R * dutyR);
return;
}
/**
* Clear the controller's memory and zero out setpoints.
*
* @return { description_of_the_return_value }
*/
static int __zero_out_controller(void)
{
setpoint.theta = 0.0;
setpoint.phi = 0.0;
setpoint.gamma = 0.0;
rc_motor_set(0,0.0);
return 0;
}
/**
* disable motors & set the setpoint.core_mode to DISARMED
*
* @return { description_of_the_return_value }
*/
static int __disarm_controller(void)
{
setpoint.arm_state = DISARMED;
return 0;
}
/**
* zero out the controller & encoders. Enable motors & arm the controller.
*
* @return 0 on success, -1 on failure
*/
static int __arm_controller(void)
{
__zero_out_controller();
rc_encoder_eqep_write(ENCODER_CHANNEL_L,0);
rc_encoder_eqep_write(ENCODER_CHANNEL_R,0);
// prefill_filter_inputs(&D1,cstate.theta);
setpoint.arm_state = ARMED;
return 0;
}
/**
* Wait for MiP to be held upright long enough to begin. Returns
*
* @return 0 if successful, -1 if the wait process was interrupted by pause
* button or shutdown signal.
*/
static int __wait_for_starting_condition(void)
{
int checks = 0;
const int check_hz = 20; // check 20 times per second
int checks_needed = round(START_DELAY*check_hz);
int wait_us = 1000000/check_hz;
// wait for MiP to be tipped back or forward first
// exit if state becomes paused or exiting
while(rc_get_state()==RUNNING){
// if within range, start counting
if(fabs(cstate.theta) > START_ANGLE) checks++;
// fell out of range, restart counter
else checks = 0;
// waited long enough, return
if(checks >= checks_needed) break;
rc_usleep(wait_us);
}
// now wait for MiP to be upright
checks = 0;
// exit if state becomes paused or exiting
while(rc_get_state()==RUNNING){
// if within range, start counting
if(fabs(cstate.theta) < START_ANGLE) checks++;
// fell out of range, restart counter
else checks = 0;
// waited long enough, return
if(checks >= checks_needed) return 0;
rc_usleep(wait_us);
}
return -1;
}
/**
* Slow loop checking battery voltage. Also changes the D1 saturation limit
* since that is dependent on the battery voltage.
*
* @return nothing, NULL poitner
*/
static void* __battery_checker(__attribute__ ((unused)) void* ptr)
{
double new_v;
while(rc_get_state()!=EXITING){
new_v = rc_adc_batt();
// if the value doesn't make sense, use nominal voltage
if (new_v>9.0 || new_v<5.0) new_v = V_NOMINAL;
cstate.vBatt = new_v;
rc_usleep(1000000 / BATTERY_CHECK_HZ);
}
return NULL;
}
/**
* prints diagnostics to console this only gets started if executing from
* terminal
*
* @return nothing, NULL pointer
*/
static void* __printf_loop(__attribute__ ((unused)) void* ptr)
{
rc_state_t last_rc_state, new_rc_state; // keep track of last state
last_rc_state = rc_get_state();
while(rc_get_state()!=EXITING){
new_rc_state = rc_get_state();
// check if this is the first time since being paused
if(new_rc_state==RUNNING && last_rc_state!=RUNNING){
printf("\nRUNNING: Hold upright to balance.\n");
printf(" θ |");
printf(" θ_ref |");
printf(" φ |");
printf(" φ_ref |");
printf(" γ |");
printf(" D1_u |");
printf(" D3_u |");
printf(" vBatt |");
printf("arm_state|");
printf("\n");
}
else if(new_rc_state==PAUSED && last_rc_state!=PAUSED){
printf("\nPAUSED: press pause again to start.\n");
}
last_rc_state = new_rc_state;
// decide what to print or exit
if(new_rc_state == RUNNING){
printf("\r");
printf("%7.3f |", cstate.theta);
printf("%7.3f |", setpoint.theta);
printf("%7.3f |", cstate.phi);
printf("%7.3f |", setpoint.phi);
printf("%7.3f |", cstate.gamma);
printf("%7.3f |", cstate.d1_u);
printf("%7.3f |", cstate.d3_u);
printf("%7.3f |", cstate.vBatt);
if(setpoint.arm_state == ARMED) printf(" ARMED |");
else printf("DISARMED |");
fflush(stdout);
}
rc_usleep(1000000 / PRINTF_HZ);
}
return NULL;
}
/**
* Disarm the controller and set system state to paused. If the user holds the
* pause button for 2 seconds, exit cleanly
*/
static void __on_pause_press(void)
{
int i=0;
const int samples = 100; // check for release 100 times in this period
const int us_wait = 2000000; // 2 seconds
switch(rc_get_state()){
// pause if running
case EXITING:
return;
case RUNNING:
__disarm_controller();
break;
case PAUSED:
__disarm_controller();
break;
default:
break;
}
// now wait to see if the user want to shut down the program
while(i<samples){
rc_usleep(us_wait/samples);
return; //user let go before time-out
}
i++;
}
printf("long press detected, shutting down\n");
//user held the button down long enough, blink and exit cleanly
return;
}
/**
* toggle between position and angle modes if MiP is paused
*/
static void __on_mode_release(void)
{
// toggle between position and angle modes
if(setpoint.drive_mode == NOVICE){
setpoint.drive_mode = ADVANCED;
printf("using drive_mode = ADVANCED\n");
}
else {
setpoint.drive_mode = NOVICE;
printf("using drive_mode = NOVICE\n");
}
return;
}