main.cpp

00001 //Balance Bot V4
00002 //Author: Carter Sharer
00003 //Date: 10/13/2016
00004 //ESE519 Lab 6 Part 3 Skeleton Code
00005 
00006 /******************************* README USAGE *******************************
00007 * This robot must be powered on while it is laying down flat on a still table 
00008 * This allows the robot to calibrate the IMU (~5 seconds)
00009 * The motors are DISABLED when the robot tilts more then +-45 degrees from
00010 * vertical.  To ENABLE the motors you must lift the robot to < +- 45 degres and
00011 * press the joystick button.  
00012 * To reset the motor positions you must press the josystick button anytime.
00013 ******************************************************************************/
00014 
00015 //Balance Bot Begin
00016 #include "pin_assignments.h"
00017 #include "I2Cdev.h"
00018 #include "JJ_MPU6050_DMP_6Axis.h"
00019 #include "balance_bot.h"
00020 #include "balance_bot_IMU.h"
00021 #include "stepper_motors.h"
00022 #include "MRF24J40.h"
00023 
00024 //Angle Offset is used to set the natural balance point of your robot.  
00025 //You should adjust this offset so that your robots balance point is near 0 
00026 #define ANGLE_OFFSET 107
00027 
00028 //For RF Communication
00029 #define JSTICK_H 8
00030 #define JSTICK_V 9
00031 #define SPACE 10
00032 #define KNOB1 11
00033 #define KNOB2 12
00034 #define KNOB3 13
00035 #define KNOB4 14
00036 #define ANGLE 15
00037 #define BUTTON 16
00038 #define JSTICK_OFFSET 100
00039 #define TX_BUFFER_LEN 18
00040 #define TX_ANGLE_OFFSET 100
00041 //Knobs
00042 #define POT1 p17
00043 #define POT2 p18
00044 #define POT3 p16
00045 #define POT4 p15
00046 //JoyStick
00047 #define POTV p19
00048 #define POTH p20
00049 
00050 //PID
00051 #define MAX_THROTTLE 100
00052 #define MAX_TARGET_ANGLE 12
00053 //PID Default control values from constant definitions
00054 float Kp1;
00055 float Kd1;
00056 float Kp2;
00057 float Kd2;
00058 
00059 //Controller Values
00060 uint8_t knob1, knob2, knob3, knob4;
00061 int8_t jstick_h, jstick_v;
00062 
00063 //Control Variables
00064 float target_angle;
00065 float throttle = 0; //From joystick
00066 float steering = 0; //From joystick
00067 float max_target_angle = MAX_TARGET_ANGLE;
00068 int robot_pos = 0; //Robots position
00069 bool fallen = true;
00070 
00071 Timer timer;
00072 int timer_value; 
00073 int timer_old; 
00074 int dt;
00075 
00076 //Loop Counters 
00077 uint8_t slow_loop_counter;
00078 uint8_t medium_loop_counter;
00079 uint8_t loop_counter;
00080 
00081 Serial pc(USBTX, USBRX);
00082 
00083 // LEDs
00084 DigitalOut led1(LED1);
00085 DigitalOut led2(LED2);
00086 DigitalOut led3(LED3);
00087 DigitalOut led4(LED4);
00088 
00089 //Button
00090 bool button;
00091 #include "communication.h"
00092 
00093 // ================================================================
00094 // ===                      INITIAL SETUP                       ===
00095 // ================================================================
00096 void init_imu()
00097 {
00098     pc.printf("\r\r\n\n Start \r\n");
00099 
00100     // Manual MPU initialization... accel=2G, gyro=2000º/s, filter=20Hz BW, output=200Hz
00101     mpu.setClockSource(MPU6050_CLOCK_PLL_ZGYRO);
00102     mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
00103     mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
00104     mpu.setDLPFMode(MPU6050_DLPF_BW_10);  //10,20,42,98,188  // Default factor for BROBOT:10
00105     mpu.setRate(4);   // 0=1khz 1=500hz, 2=333hz, 3=250hz [4=200hz]default
00106     mpu.setSleepEnabled(false);
00107     wait_ms(500);
00108 
00109     // load and configure the DMP
00110     devStatus = mpu.dmpInitialize();
00111     if(devStatus == 0) {
00112         mpu.setDMPEnabled(true);
00113         mpuIntStatus = mpu.getIntStatus();
00114         dmpReady = true;
00115     } else {
00116         // 1 = initial memory load failed
00117         // 2 = DMP configuration updates failed
00118         pc.printf("DMP INIT error \r\n");
00119     }
00120 
00121     //Gyro Calibration
00122     wait_ms(500);
00123     pc.printf("Gyro calibration!!  Dont move the robot in 10 seconds... \r\n");
00124     wait_ms(500);
00125 
00126     // verify connection
00127     pc.printf(mpu.testConnection() ? "Connection Good \r\n" : "Connection Failed\r\n");
00128 
00129     //Adjust Sensor Fusion Gain
00130     dmpSetSensorFusionAccelGain(0x20);
00131 
00132     wait_ms(200);
00133     mpu.resetFIFO();
00134 }
00135 
00136 // ================================================================
00137 // ===                    MAIN PROGRAM LOOP                     ===
00138 // ================================================================
00139 int main()
00140 {
00141     //Set the Channel. 0 is default, 15 is max
00142     uint8_t channel = 2;
00143     mrf.SetChannel(channel);
00144 
00145     pc.baud(115200);
00146     pc.printf("Start\r\n");
00147     init_imu();
00148     timer.start();
00149     //timer
00150     timer_value = timer.read_us();
00151 
00152     //Init Stepper Motors
00153     //Attach Timer Interupts (Tiker)
00154     timer_M1.attach_us(&ISR1, ZERO_SPEED);
00155     timer_M2.attach_us(&ISR2, ZERO_SPEED);
00156     step_M1 = 1;
00157     dir_M1 = 1;
00158     enable = DISABLE; //Disable Motors
00159 
00160     //Attach Interupt for IMU
00161     checkpin.rise(&dmpDataReady);
00162 
00163     //Used to set angle upon startup, filter
00164     bool FILTER_DISABLE = true;
00165 
00166     //Enable Motors
00167     enable = ENABLE;
00168 
00169     while(1) {
00170         //Led 4 to indicate if robot it STANDING or FALLEN
00171         led4 = !fallen;
00172 
00173         //Led 2 to indicate a button press
00174         led2 = button;
00175 
00176         //If button is pressed reset motor position
00177         if(button) {
00178             pos_M1 = 0; //Reset position of Motor 1
00179             pos_M2 = 0; //Reset position of motor 2
00180             fallen = false; //Reset fallen flag
00181         }
00182 
00183         //This is the main while loop, all your code goes here
00184         while(!mpuInterrupt) {
00185             //Timer
00186             timer_value = timer.read_us();
00187 
00188             //Set gainz with knobs
00189             Kp1 = knob1;
00190             Kd1 = knob2;
00191             Kp2 = knob3;
00192             Kd2 = knob4;
00193 
00194             //Joystick control
00195             throttle = jstick_v;
00196             steering = jstick_h;
00197 
00198             /**** Update Values DO NOT MODIFY ********/
00199             loop_counter++;
00200             slow_loop_counter++;
00201             medium_loop_counter++;
00202             dt = (timer_value - timer_old);
00203             timer_old = timer_value;
00204             angle_old = angle;
00205             /*****************************************/
00206 
00207             //STANDING: Motor Control Enabled
00208             if(((angle < 45) && (angle > -45)) && (fallen == false)) {
00209 
00210                 //Enable Motor
00211                 enable = ENABLE;
00212                 
00213                 /* This is where you want to impliment your controlers 
00214                 * Start off with a simple P controller.
00215                 *
00216                 * float error = angle - 0; //should be balanced at 0
00217                 * motor1 = error * Kp; 
00218                 * motor2 = error * Kp;        */
00219                 
00220                 //Calculate motor inputs
00221                 motor1 = int16_t(throttle/2 + steering/8);
00222                 motor2 = int16_t(throttle/2 - steering/8);
00223                 
00224                 //Cap the max and min values [-100, 100]
00225                 motor1 = CAP(motor1, MAX_CONTROL_OUTPUT);
00226                 motor2 = CAP(motor2, MAX_CONTROL_OUTPUT);
00227                 
00228                 //Set Motor Speed here
00229                 setMotor1Speed(motor1);
00230                 setMotor2Speed(motor2);
00231 
00232             } else { //FALLEN: Motor Control Disabled
00233                 //Disable Motors
00234                 enable = DISABLE;
00235 
00236                 //Set fallen flag
00237                 fallen = true;
00238             }
00239     
00240             /* Here are some loops that trigger at different intervals, this 
00241             * will allow you to do things at a slower rate, thus saving speed
00242             * it is important to keep this fast so we dont miss IMU readings */
00243             
00244             //Fast Loop: Good for printing to serial monitor
00245             if(loop_counter >= 5) {
00246                 loop_counter = 0;
00247                 pc.printf("angle:%d Kp1:%0.3f Kd1:%0.2f Kp2:%0.2f Kd2:%0.3f pos_M1:%d pos_M2:%d \r\n", (int)angle, Kp1, Kd1, Kp2, Kd2, pos_M1, pos_M2, robot_pos);
00248             }
00249 
00250             //Meduim Loop: Good for sending and receiving 
00251             if (medium_loop_counter >= 10) {
00252                 medium_loop_counter = 0; // Read  status
00253 
00254                 //Recieve Data
00255                 rxLen = rf_receive(rxBuffer, 128);
00256                 if(rxLen > 0) {
00257                     led1 = led1^1;
00258                     //Process data with our protocal
00259                     communication_protocal(rxLen);
00260                 }
00261 
00262             }  // End of medium loop
00263 
00264             //Slow Loop: Good for sending if speed is not an issue 
00265             if(slow_loop_counter >= 99) {
00266                 slow_loop_counter = 0;
00267 
00268                 /* Send Data To Controller goes here *
00269                  *                                   */
00270             } //End of Slow Loop
00271 
00272             //Reattach interupt
00273             checkpin.rise(&dmpDataReady);
00274         } //END WHILE
00275 
00276 
00277         /********************* All IMU Handling DO NOT MODIFY *****************/
00278         //Disable IRQ
00279         checkpin.rise(NULL);
00280 
00281         //reset interrupt flag and get INT_STATUS byte
00282         mpuInterrupt = false;
00283         mpuIntStatus = mpu.getIntStatus();
00284 
00285         //get current FIFO count
00286         fifoCount = mpu.getFIFOCount();
00287 
00288         // check for overflow (this should never happen unless our code is too inefficient)
00289         if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
00290             // reset so we can continue cleanly
00291             mpu.resetFIFO();
00292             pc.printf("FIFO overflow!");
00293 
00294             //otherwise, check for DMP data ready interrupt (this should happen frequently)
00295         } else if (mpuIntStatus & 0x02) {
00296             //wait for correct available data length, should be a VERY short wait
00297             while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
00298 
00299             //read a packet from FIFO
00300             mpu.getFIFOBytes(fifoBuffer, packetSize);
00301 
00302             //track FIFO count here in case there is > 1 packet available
00303             //(this lets us immediately read more without waiting for an interrupt)
00304             fifoCount -= packetSize;
00305 
00306             //Read new angle from IMU
00307             new_angle = (float)(dmpGetPhi() - ANGLE_OFFSET);
00308             dAngle = new_angle - angle;
00309 
00310             //Filter out angle readings larger then MAX_ANGLE_DELTA
00311             if( ((dAngle < 15) && (dAngle > -15)) || FILTER_DISABLE) {
00312                 angle = new_angle;
00313                 FILTER_DISABLE = false; //turn of filter disabler
00314             } 
00315         }
00316         /********************* All IMU Handling DO NOT MODIFY *****************/
00317 
00318     } //end main loop
00319 } //End Main()