Diff: main.cpp

Revision:

6:62cdb7482b50

Parent:

4:2512939c10f0

Child:

8:8389c0a9339e

--- a/main.cpp	Tue Oct 18 20:46:01 2016 +0000
+++ b/main.cpp	Sat Dec 17 22:46:59 2016 +0000
@@ -1,6 +1,7 @@
 //BroBot V3
 //Author: Carter Sharer
 //Date: 10/13/2016
+//Added communication protocol v1 (no type selection)
 
 //BroBot Begin
 #include "pin_assignments.h"
@@ -11,6 +12,10 @@
 #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 points is near 0
+#define ANGLE_OFFSET 107
+
 //For RF Communication
 #define JSTICK_H 8
 #define JSTICK_V 9
@@ -44,35 +49,16 @@
 #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;
-
 //Controller Values
-uint8_t knob1, knob2, knob3, knob4;
-int8_t jstick_h, jstick_v;
-
+float knob1, knob2, knob3, knob4;
+float 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 Kp1 = KP;
+float Kd1 = KD;
+float Kp2 = KP_THROTTLE;
+float Ki2 = KI_THROTTLE;
+float Kd2; //Added for CS Pos contorl
 float PID_errorSum;
 float PID_errorOld = 0;
 float PID_errorOld2 = 0;
@@ -98,7 +84,6 @@
 uint8_t medium_loop_counter;
 uint8_t loop_counter;
 
-
 Serial pc(USBTX, USBRX);
 
 // LEDs
@@ -109,48 +94,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                       ===
 // ================================================================
@@ -215,9 +160,14 @@
 float change_in_pos;
 float robot_pos_old, robot_pos_old1, robot_pos_old2;
 
+bool fallen = true;
 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 +180,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,24 +188,34 @@
     //Used to set angle upon startup, filter
     bool FILTER_DISABLE = true;
 
+    //Enable Motors 
+    enable = ENABLE;
+    
     while(1) {
-
+        //led1 = led1^1;
+        led4 = !fallen;
+        led2 = button;
+        
+        if(jstick_v > 80)
+            led3 = 1;
+        else
+            led3 = 0;
+        
         if(button) {
             pos_M1 = 0;
             pos_M2 = 0;
             target_pos = 0;
+            fallen = false;
         }
 
-        while(!mpuInterrupt) { // && fifoCount < packetSize) {
-            //led4 = led4^1;
-            //pc.printf("In while comp loop \r\n");
+        while(!mpuInterrupt) { 
             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;
+            Kp1 = ((float)knob1) / 1000.0;
+            Kd1 = ((float)knob2) / 1.0;
+            Kp2 = ((float)knob3) / 1000.0;
+            Kd2 = ((float)knob4) / 100.0;
 
             //Joystick control
             throttle = (float)jstick_v  /10.0;
@@ -273,40 +229,25 @@
             timer_old = timer_value;
             angle_old = angle;
 
-            // Motor contorl
-            if((angle < 45) && (angle > -45)) {
+            // STANDING: Motor Control Enabled 
+            if(((angle < 45) && (angle > -45)) && (fallen == false)) {
 
-                //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.
-
-                //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);
+                //CS Pd Target Angle Contoller Goes Here
+                
+                //Robot Position
+                robot_pos = (pos_M1 + pos_M2) / 2;
+                target_pos += throttle/2;
                 
-                //CS Pd Target Angle Contoller Goes Here
-                target_pos += throttle;
-                robot_pos = (pos_M1 + pos_M2) / 2;
-                //KP Term
+                //Position error
                 pos_error = robot_pos - target_pos; //robot_pos - target_pos;
-                kp_pos_term = -Kp_thr * pos_error;
+                
+                //KP Term  
+                kp_pos_term = -Kp2 * 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;
+                kd_pos_term = ((-Kd2 * change_in_target_pos) + (-Kd2*change_in_pos)) /dt;
                 target_angle = kp_pos_term + kd_pos_term;
                 target_angle = CAP(target_angle, MAX_TARGET_ANGLE);
                 
@@ -317,17 +258,19 @@
                 
                 //CS PD Stability CONTROLLER HERE
                 error = target_angle - angle;
-                kp_term = Kp * error;
+                kp_term = Kp1 * 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;
-
+                kd_term = ((Kd1 * change_in_target_angle) - Kd1*(change_in_angle)) / dt;
+    
+                //pc.printf("dAngle:%f\r\n", angle-angle_old1);
+    
                 //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));
+                motor1 = (int16_t)(control_output + (steering));
+                motor2 = (int16_t)(control_output - (steering));
                 motor1 = CAP(motor1, MAX_CONTROL_OUTPUT);
                 motor2 = CAP(motor2, MAX_CONTROL_OUTPUT);
 
@@ -342,72 +285,58 @@
                 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}
                 //Disable Motors
                 enable = DISABLE;
-                //Set Motor Speed 0
-                PID_errorSum = 0;  // Reset PID I term
+                
+                //Set fallen flag
+                fallen = true;
             }
 
-            //Fast Loop
+            //Fast Loop 
             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  tang: %0.2f dt:%d pos_M1:%d pos_M2:%d rob_pos: %d\r\n", (int)angle, Kp1, Kd1, Kp2, Ki2, target_angle, dt, pos_M1, pos_M2, robot_pos);
+                //pc.printf("Jstick_h: %d Jstick_v: %d Knob1 %d Knob2 %d Knob3 %d Knob4 %d Button: %d\r\n", jstick_h, jstick_v, knob1, knob2, knob3, knob4, button);
             }
 
-
             //Meduim Loop
             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
             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,23 +345,22 @@
             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;
@@ -440,8 +368,8 @@
             } 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