Diff: main.cpp

Revision:

9:a8fd0bd49279

Parent:

6:ae3e6aefe908

Child:

10:4b5f975c21c4

--- a/main.cpp	Fri Nov 11 19:20:41 2016 +0000
+++ b/main.cpp	Thu Mar 30 22:13:44 2017 +0000
@@ -4,27 +4,21 @@
 //ESE519 Lab 6 Part 3 Skeleton Code
 
 /******************************* README USAGE *******************************
-* This robot must be powered on while it is laying down flat on a still table 
+* This robot must be powered on while it is laying down flat on a still table
 * This allows the robot to calibrate the IMU (~5 seconds)
 * The motors are DISABLED when the robot tilts more then +-45 degrees from
 * vertical.  To ENABLE the motors you must lift the robot to < +- 45 degres and
-* press the joystick button.  
+* press the joystick button.
 * To reset the motor positions you must press the josystick button anytime.
 ******************************************************************************/
 
 //Balance Bot Begin
+#include "mbed.h"
 #include "pin_assignments.h"
-#include "I2Cdev.h"
-#include "JJ_MPU6050_DMP_6Axis.h"
 #include "balance_bot.h"
-#include "balance_bot_IMU.h"
 #include "stepper_motors.h"
 #include "MRF24J40.h"
 
-//Angle Offset is used to set the natural balance point of your robot.  
-//You should adjust this offset so that your robots balance point is near 0 
-#define ANGLE_OFFSET 107
-
 //For RF Communication
 #define JSTICK_H 8
 #define JSTICK_V 9
@@ -33,7 +27,6 @@
 #define KNOB2 12
 #define KNOB3 13
 #define KNOB4 14
-#define ANGLE 15
 #define BUTTON 16
 #define JSTICK_OFFSET 100
 #define TX_BUFFER_LEN 18
@@ -61,19 +54,16 @@
 int8_t jstick_h, jstick_v;
 
 //Control Variables
-float target_angle;
 float throttle = 0; //From joystick
 float steering = 0; //From joystick
-float max_target_angle = MAX_TARGET_ANGLE;
 int robot_pos = 0; //Robots position
-bool fallen = true;
 
 Timer timer;
-int timer_value; 
-int timer_old; 
+int timer_value;
+int timer_old;
 int dt;
 
-//Loop Counters 
+//Loop Counters
 uint8_t slow_loop_counter;
 uint8_t medium_loop_counter;
 uint8_t loop_counter;
@@ -93,58 +83,24 @@
 // ================================================================
 // ===                      INITIAL SETUP                       ===
 // ================================================================
-void init_imu()
-{
-    pc.printf("\r\r\n\n Start \r\n");
-
-    // Manual MPU initialization... accel=2G, gyro=2000º/s, filter=20Hz BW, output=200Hz
-    mpu.setClockSource(MPU6050_CLOCK_PLL_ZGYRO);
-    mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
-    mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
-    mpu.setDLPFMode(MPU6050_DLPF_BW_10);  //10,20,42,98,188  // Default factor for BROBOT:10
-    mpu.setRate(4);   // 0=1khz 1=500hz, 2=333hz, 3=250hz [4=200hz]default
-    mpu.setSleepEnabled(false);
-    wait_ms(500);
-
-    // load and configure the DMP
-    devStatus = mpu.dmpInitialize();
-    if(devStatus == 0) {
-        mpu.setDMPEnabled(true);
-        mpuIntStatus = mpu.getIntStatus();
-        dmpReady = true;
-    } else {
-        // 1 = initial memory load failed
-        // 2 = DMP configuration updates failed
-        pc.printf("DMP INIT error \r\n");
-    }
-
-    //Gyro Calibration
-    wait_ms(500);
-    pc.printf("Gyro calibration!!  Dont move the robot in 10 seconds... \r\n");
-    wait_ms(500);
-
-    // verify connection
-    pc.printf(mpu.testConnection() ? "Connection Good \r\n" : "Connection Failed\r\n");
-
-    //Adjust Sensor Fusion Gain
-    dmpSetSensorFusionAccelGain(0x20);
-
-    wait_ms(200);
-    mpu.resetFIFO();
-}
 
 // ================================================================
 // ===                    MAIN PROGRAM LOOP                     ===
 // ================================================================
 int main()
 {
+    //Used to toggle motors on and off
+    bool ROBOT_ON = false;
+
     //Set the Channel. 0 is default, 15 is max
-    uint8_t channel = 2;
+    uint8_t channel = 9;
     mrf.SetChannel(channel);
+    
+    //Used for button press
+    int last_button_time = 0;
 
     pc.baud(115200);
     pc.printf("Start\r\n");
-    init_imu();
     timer.start();
     //timer
     timer_value = timer.read_us();
@@ -157,18 +113,12 @@
     dir_M1 = 1;
     enable = DISABLE; //Disable Motors
 
-    //Attach Interupt for IMU
-    checkpin.rise(&dmpDataReady);
-
-    //Used to set angle upon startup, filter
-    bool FILTER_DISABLE = true;
-
     //Enable Motors
     enable = ENABLE;
 
     while(1) {
-        //Led 4 to indicate if robot it STANDING or FALLEN
-        led4 = !fallen;
+        //Led 4 to indicate if robot it Running or Not
+        led4 = ROBOT_ON;
 
         //Led 2 to indicate a button press
         led2 = button;
@@ -177,143 +127,86 @@
         if(button) {
             pos_M1 = 0; //Reset position of Motor 1
             pos_M2 = 0; //Reset position of motor 2
-            fallen = false; //Reset fallen flag
+            
+            if((timer.read_us() - last_button_time) > 250000) {
+                ROBOT_ON = ROBOT_ON^1;
+                pc.printf("BUTTON WAS PRESSED \r\n");
+                last_button_time = timer.read_us();
+            }
         }
 
-        //This is the main while loop, all your code goes here
-        while(!mpuInterrupt) {
-            //Timer
-            timer_value = timer.read_us();
+
+        //Timer
+        timer_value = timer.read_us();
 
-            //Set gainz with knobs
-            Kp1 = knob1;
-            Kd1 = knob2;
-            Kp2 = knob3;
-            Kd2 = knob4;
+        //Joystick control
+        throttle = jstick_v;
+        steering = jstick_h;
 
-            //Joystick control
-            throttle = jstick_v;
-            steering = jstick_h;
+        /**** Update Values DO NOT MODIFY ********/
+        loop_counter++;
+        slow_loop_counter++;
+        medium_loop_counter++;
+        dt = (timer_value - timer_old);
+        timer_old = timer_value;
+        /*****************************************/
 
-            /**** Update Values DO NOT MODIFY ********/
-            loop_counter++;
-            slow_loop_counter++;
-            medium_loop_counter++;
-            dt = (timer_value - timer_old);
-            timer_old = timer_value;
-            angle_old = angle;
-            /*****************************************/
+        //Running: Motor Control Enabled
+        if(ROBOT_ON) {
+            //Enable Motor
+            enable = ENABLE;
 
-            //STANDING: Motor Control Enabled
-            if(((angle < 45) && (angle > -45)) && (fallen == false)) {
+            //Calculate motor inputs
+            motor1 = int16_t(throttle/2 + steering/8);
+            motor2 = int16_t(throttle/2 - steering/8);
+
+            //Cap the max and min values [-100, 100]
+            motor1 = CAP(motor1, MAX_CONTROL_OUTPUT);
+            motor2 = CAP(motor2, MAX_CONTROL_OUTPUT);
 
-                //Enable Motor
-                enable = ENABLE;
-                
-                /* This is where you want to impliment your controlers 
-                * Start off with a simple P controller.
-                *
-                * float error = angle - 0; //should be balanced at 0
-                * motor1 = error * Kp; 
-                * motor2 = error * Kp;        */
-                
-                //Calculate motor inputs
-                motor1 = int16_t(throttle/2 + steering/8);
-                motor2 = int16_t(throttle/2 - steering/8);
-                
-                //Cap the max and min values [-100, 100]
-                motor1 = CAP(motor1, MAX_CONTROL_OUTPUT);
-                motor2 = CAP(motor2, MAX_CONTROL_OUTPUT);
-                
-                //Set Motor Speed here
-                setMotor1Speed(motor1);
-                setMotor2Speed(motor2);
+            //Set Motor Speed here
+            setMotor1Speed(motor1);
+            setMotor2Speed(motor2);
+
+        } 
+        //Robot is off so disable the motors
+        else {
+            enable = DISABLE;
+        }
+
+        /* Here are some loops that trigger at different intervals, this
+        * will allow you to do things at a slower rate, thus saving speed
+        * it is important to keep this fast so we dont miss IMU readings */
 
-            } else { //FALLEN: Motor Control Disabled
-                //Disable Motors
-                enable = DISABLE;
+        //Fast Loop: Good for printing to serial monitor
+        if(loop_counter >= 5) {
+            loop_counter = 0;
+            pc.printf("ROBOT_ON:%d pos_M1:%d pos_M2:%d robot_pos%d \r\n", ROBOT_ON, pos_M1, pos_M2, robot_pos);
+        }
 
-                //Set fallen flag
-                fallen = true;
-            }
-    
-            /* Here are some loops that trigger at different intervals, this 
-            * will allow you to do things at a slower rate, thus saving speed
-            * it is important to keep this fast so we dont miss IMU readings */
-            
-            //Fast Loop: Good for printing to serial monitor
-            if(loop_counter >= 5) {
-                loop_counter = 0;
-                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);
+        //Meduim Loop: Good for sending and receiving
+        if (medium_loop_counter >= 10) {
+            medium_loop_counter = 0; // Read  status
+
+            //Recieve Data
+            rxLen = rf_receive(rxBuffer, 128);
+            if(rxLen > 0) {
+                led1 = led1^1;
+                //Process data with our protocal
+                communication_protocal(rxLen);
             }
 
-            //Meduim Loop: Good for sending and receiving 
-            if (medium_loop_counter >= 10) {
-                medium_loop_counter = 0; // Read  status
-
-                //Recieve Data
-                rxLen = rf_receive(rxBuffer, 128);
-                if(rxLen > 0) {
-                    led1 = led1^1;
-                    //Process data with our protocal
-                    communication_protocal(rxLen);
-                }
+        }  // End of medium loop
 
-            }  // End of medium loop
-
-            //Slow Loop: Good for sending if speed is not an issue 
-            if(slow_loop_counter >= 99) {
-                slow_loop_counter = 0;
+        //Slow Loop: Good for sending if speed is not an issue
+        if(slow_loop_counter >= 99) {
+            slow_loop_counter = 0;
 
-                /* Send Data To Controller goes here *
-                 *                                   */
-            } //End of Slow Loop
-
-            //Reattach interupt
-            checkpin.rise(&dmpDataReady);
-        } //END WHILE
+            /* Send Data To Controller goes here *
+             *                                   */
+        } //End of Slow Loop
 
 
-        /********************* All IMU Handling DO NOT MODIFY *****************/
-        //Disable IRQ
-        checkpin.rise(NULL);
-
-        //reset interrupt flag and get INT_STATUS byte
-        mpuInterrupt = false;
-        mpuIntStatus = mpu.getIntStatus();
-
-        //get current FIFO count
-        fifoCount = mpu.getFIFOCount();
-
-        // check for overflow (this should never happen unless our code is too inefficient)
-        if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
-            // reset so we can continue cleanly
-            mpu.resetFIFO();
-            pc.printf("FIFO overflow!");
-
-            //otherwise, check for DMP data ready interrupt (this should happen frequently)
-        } else if (mpuIntStatus & 0x02) {
-            //wait for correct available data length, should be a VERY short wait
-            while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
-
-            //read a packet from FIFO
-            mpu.getFIFOBytes(fifoBuffer, packetSize);
-
-            //track FIFO count here in case there is > 1 packet available
-            //(this lets us immediately read more without waiting for an interrupt)
-            fifoCount -= packetSize;
-
-            //Read new angle from IMU
-            new_angle = (float)(dmpGetPhi() - ANGLE_OFFSET);
-            dAngle = new_angle - angle;
-
-            //Filter out angle readings larger then MAX_ANGLE_DELTA
-            if( ((dAngle < 15) && (dAngle > -15)) || FILTER_DISABLE) {
-                angle = new_angle;
-                FILTER_DISABLE = false; //turn of filter disabler
-            } 
-        }
-        /********************* All IMU Handling DO NOT MODIFY *****************/
 
     } //end main loop
 } //End Main()
\ No newline at end of file