Beagle Board - beagleboard.org
Saad Yousaf
Published

eduMIP DSTR Controller

Take the eduMIP and hook it up with the DSTR app in order to control the Mobile Inverted Pendulum from your very own cell phone!

IntermediateWork in progress6 hours521
eduMIP DSTR Controller

Things used in this project

Hardware components

eduMIP
Renaissance Robotics eduMIP
×1
BeagleBone Blue
BeagleBoard.org BeagleBone Blue
×1
Robotics Cape
Renaissance Robotics Robotics Cape
×1
USB-A to Micro-USB Cable
USB-A to Micro-USB Cable
×1

Software apps and online services

DSTR

Story

Read more

Schematics

BeagleBone Blue source

Schematics, layout and documentation for a community-supported WiFi/BT-enabled Linux controller board.

Code

rc_balance.c

C/C++
This version of rc_balance.c includes the relevant code to interface with the DSTR.py file's output.
/**
* @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 <roboticscape.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,
	DSTR,
	DSM,
	STDIN
} m_input_mode_t;


void balance_controller();		///< mpu interrupt routine
void* setpoint_manager(void* ptr);	///< background thread
void* battery_checker(void* ptr);	///< background thread
void* printf_loop(void* ptr);		///< background thread
int zero_out_controller();
int disarm_controller();
int arm_controller();
int wait_for_starting_condition();
void on_pause_press();
void on_mode_release();
int blink_green();
int blink_red();

// global variables
core_state_t cstate;
setpoint_t setpoint;
rc_filter_t D1, D2, D3;
rc_mpu_data_t mpu_data;
m_input_mode_t m_input_mode = DSM;

/*
 * printed if some invalid argument was given
 */
void print_usage(){
	printf("\n");
	printf("-i {dsm|dstr|stdin|none}     specify input\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;

	// parse arguments
	opterr = 0;
	while ((c = getopt(argc, argv, "i:")) != -1){
		switch (c){
		case 'i': // input option
			if(!strcmp("dsm", optarg)) {
				m_input_mode = DSM;
			} else if(!strcmp("dstr", optarg)) {
				m_input_mode = DSTR;
			} 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 'h':
			print_usage();
			return -1;
			break;
		default:
			print_usage();
			return -1;
			break;
		}
	}

	// make sure another instance isn't running
	// return value -3 means a root process is running and we need more
	// privileges to stop it.
	if(rc_kill_existing_process(2.0)==-3) return -1;

	// start signal handler so we can exit cleanly
	if(rc_enable_signal_handler()==-1){
		fprintf(stderr,"ERROR: failed to start signal handler\n");
		return -1;
	}

	// initialize buttons
	if(rc_button_init(RC_BTN_PIN_PAUSE, RC_BTN_POLARITY_NORM_HIGH,
						RC_BTN_DEBOUNCE_DEFAULT_US)){
		fprintf(stderr,"ERROR: failed to initialize pause button\n");
		return -1;
	}
	if(rc_button_init(RC_BTN_PIN_MODE, RC_BTN_POLARITY_NORM_HIGH,
						RC_BTN_DEBOUNCE_DEFAULT_US)){
		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
	if(rc_encoder_eqep_init()==-1){
		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");
	}

	// make PID file to indicate project is running
	rc_make_pid_file();

	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
	rc_mpu_config_t mpu_config = rc_mpu_default_config();
	mpu_config.dmp_sample_rate = SAMPLE_RATE_HZ;
	mpu_config.orient = ORIENTATION_Y_UP;

	// if gyro isn't calibrated, run the calibration routine
	if(!rc_mpu_is_gyro_calibrated()){
		printf("Gyro not calibrated, automatically starting calibration routine\n");
		printf("Let your MiP sit still on a firm surface\n");
		rc_mpu_calibrate_gyro_routine(mpu_config);
	}

	// make sure setpoint starts at normal values
	setpoint.arm_state = DISARMED;
	setpoint.drive_mode = NOVICE;

	D1=rc_filter_empty();
	D2=rc_filter_empty();
	D3=rc_filter_empty();

	// 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");
	rc_filter_print(D1);
	printf("\nOuter Loop controller D2:\n");
	rc_filter_print(D2);

	// 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(rc_pthread_create(&battery_thread, battery_checker, (void*) NULL, SCHED_OTHER, 0)){
		fprintf(stderr, "failed to start battery thread\n");
		return -1;
	}
	// wait for the battery thread to make the first read
	while(cstate.vBatt==0 && rc_get_state()!=EXITING) rc_usleep(1000);

	// start printf_thread if running from a terminal
	// if it was started as a background process then don't bother
	if(isatty(fileno(stdout))){
		if(rc_pthread_create(&printf_thread, printf_loop, (void*) NULL, SCHED_OTHER, 0)){
			fprintf(stderr, "failed to start battery 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");
		rc_led_blink(RC_LED_RED, 5, 5);
		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 battery 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");
	rc_set_state(RUNNING);

	// chill until something exits the program
	rc_set_state(RUNNING);
	while(rc_get_state()!=EXITING){
		rc_usleep(200000);
	}

	// join threads
	rc_pthread_timed_join(setpoint_thread, NULL, 1.5);
	rc_pthread_timed_join(battery_thread, NULL, 1.5);
	rc_pthread_timed_join(printf_thread, NULL, 1.5);

	// cleanup
	rc_filter_free(&D1);
	rc_filter_free(&D2);
	rc_filter_free(&D3);
	rc_mpu_power_off();
	rc_led_set(RC_LED_GREEN, 0);
	rc_led_set(RC_LED_RED, 0);
	rc_led_cleanup();
	rc_encoder_eqep_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);
	rc_set_state(RUNNING);
	rc_led_set(RC_LED_RED,0);
	rc_led_set(RC_LED_GREEN,1);

	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 DSTR
			FILE *file;
			if ((file = fopen("/var/lib/cloud9/BBBlue_DSTR/up.txt.", "r")))
		    {
		        remove(file);
		        setpoint.phi_dot = 2;
		    }
		    if ((file = fopen("/var/lib/cloud9/BBBlue_DSTR/down.txt.", "r")))
		    {
		        remove(file);
		        setpoint.phi_dot = -2;
		    }
		    if ((file = fopen("/var/lib/cloud9/BBBlue_DSTR/right.txt.", "r")))
		    {
		        remove(file);
		        setpoint.gamma_dot = 2;
		    }
		    if ((file = fopen("/var/lib/cloud9/BBBlue_DSTR/left.txt.", "r")))
		    {
		        remove(file);
		        setpoint.gamma_dot = 2;
		    }
			continue;
		case DSM:
			if(rc_dsm_is_new_data()){
				// 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
			else if(rc_dsm_is_connection_active()==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){
						if(chan == DSM_TURN_CH){
							turn_stick = strtof(in_str, NULL)* DSM_TURN_POL;
							setpoint.phi_dot = drive_stick;
						}
						else if(chan == DSM_TURN_CH){
							drive_stick = strtof(in_str, NULL)* DSM_DRIVE_POL;
							setpoint.gamma_dot = turn_stick;
						}
					}
					if(ch == 'n') i = 1;
					else i = 0;
				}
				else if(i == 1){
					chan = ch - 0x30;
					i = 2;
				}
				else{
					in_str[i-2] = ch;
				}
			}

			// 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
 */
void balance_controller()
{
	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] + CAPE_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
	***************************************************************/
	if(rc_get_state()==EXITING){
		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(setpoint.phi_dot != 0.0) 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 }
 */
int zero_out_controller()
{
	rc_filter_reset(&D1);
	rc_filter_reset(&D2);
	rc_filter_reset(&D3);
	setpoint.theta = 0.0;
	setpoint.phi   = 0.0;
	setpoint.gamma = 0.0;
	rc_motor_set(0,0.0f);
	return 0;
}

/**
 * disable motors & set the setpoint.core_mode to DISARMED
 *
 * @return     { description_of_the_return_value }
 */
int disarm_controller()
{
	rc_motor_standby(1);
	rc_motor_free_spin(0);
	setpoint.arm_state = DISARMED;
	return 0;
}

/**
 * zero out the controller & encoders. Enable motors & arm the controller.
 *
 * @return     0 on success, -1 on failure
 */
int arm_controller()
{
	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);
	rc_motor_standby(0);
	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.
 */
int wait_for_starting_condition()
{
	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
 */
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
 */
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
 */
void on_pause_press()
{
	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:
		rc_set_state(PAUSED);
		disarm_controller();
		rc_led_set(RC_LED_RED,1);
		rc_led_set(RC_LED_GREEN,0);
		break;
	case PAUSED:
		rc_set_state(RUNNING);
		disarm_controller();
		rc_led_set(RC_LED_GREEN,1);
		rc_led_set(RC_LED_RED,0);
		break;
	default:
		break;
	}

	// now wait to see if the user want to shut down the program
	while(i<samples){
		rc_usleep(us_wait/samples);
		if(rc_button_get_state(RC_BTN_PIN_PAUSE)==RC_BTN_STATE_RELEASED){
			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
	rc_led_blink(RC_LED_RED,5,2);
	rc_set_state(EXITING);
	return;
}

/**
 * toggle between position and angle modes if MiP is paused
 */
void on_mode_release()
{
	// 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");
	}

	rc_led_blink(RC_LED_GREEN,5,1);
	return;
}

DSTR.py

Python
This modification of DSTR.py creates the appropriate outputs to control the eduMIP through rc_balance.c.
#!/usr/bin/env python3
# import python libraries
import time
import socket

# import rcpy library
# This automatically initizalizes the robotics cape
import rcpy
import rcpy.gpio as gpio
import rcpy.led as led

# welcome message
#print("Hello BeagleBone!")
#print("Press Ctrl-C to exit")

UDP_IP = "192.168.1.1"
UDP_PORT = 3553 #port that the DSTR app listens to
bufferSize = 512
#  ALWAYS check these they could be flipped 
#depending on the hardware setup
led.red.on()
# set state to rcpy.RUNNING
rcpy.set_state(rcpy.RUNNING)

try:
    # Create a UDP/IP socket
    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    while True:
        try:    
            sock.bind((UDP_IP, UDP_PORT))
            led.red.off()
            led.green.on()
            break
        except:
            pass
    sock.settimeout(.25)
    # keep running forever
    while True:
        # running?
        if rcpy.get_state() == rcpy.RUNNING:
            try:    
                data, addr = sock.recvfrom(1024) # buffer size is 1024 bytes
            except socket.timeout:
                data = 0
                continue
            
            if int(data[0])==170 and int(data[2]) == 170 and data[1]<50 and data[3]<50:
                f = open("up.txt","w+")
                print("up")
            if int(data[0])==187 and int(data[2]) == 187 and data[1]<50 and data[3]<50:
                f = open("down.txt","w+")
                # print("down")
            if int(data[0])==170 and int(data[2]) == 187 and data[1]<50 and data[3]<50:
                f = open("right.txt","w+")
                print("right")
            if int(data[0])==187 and int(data[2]) == 170 and data[1]<50 and data[3]<50:
                f = open("left.txt","w+")
                # print("left")
                
            pass
            
    
        # paused?
        elif rcpy.get_state() == rcpy.PAUSED:
            # do nothing
            pass

except KeyboardInterrupt:
    # Catch Ctrl-C
    pass
        
finally:
    # say bye
    print("\nBye Beaglebone!")
            
# exiting program will automatically clean up cape

Credits

Saad Yousaf

Saad Yousaf

1 project • 0 followers
Contact
Thanks to Erik Welsh, Strawson Design, and Mitch Martinez, Austin Carter, Matt Seago.

Comments

Add projectSign up / Login