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