Diff: main.cpp

Revision:

6:ae3e6aefe908

Parent:

4:2512939c10f0

Child:

9:a8fd0bd49279

--- a/main.cpp	Tue Oct 18 20:46:01 2016 +0000
+++ b/main.cpp	Thu Nov 10 19:20:55 2016 +0000
@@ -1,16 +1,30 @@
-//BroBot V3
+//Balance Bot V4
 //Author: Carter Sharer
 //Date: 10/13/2016
+//ESE519 Lab 6 Part 3 Skeleton Code
 
-//BroBot Begin
+/******************************* README USAGE *******************************
+* 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.  
+* To reset the motor positions you must press the josystick button anytime.
+******************************************************************************/
+
+//Balance Bot Begin
 #include "pin_assignments.h"
 #include "I2Cdev.h"
 #include "JJ_MPU6050_DMP_6Axis.h"
-#include "BroBot.h"
-#include "BroBot_IMU.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
@@ -34,71 +48,36 @@
 #define POTH p20
 
 //PID
-#define MAX_THROTTLE 580
-#define MAX_STEERING 150
+#define MAX_THROTTLE 100
 #define MAX_TARGET_ANGLE 12
-#define KP 0.19
-#define KD 28
-#define KP_THROTTLE 0.01 //0.07    
-#define KI_THROTTLE 0//0.04
-#define ITERM_MAX_ERROR 25   // Iterm windup constants for PI control //40
-#define ITERM_MAX 8000       // 5000
-
-//MRF24J40
-PinName mosi(SDI); //SDI
-PinName miso(SDO); //SDO
-PinName sck(SCK);  //SCK
-PinName cs(CS); //CS
-PinName reset(RESET); //RESET
-// RF tranceiver to link with handheld.
-MRF24J40 mrf(mosi, miso, sck, cs, reset);
-uint8_t txBuffer[128]= {1, 8, 0, 0xA1, 0xB2, 0xC3, 0xD4, 0x00};
-uint8_t rxBuffer[128];
-uint8_t rxLen;
+//PID Default control values from constant definitions
+float Kp1;
+float Kd1;
+float Kp2;
+float Kd2;
 
 //Controller Values
 uint8_t knob1, knob2, knob3, knob4;
 int8_t jstick_h, jstick_v;
 
-
-//PID Default control values from constant definitions
-float Kp = KP;
-float Kd = KD;
-float Kp_thr = KP_THROTTLE;
-float Ki_thr = KI_THROTTLE;
-float Kd_thr; //Added for CS Pos contorl
-float Kp_user = KP;
-float Kd_user = KD;
-float Kp_thr_user = KP_THROTTLE;
-float Ki_thr_user = KI_THROTTLE;
-bool newControlParameters = false;
-bool modifing_control_parameters = false;
-float PID_errorSum;
-float PID_errorOld = 0;
-float PID_errorOld2 = 0;
-float setPointOld = 0;
+//Control Variables
 float target_angle;
-float throttle = 0;
-float steering = 0;
-float max_throttle = MAX_THROTTLE;
-float max_steering = MAX_STEERING;
+float throttle = 0; //From joystick
+float steering = 0; //From joystick
 float max_target_angle = MAX_TARGET_ANGLE;
-float control_output;
-int16_t actual_robot_speed;        // overall robot speed (measured from steppers speed)
-int16_t actual_robot_speed_old;
-float estimated_speed_filtered;    // Estimated robot speed
-int robot_pos = 0;
+int robot_pos = 0; //Robots position
+bool fallen = true;
 
 Timer timer;
-int timer_value; //maybe make this a long
-int timer_old; //maybe make this a long
+int timer_value; 
+int timer_old; 
 int dt;
 
+//Loop Counters 
 uint8_t slow_loop_counter;
 uint8_t medium_loop_counter;
 uint8_t loop_counter;
 
-
 Serial pc(USBTX, USBRX);
 
 // LEDs
@@ -109,48 +88,8 @@
 
 //Button
 bool button;
-
-// =============================================================================
-// ===      PD controller implementation(Proportional, derivative)           ===
-// =============================================================================
-// PD controller implementation(Proportional, derivative). DT is in miliseconds
-//    stabilityPDControl(      dt,       angle,   target_angle,        Kp,       Kd);
-float stabilityPDControl(float DT, float input, float setPoint,  float Kp, float Kd)
-{
-    float error;
-    float output;
-
-    error = setPoint - input;
-
-
-    // Kd is implemented in two parts
-    //    The biggest one using only the input (sensor) part not the SetPoint input-input(t-2)
-    //    And the second using the setpoint to make it a bit more agressive   setPoint-setPoint(t-1)
-    output = Kp * error; //+ (Kd * (setPoint - setPointOld) - Kd * (input - PID_errorOld2)) / DT;
+#include "communication.h"
 
-    PID_errorOld2 = PID_errorOld;
-    PID_errorOld = input;  // error for Kd is only the input component
-    setPointOld = setPoint;
-    return output;
-
-
-}
-
-// PI controller implementation (Proportional, integral). DT is in miliseconds
-float speedPIControl(float DT, float input, float setPoint,  float Kp, float Ki)
-{
-    float error;
-    float output;
-
-    error = setPoint - input;
-    PID_errorSum += CAP(error, ITERM_MAX_ERROR);
-    PID_errorSum = CAP(PID_errorSum, ITERM_MAX);
-
-    //Serial.println(PID_errorSum);
-
-    output = Kp * error + Ki * PID_errorSum * DT * 0.001; // DT is in miliseconds...
-    return (output);
-}
 // ================================================================
 // ===                      INITIAL SETUP                       ===
 // ================================================================
@@ -197,27 +136,13 @@
 // ================================================================
 // ===                    MAIN PROGRAM LOOP                     ===
 // ================================================================
-//CS PID CONTROLLER TEST
-float target_angle_old = 0;
-float change_in_target_angle = 0;
-float change_in_angle = 0;
-float angle_old1 = 0;
-float angle_old2 = 0;
-float kp_term = 0;
-float kd_term = 0;
-float error;
-//For Position controller
-float pos_error = 0;
-float kp_pos_term = 0;
-float kd_pos_term = 0;
-float change_in_target_pos;
-float target_pos, target_pos_old;
-float change_in_pos;
-float robot_pos_old, robot_pos_old1, robot_pos_old2;
-
 int main()
 {
-    pc.baud(230400);
+    //Set the Channel. 0 is default, 15 is max
+    uint8_t channel = 2;
+    mrf.SetChannel(channel);
+
+    pc.baud(115200);
     pc.printf("Start\r\n");
     init_imu();
     timer.start();
@@ -230,11 +155,7 @@
     timer_M2.attach_us(&ISR2, ZERO_SPEED);
     step_M1 = 1;
     dir_M1 = 1;
-    enable = ENABLE; //Enable Motors
-
-    //Set Gains
-    Kp_thr = 0; //0.15;
-    Ki_thr = 0; //0.15;
+    enable = DISABLE; //Disable Motors
 
     //Attach Interupt for IMU
     checkpin.rise(&dmpDataReady);
@@ -242,172 +163,126 @@
     //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 2 to indicate a button press
+        led2 = button;
+
+        //If button is pressed reset motor position
         if(button) {
-            pos_M1 = 0;
-            pos_M2 = 0;
-            target_pos = 0;
+            pos_M1 = 0; //Reset position of Motor 1
+            pos_M2 = 0; //Reset position of motor 2
+            fallen = false; //Reset fallen flag
         }
 
-        while(!mpuInterrupt) { // && fifoCount < packetSize) {
-            //led4 = led4^1;
-            //pc.printf("In while comp loop \r\n");
+        //This is the main while loop, all your code goes here
+        while(!mpuInterrupt) {
+            //Timer
             timer_value = timer.read_us();
 
-            //Set Gainz with knobs
-            Kp = ((float)knob1) / 1000.0;
-            Kd = ((float)knob2) / 1.0;
-            Kp_thr = ((float)knob3) / 1000.0;
-            Kd_thr = ((float)knob4) / 100.0;
+            //Set gainz with knobs
+            Kp1 = knob1;
+            Kd1 = knob2;
+            Kp2 = knob3;
+            Kd2 = knob4;
 
             //Joystick control
-            throttle = (float)jstick_v  /10.0;
-            steering = (float)jstick_h / 10.0;
+            throttle = jstick_v;
+            steering = jstick_h;
 
-            //Update Values
+            /**** 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;
-
-            // Motor contorl
-            if((angle < 45) && (angle > -45)) {
+            /*****************************************/
 
-                //PID CONTROL MAGIC GOES HERE
-                // We calculate the estimated robot speed:
-                // Estimated_Speed = angular_velocity_of_stepper_motors(combined) - angular_velocity_of_robot(angle measured by IMU)
-                actual_robot_speed_old = actual_robot_speed;
-                actual_robot_speed = (speed_M1 + speed_M2) / 2; // Positive: forward
-                int16_t angular_velocity = (angle - angle_old) * 90.0; // 90 is an empirical extracted factor to adjust for real units
-                int16_t estimated_speed = -actual_robot_speed_old - angular_velocity;     // We use robot_speed(t-1) or (t-2) to compensate the delay
-                estimated_speed_filtered = estimated_speed_filtered * 0.95 + (float)estimated_speed * 0.05;  // low pass filter on estimated speed
-                // SPEED CONTROL: This is a PI controller.
-                //    input:user throttle, variable: estimated robot speed, output: target robot angle to get the desired speed
-                //CS target_angle = speedPIControl(dt, estimated_speed_filtered, throttle, Kp_thr, Ki_thr);
-                //CS target_angle = CAP(target_angle, max_target_angle); // limited output
-                //target_angle = 0;
-                // Stability control: This is a PD controller.
-                //    input: robot target angle(from SPEED CONTROL), variable: robot angle, output: Motor speed
-                //    We integrate the output (sumatory), so the output is really the motor acceleration, not motor speed.
+            //STANDING: Motor Control Enabled
+            if(((angle < 45) && (angle > -45)) && (fallen == false)) {
 
-                //pc.printf("dt: %f, angle: %f, target_angle: %f, Kp: %f, Kd: %f \r\n", dt, angle, target_angle, Kp, Kd);
-                //control_output = stabilityPDControl(dt, angle, target_angle, Kp, Kd);
+                //Enable Motor
+                enable = ENABLE;
                 
-                //CS Pd Target Angle Contoller Goes Here
-                target_pos += throttle;
-                robot_pos = (pos_M1 + pos_M2) / 2;
-                //KP Term
-                pos_error = robot_pos - target_pos; //robot_pos - target_pos;
-                kp_pos_term = -Kp_thr * pos_error;
-                
-                //KD Term
-                change_in_target_pos = target_pos - target_pos_old;
-                change_in_pos = robot_pos - robot_pos_old2;
-                kd_pos_term = ((-Kd_thr * change_in_target_pos) - (-Kd_thr*change_in_pos)) /dt;
-                target_angle = kp_pos_term + kd_pos_term;
-                target_angle = CAP(target_angle, MAX_TARGET_ANGLE);
+                /* 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;        */
                 
-                //Update values
-                target_pos_old = target_pos;
-                robot_pos_old2 = robot_pos_old1;
-                robot_pos_old1 = robot_pos_old;
+                //Calculate motor inputs
+                motor1 = int16_t(throttle/2 + steering/8);
+                motor2 = int16_t(throttle/2 - steering/8);
                 
-                //CS PD Stability CONTROLLER HERE
-                error = target_angle - angle;
-                kp_term = Kp * error;
-
-                change_in_target_angle = target_angle - target_angle_old; //add
-                change_in_angle = angle - angle_old2; //add
-                kd_term = ((Kd * change_in_target_angle) - Kd*(change_in_angle)) / dt;
-
-                //Control Output
-                control_output += kp_term + kd_term;
-                control_output = CAP(control_output, MAX_CONTROL_OUTPUT); // Limit max output from control
-                motor1 = (int16_t)(control_output + (steering/4));
-                motor2 = (int16_t)(control_output - (steering/4));
+                //Cap the max and min values [-100, 100]
                 motor1 = CAP(motor1, MAX_CONTROL_OUTPUT);
                 motor2 = CAP(motor2, MAX_CONTROL_OUTPUT);
-
-                //Update variables
-                target_angle_old = target_angle;
-                angle_old2 = angle_old1;
-                angle_old1 = angle;
+                
+                //Set Motor Speed here
+                setMotor1Speed(motor1);
+                setMotor2Speed(motor2);
 
-                //Enable Motors
-                enable = ENABLE;
-                setMotor1Speed(-motor1);
-                setMotor2Speed(-motor2);
-                robot_pos += (-motor1 + -motor2) / 2;
-                //pc.printf("m1: %d m2: %d angle: %0.1f, controlout: %f tAngle: %f dt: %f timer: %d \r\n", motor1, motor2, angle, control_output, target_angle, dt, timer_value);
-            } else {
+            } else { //FALLEN: Motor Control Disabled
                 //Disable Motors
                 enable = DISABLE;
-                //Set Motor Speed 0
-                PID_errorSum = 0;  // Reset PID I term
+
+                //Set fallen flag
+                fallen = true;
             }
-
-            //Fast Loop
+    
+            /* 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 horz: %d verti: %d knob1: %d knob2: %d knob3: %d knob4: %d \r\n", int16_t(angle-ANGLE_OFFSET), jstick_h, jstick_v, knob1, knob2, knob3, knob4);
-                //setMotor1Speed(int16_t(angle));
-                //setMotor2Speed(int16_t(angle));
-                //pc.printf("horz: %d verti: %d knob1: %d angle: %d \r\n", jstick_h, jstick_v, knob1, (int)angle);
-                //pc.printf("angle: %d \r\n", (int)angle);
-                pc.printf("angle:%d Kp: %0.3f Kd: %0.2f  Kp_thr: %0.2f Kd_thr: %0.3f  tang: %0.2f dt:%d pos_M1:%d pos_M2:%d rob_pos: %d\r\n", (int)angle, Kp, Kd, Kp_thr, Ki_thr, target_angle, dt, pos_M1, pos_M2, robot_pos);
+                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
+            //Meduim Loop: Good for sending and receiving 
             if (medium_loop_counter >= 10) {
                 medium_loop_counter = 0; // Read  status
-                led2 = led2^1;
 
                 //Recieve Data
-                rxLen = mrf.Receive(rxBuffer, 128);
-                if(rxLen) {
-                    if((rxBuffer[0] == (uint8_t)1) && (rxBuffer[1] == (uint8_t)8) && (rxBuffer[2]==(uint8_t)0)) {
-                        jstick_h = (int8_t)rxBuffer[JSTICK_H] - JSTICK_OFFSET;
-                        jstick_v = (int8_t)rxBuffer[JSTICK_V] - JSTICK_OFFSET;
-                        knob1 = rxBuffer[KNOB1];
-                        knob2 = rxBuffer[KNOB2];
-                        knob3 = rxBuffer[KNOB3];
-                        knob4 = rxBuffer[KNOB4];
-                        button = rxBuffer[BUTTON];
-                        led1= led1^1;  //flash led for debuggin
-                        led4 = button;
-                    }
-                } else {
-                    mrf.Reset();
+                rxLen = rf_receive(rxBuffer, 128);
+                if(rxLen > 0) {
+                    led1 = led1^1;
+                    //Process data with our protocal
+                    communication_protocal(rxLen);
                 }
+
             }  // End of medium loop
-            
-            //Slow Loop
+
+            //Slow Loop: Good for sending if speed is not an issue 
             if(slow_loop_counter >= 99) {
                 slow_loop_counter = 0;
 
-                //Send Data
-                txBuffer[ANGLE] = (uint8_t)(angle + TX_ANGLE_OFFSET);
-                mrf.Send(txBuffer, TX_BUFFER_LEN);
+                /* Send Data To Controller goes here *
+                 *                                   */
             } //End of Slow Loop
 
             //Reattach interupt
             checkpin.rise(&dmpDataReady);
         } //END WHILE
 
+
+        /********************* All IMU Handling DO NOT MODIFY *****************/
         //Disable IRQ
         checkpin.rise(NULL);
-        led3 = led3^1;
-        //pc.printf("taking care of imu stuff angle: %f \r\n", angle);
-        //All IMU stuff
-        // reset interrupt flag and get INT_STATUS byte
+
+        //reset interrupt flag and get INT_STATUS byte
         mpuInterrupt = false;
         mpuIntStatus = mpu.getIntStatus();
 
-        // get current FIFO count
+        //get current FIFO count
         fifoCount = mpu.getFIFOCount();
 
         // check for overflow (this should never happen unless our code is too inefficient)
@@ -416,32 +291,29 @@
             mpu.resetFIFO();
             pc.printf("FIFO overflow!");
 
-            // otherwise, check for DMP data ready interrupt (this should happen frequently)
+            //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
+            //wait for correct available data length, should be a VERY short wait
             while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
 
-            // read a packet from FIFO
+            //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)
+            //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
-            } else {
-                pc.printf("\t\t\t filtered angle \r\n");
-            }
-            //END IMU STUFF
+            } 
+        }
+        /********************* All IMU Handling DO NOT MODIFY *****************/
 
-        }
     } //end main loop
 } //End Main()
\ No newline at end of file