Diff: main.cpp

Revision:

4:2512939c10f0

Parent:

3:2f76ffbc5cef

Child:

6:ae3e6aefe908

--- a/main.cpp	Wed Oct 12 05:04:10 2016 +0000
+++ b/main.cpp	Tue Oct 18 20:44:21 2016 +0000
@@ -1,13 +1,37 @@
-//BroBot V2
-//Author: Carter Sharer 
-//Date: 10/11/2016
+//BroBot V3
+//Author: Carter Sharer
+//Date: 10/13/2016
 
+//BroBot Begin
 #include "pin_assignments.h"
 #include "I2Cdev.h"
 #include "JJ_MPU6050_DMP_6Axis.h"
 #include "BroBot.h"
 #include "BroBot_IMU.h"
 #include "stepper_motors.h"
+#include "MRF24J40.h"
+
+//For RF Communication
+#define JSTICK_H 8
+#define JSTICK_V 9
+#define SPACE 10
+#define KNOB1 11
+#define KNOB2 12
+#define KNOB3 13
+#define KNOB4 14
+#define ANGLE 15
+#define BUTTON 16
+#define JSTICK_OFFSET 100
+#define TX_BUFFER_LEN 18
+#define TX_ANGLE_OFFSET 100
+//Knobs
+#define POT1 p17
+#define POT2 p18
+#define POT3 p16
+#define POT4 p15
+//JoyStick
+#define POTV p19
+#define POTH p20
 
 //PID
 #define MAX_THROTTLE 580
@@ -20,11 +44,29 @@
 #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;
+
+
 //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;
@@ -45,17 +87,29 @@
 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;
 
 Timer timer;
 int timer_value; //maybe make this a long
 int timer_old; //maybe make this a long
-float dt;
+int dt;
 
 uint8_t slow_loop_counter;
+uint8_t medium_loop_counter;
 uint8_t loop_counter;
 
+
 Serial pc(USBTX, USBRX);
 
+// LEDs
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+
+//Button
+bool button;
+
 // =============================================================================
 // ===      PD controller implementation(Proportional, derivative)           ===
 // =============================================================================
@@ -68,15 +122,18 @@
 
     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;
-    //Serial.print(Kd*(error-PID_errorOld));Serial.print("\t");
-    //PID_errorOld2 = PID_errorOld;
-    //PID_errorOld = input;  // error for Kd is only the input component
-    //setPointOld = setPoint;
+    output = Kp * error; //+ (Kd * (setPoint - setPointOld) - Kd * (input - PID_errorOld2)) / DT;
+
+    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
@@ -106,7 +163,7 @@
     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
+    mpu.setRate(4);   // 0=1khz 1=500hz, 2=333hz, 3=250hz [4=200hz]default
     mpu.setSleepEnabled(false);
     wait_ms(500);
 
@@ -116,7 +173,7 @@
         mpu.setDMPEnabled(true);
         mpuIntStatus = mpu.getIntStatus();
         dmpReady = true;
-    } else { 
+    } else {
         // 1 = initial memory load failed
         // 2 = DMP configuration updates failed
         pc.printf("DMP INIT error \r\n");
@@ -126,7 +183,7 @@
     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");
 
@@ -140,14 +197,32 @@
 // ================================================================
 // ===                    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(115200);
+    pc.baud(230400);
     pc.printf("Start\r\n");
     init_imu();
     timer.start();
     //timer
-    timer_value = timer.read_ms();
+    timer_value = timer.read_us();
 
     //Init Stepper Motors
     //Attach Timer Interupts (Tiker)
@@ -160,50 +235,174 @@
     //Set Gains
     Kp_thr = 0; //0.15;
     Ki_thr = 0; //0.15;
-    
+
+    //Attach Interupt for IMU
+    checkpin.rise(&dmpDataReady);
+
+    //Used to set angle upon startup, filter
+    bool FILTER_DISABLE = true;
+
     while(1) {
-        // New DMP Orientation solution?
-        fifoCount = mpu.getFIFOCount();
-        if (fifoCount >= 18) {
-            if (fifoCount > 18) { // If we have more than one packet we take the easy path: discard the buffer and wait for the next one
-                pc.printf("FIFO RESET!!\r\n");
-                mpu.resetFIFO();
-                return;
-            }
+
+        if(button) {
+            pos_M1 = 0;
+            pos_M2 = 0;
+            target_pos = 0;
+        }
+
+        while(!mpuInterrupt) { // && fifoCount < packetSize) {
+            //led4 = led4^1;
+            //pc.printf("In while comp loop \r\n");
+            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;
+
+            //Joystick control
+            throttle = (float)jstick_v  /10.0;
+            steering = (float)jstick_h / 10.0;
+
+            //Update Values
             loop_counter++;
             slow_loop_counter++;
+            medium_loop_counter++;
             dt = (timer_value - timer_old);
             timer_old = timer_value;
+            angle_old = angle;
 
-            angle_old = angle;
-            // Get new orientation angle from IMU (MPU6050)
-            angle = dmpGetPhi();
+            // Motor contorl
+            if((angle < 45) && (angle > -45)) {
 
-            mpu.resetFIFO();  // We always reset FIFO
-        } // End of new IMU data
+                //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.
 
-        if(loop_counter >= 5) {
-            loop_counter = 0;
-            int16_t offset = 
-            pc.printf("angle: %d \r\n", int16_t(angle-ANGLE_OFFSET));
-            setMotor1Speed(int16_t(angle-ANGLE_OFFSET));
-            setMotor2Speed(int16_t(angle-ANGLE_OFFSET));
-        }
-        if (slow_loop_counter >= 99) { // 2Hz
-            slow_loop_counter = 0; // Read  status
-        }  // End of slow loop
+                //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
+                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);
+                
+                //Update values
+                target_pos_old = target_pos;
+                robot_pos_old2 = robot_pos_old1;
+                robot_pos_old1 = robot_pos_old;
+                
+                //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));
+                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;
+
+                //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 {
+                //Disable Motors
+                enable = DISABLE;
+                //Set Motor Speed 0
+                PID_errorSum = 0;  // Reset PID I term
+            }
+
+            //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);
+            }
 
 
-        /*
-        //Set Gains
-        Kp = 0.02;
-        Kd = 0.01;
+            //Meduim Loop
+            if (medium_loop_counter >= 10) {
+                medium_loop_counter = 0; // Read  status
+                led2 = led2^1;
 
-        timer_value = timer.read_ms();
+                //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();
+                }
+            }  // End of medium loop
+            
+            //Slow Loop
+            if(slow_loop_counter >= 99) {
+                slow_loop_counter = 0;
 
-        // if programming failed, don't try to do anything
-        if (!dmpReady) return;
+                //Send Data
+                txBuffer[ANGLE] = (uint8_t)(angle + TX_ANGLE_OFFSET);
+                mrf.Send(txBuffer, TX_BUFFER_LEN);
+            } //End of Slow Loop
 
+            //Reattach interupt
+            checkpin.rise(&dmpDataReady);
+        } //END WHILE
+
+        //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
         mpuInterrupt = false;
         mpuIntStatus = mpu.getIntStatus();
@@ -215,90 +414,34 @@
         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
-            //CS while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
+            while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
 
             // read a packet from FIFO
             mpu.getFIFOBytes(fifoBuffer, packetSize);
 
-            angle_old = angle; //Update old angle before reading new angle
-
             // track FIFO count here in case there is > 1 packet available
             // (this lets us immediately read more without waiting for an interrupt)
             fifoCount -= packetSize;
-            mpu.dmpGetQuaternion(&q, fifoBuffer);
-            angle = atan2(2 * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z) * RAD2GRAD;
-            angle = angle - ANGLE_OFFSET;
-            //pc.printf("angle: %f \r\n", angle);
 
-
-            //Update timer
-            dt = (timer_value - timer_old);
-            timer_old = timer_value;
-
-            //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)
-            //CS actual_robot_speed_old = actual_robot_speed;
-            //CS actual_robot_speed = (speed_M1 + speed_M2) / 2; // Positive: forward
-            //CS int16_t angular_velocity = (angle - angle_old) * 90.0; // 90 is an empirical extracted factor to adjust for real units
-            //CS int16_t estimated_speed = -actual_robot_speed_old - angular_velocity;     // We use robot_speed(t-1) or (t-2) to compensate the delay
-            //CS 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);
-            //CD target_angle = constrain(target_angle, -max_target_angle, max_target_angle); // limited output
-            // 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);
-            control_output = constrain(control_output, -MAX_CONTROL_OUTPUT, MAX_CONTROL_OUTPUT); // Limit max output from control
-            motor1 = control_output + steering;
-            motor2 = control_output - steering;
+            //Read new angle from IMU
+            new_angle = (float)(dmpGetPhi() - ANGLE_OFFSET);
+            dAngle = new_angle - angle;
 
 
-            //TEST P CONTROL
-            float gain = 1;
-            motor1 = angle * gain;
-            motor2 = angle * gain;
-            pc.printf("motor: %d control output: %f \r\n", motor1, control_output);
-
-            // Limit max speed (control output)
-            motor1 = constrain(motor1, -MAX_CONTROL_OUTPUT, MAX_CONTROL_OUTPUT);
-            motor2 = constrain(motor2, -MAX_CONTROL_OUTPUT, MAX_CONTROL_OUTPUT);
-
+            //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
 
         }
-
-
-        // Put all the pid loop stuff here
-        if((angle < 45) && (angle > -45)) {
-            //Enable Motors
-            if(motor1 == 0) {
-                enable = DISABLE;
-                setMotor1Speed(0);
-                setMotor2Speed(0);
-            } else {
-                enable = ENABLE;
-                setMotor1Speed(motor1);
-                setMotor2Speed(motor2);
-            }
-            //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 {
-            //Disable Motors
-            enable = DISABLE;
-            //Set Motor Speed 0
-            PID_errorSum = 0;  // Reset PID I term
-            Kp = 0;
-            Kd = 0;
-            Kp_thr = 0;
-            Ki_thr = 0;
-        }
-
-        *////////////
     } //end main loop
 } //End Main()
\ No newline at end of file