Skip to content

Commit

Permalink
initial init
Browse files Browse the repository at this point in the history
  • Loading branch information
@mohie34
mohie34 committed Jan 7, 2020
0 parents commit de77eae
Show file tree
Hide file tree
Showing 52 changed files with 6,057 additions and 0 deletions.
24 changes: 24 additions & 0 deletions Makefile
Original file line number Diff line number Diff line change
@@ -0,0 +1,24 @@
all:
@make -C xbee_serial --no-print-directory
@make -C test_xbee_receive --no-print-directory
@make -C measure_moments --no-print-directory
@make -C measure_motors --no-print-directory
@make -C test_motors --no-print-directory
@make -C balancebot --no-print-directory


opti:
@make -C xbee_serial --no-print-directory
@make -C test_xbee_receive --no-print-directory
@make -C optitrack/common --no-print-directory
@make -C optitrack --no-print-directory

clean:
@make -C balancebot -s clean
@make -C measure_motors -s clean
@make -C measure_moments -s clean
@make -C test_motors -s clean
@make -C xbee_serial -s clean
@make -C test_xbee_receive -s clean
@make -C optitrack -s clean
@make -C optitrack/common -s clean
15 changes: 15 additions & 0 deletions README.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,15 @@
/*******************************************************************************
* Balancebot Template Code
* pgaskell@umich.edu
*
*******************************************************************************/

bin/ : Binaries folder
balanceebot/balanceebot.c/.h : Main setup and threads
test_motors/test_motors.c/.h : Program to test motor implementation
common/mb_controller.c/.h : Contoller for manual and autonomous nav
common/mb_defs.h : Define hardware config
common/mb_motors.c/.h : Motor functions to be used by balancebot
common/mb_odometry.c/.h : Odometry functions
optitrack/ : optitrack driver/server
xbee_serial : xbee serial optitrack client code
66 changes: 66 additions & 0 deletions balancebot/Makefile
Original file line number Diff line number Diff line change
@@ -0,0 +1,66 @@
# this target will be compiled
TARGET = ../bin/balancebot

CC := gcc
LINKER := gcc -o
CFLAGS := -std=gnu99 -c -Wall -g
LFLAGS := -lm -lrt -lpthread -l:librobotcontrol.so.1

SOURCES := $(wildcard *.c)
SOURCES := $(SOURCES) $(wildcard ../common/*.c)
SOURCES := $(SOURCES) $(wildcard ../xbee_serial/*.c)
INCLUDES := $(wildcard *.h)
INCLUDES := $(INCLUDES) $(wildcard ../common/*.h)
INCLUDES := $(INCLUDES) $(wildcard ../xbee_serial/*.h)
OBJECTS := $(SOURCES:$%.c=$%.o)

prefix := /usr/local
RM := rm -f
INSTALL := install -m 4755
INSTALLDIR := install -d -m 755

LINK := ln -s -f
LINKDIR := /etc/roboticscape
LINKNAME := link_to_startup_program


# linking Objects
$(TARGET): $(OBJECTS)
@$(LINKER) $@ $(OBJECTS) $(LFLAGS)
@echo "Made: $@"


# compiling command
$(OBJECTS): %.o : %.c $(INCLUDES)
@$(CC) $(CFLAGS) -c $< -o $(@)
@echo "Compiled: "$<

all:
$(TARGET)

debug:
$(MAKE) $(MAKEFILE) DEBUGFLAG="-g -D DEBUG"
@echo " "
@echo "$(TARGET) Make Debug Complete"
@echo " "

install:
@$(MAKE) --no-print-directory
@$(INSTALLDIR) $(DESTDIR)$(prefix)/bin
@$(INSTALL) $(TARGET) $(DESTDIR)$(prefix)/bin
@echo "$(TARGET) Install Complete"

clean:
@$(RM) $(OBJECTS)
@$(RM) $(TARGET)
@echo "$(TARGET) Clean Complete"

uninstall:
@$(RM) $(DESTDIR)$(prefix)/bin/$(TARGET)
@echo "$(TARGET) Uninstall Complete"

runonboot:
@$(MAKE) install --no-print-directory
@$(LINK) $(DESTDIR)$(prefix)/bin/$(TARGET) $(LINKDIR)/$(LINKNAME)
@echo "$(TARGET) Set to Run on Boot"

276 changes: 276 additions & 0 deletions balancebot/balancebot.c
Original file line number Diff line number Diff line change
@@ -0,0 +1,276 @@
/*******************************************************************************
* balancebot.c
*
* Main template code for the BalanceBot Project
* based on rc_balance
*
*******************************************************************************/

#include <math.h>
#include <rc/start_stop.h>
#include <rc/adc.h>
#include <rc/servo.h>
#include <rc/mpu.h>
#include <rc/dsm.h>
#include <rc/cpu.h>
#include <rc/bmp.h>
#include <rc/button.h>
#include <rc/led.h>
#include <rc/pthread.h>
#include <rc/encoder_eqep.h>
#include <rc/time.h>


#include "balancebot.h"

/*******************************************************************************
* int main()
*
*******************************************************************************/
int main(){
// 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
if(rc_enable_signal_handler()==-1){
fprintf(stderr,"ERROR: failed to start signal handler\n");
return -1;
}

if(rc_cpu_set_governor(RC_GOV_PERFORMANCE)<0){
fprintf(stderr,"Failed to set governor to PERFORMANCE\n");
return -1;
}

// initialize enocders
if(rc_encoder_eqep_init()==-1){
fprintf(stderr,"ERROR: failed to initialize eqep encoders\n");
return -1;
}

// initialize adc
if(rc_adc_init()==-1){
fprintf(stderr, "ERROR: failed to initialize adc\n");
return -1;
}

if(rc_dsm_init()==-1){
fprintf(stderr,"failed to start initialize DSM\n");
return -1;
}

printf("initializing xbee... \n");
//initalize XBee Radio
int baudrate = BAUDRATE;
if(XBEE_init(baudrate)==-1){
fprintf(stderr,"Error initializing XBee\n");
return -1;
};

// 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.
rc_make_pid_file();

// start printf_thread if running from a terminal
// if it was started as a background process then don't bother
printf("starting print thread... \n");
pthread_t printf_thread;
rc_pthread_create(&printf_thread, printf_loop, (void*) NULL, SCHED_OTHER, 0);

// start control thread
printf("starting setpoint thread... \n");
pthread_t setpoint_control_thread;
rc_pthread_create(&setpoint_control_thread, setpoint_control_loop, (void*) NULL, SCHED_FIFO, 50);


// TODO: start motion capture message recieve thread

// set up IMU configuration
printf("initializing imu... \n");
// 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_Z_UP;

// now set up the imu for dmp interrupt operation
if(rc_mpu_initialize_dmp(&mpu_data, mpu_config)){
printf("rc_mpu_initialize_failed\n");
return -1;
}

//rc_nanosleep(5E9); // wait for imu to stabilize

//initialize state mutex
pthread_mutex_init(&state_mutex, NULL);
pthread_mutex_init(&setpoint_mutex, NULL);

//attach controller function to IMU interrupt
printf("initializing controller...\n");
mb_controller_init();

printf("initializing motors...\n");
mb_motor_init();

printf("resetting encoders...\n");
rc_encoder_eqep_write(1, 0);
rc_encoder_eqep_write(2, 0);

printf("initializing odometry...\n");
mb_odometry_init(&mb_odometry, 0.0,0.0,0.0);

printf("attaching imu interupt...\n");
rc_mpu_set_dmp_callback(&balancebot_controller);

printf("we are running!!!...\n");
// done initializing so set state to RUNNING
rc_set_state(RUNNING);

// Keep looping until state changes to EXITING
while(rc_get_state()!=EXITING){

// all the balancing is handled in the imu interupt function
// other functions are handled in other threads
// there is no need to do anything here but sleep
// always sleep at some point
rc_nanosleep(1E9);
}

// exit cleanly
rc_mpu_power_off();
mb_motor_cleanup();
rc_led_cleanup();
rc_encoder_eqep_cleanup();
rc_remove_pid_file(); // remove pid file LAST
return 0;
}


/*******************************************************************************
* void balancebot_controller()
*
* discrete-time balance controller operated off IMU interrupt
* Called at SAMPLE_RATE_HZ
*
* TODO: You must implement this function to keep the balancebot balanced
*
*
*******************************************************************************/
void balancebot_controller(){

//lock state mutex
pthread_mutex_lock(&state_mutex);
// Read IMU
mb_state.theta = mpu_data.dmp_TaitBryan[TB_PITCH_X];
// Read encoders
mb_state.left_encoder = rc_encoder_eqep_read(1);
mb_state.right_encoder = rc_encoder_eqep_read(2);
// Update odometry


// Calculate controller outputs

if(!mb_setpoints.manual_ctl){
//send motor commands
}

if(mb_setpoints.manual_ctl){
//send motor commands
}

XBEE_getData();
double q_array[4] = {xbeeMsg.qw, xbeeMsg.qx, xbeeMsg.qy, xbeeMsg.qz};
double tb_array[3] = {0, 0, 0};
rc_quaternion_to_tb_array(q_array, tb_array);
mb_state.opti_x = xbeeMsg.x;
mb_state.opti_y = -xbeeMsg.y; //xBee quaternion is in Z-down, need Z-up
mb_state.opti_roll = tb_array[0];
mb_state.opti_pitch = -tb_array[1]; //xBee quaternion is in Z-down, need Z-up
mb_state.opti_yaw = -tb_array[2]; //xBee quaternion is in Z-down, need Z-up


//unlock state mutex
pthread_mutex_unlock(&state_mutex);

}


/*******************************************************************************
* setpoint_control_loop()
*
* sets current setpoints based on dsm radio data, odometry, and Optitrak
*
*
*******************************************************************************/
void* setpoint_control_loop(void* ptr){

while(1){

if(rc_dsm_is_new_data()){
// TODO: Handle the DSM data from the Spektrum radio reciever
// You may should implement switching between manual and autonomous mode
// using channel 5 of the DSM data.
}
rc_nanosleep(1E9 / RC_CTL_HZ);
}
return NULL;
}




/*******************************************************************************
* printf_loop()
*
* prints diagnostics to console
* this only gets started if executing from terminal
*
* TODO: Add other data to help you tune/debug your code
*******************************************************************************/
void* printf_loop(void* ptr){
rc_state_t last_state, new_state; // keep track of last state
while(rc_get_state()!=EXITING){
new_state = rc_get_state();
// check if this is the first time since being paused
if(new_state==RUNNING && last_state!=RUNNING){
printf("\nRUNNING: Hold upright to balance.\n");
printf(" SENSORS | MOCAP |");
printf("\n");
printf(" θ |");
printf(" φ |");
printf(" L Enc |");
printf(" R Enc |");
printf(" X |");
printf(" Y |");
printf(" ψ |");

printf("\n");
}
else if(new_state==PAUSED && last_state!=PAUSED){
printf("\nPAUSED\n");
}
last_state = new_state;

if(new_state == RUNNING){
printf("\r");
//Add Print stattements here, do not follow with /n
pthread_mutex_lock(&state_mutex);
printf("%7.3f |", mb_state.theta);
printf("%7.3f |", mb_state.phi);
printf("%7d |", mb_state.left_encoder);
printf("%7d |", mb_state.right_encoder);
printf("%7.3f |", mb_state.opti_x);
printf("%7.3f |", mb_state.opti_y);
printf("%7.3f |", mb_state.opti_yaw);
pthread_mutex_unlock(&state_mutex);
fflush(stdout);
}
rc_nanosleep(1E9/PRINTF_HZ);
}
return NULL;
}
Loading

0 comments on commit de77eae

Please sign in to comment.