Diff: main.cpp

Revision:

3:2f76ffbc5cef

Parent:

2:42c4f3a7813f

Child:

4:2512939c10f0

--- a/main.cpp	Sun Jan 31 14:12:13 2016 +0000
+++ b/main.cpp	Wed Oct 12 05:04:10 2016 +0000
@@ -1,17 +1,304 @@
-//#include "MPU6050_DMP6.h"
-//
-//int main() {
-//    MPU6050DMP6::setup();
-//    while(1) {
-//        MPU6050DMP6::loop();
-//    }
-//}
+//BroBot V2
+//Author: Carter Sharer 
+//Date: 10/11/2016
+
+#include "pin_assignments.h"
+#include "I2Cdev.h"
+#include "JJ_MPU6050_DMP_6Axis.h"
+#include "BroBot.h"
+#include "BroBot_IMU.h"
+#include "stepper_motors.h"
+
+//PID
+#define MAX_THROTTLE 580
+#define MAX_STEERING 150
+#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
+
+//PID Default control values from constant definitions
+float Kp = KP;
+float Kd = KD;
+float Kp_thr = KP_THROTTLE;
+float Ki_thr = KI_THROTTLE;
+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;
+float target_angle;
+float throttle = 0;
+float steering = 0;
+float max_throttle = MAX_THROTTLE;
+float max_steering = MAX_STEERING;
+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
+
+Timer timer;
+int timer_value; //maybe make this a long
+int timer_old; //maybe make this a long
+float dt;
+
+uint8_t slow_loop_counter;
+uint8_t loop_counter;
+
+Serial pc(USBTX, USBRX);
+
+// =============================================================================
+// ===      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;
+    //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;
+    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                       ===
+// ================================================================
+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
+    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()
+{
+    pc.baud(115200);
+    pc.printf("Start\r\n");
+    init_imu();
+    timer.start();
+    //timer
+    timer_value = timer.read_ms();
 
-#include "MPU6050_raw.h"
+    //Init Stepper Motors
+    //Attach Timer Interupts (Tiker)
+    timer_M1.attach_us(&ISR1, ZERO_SPEED);
+    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;
+    
+    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;
+            }
+            loop_counter++;
+            slow_loop_counter++;
+            dt = (timer_value - timer_old);
+            timer_old = timer_value;
+
+            angle_old = angle;
+            // Get new orientation angle from IMU (MPU6050)
+            angle = dmpGetPhi();
+
+            mpu.resetFIFO();  // We always reset FIFO
+        } // End of new IMU data
+
+        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
+
+
+        /*
+        //Set Gains
+        Kp = 0.02;
+        Kd = 0.01;
+
+        timer_value = timer.read_ms();
+
+        // if programming failed, don't try to do anything
+        if (!dmpReady) return;
+
+        // 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();
+            // 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();
+
+            // read a packet from FIFO
+            mpu.getFIFOBytes(fifoBuffer, packetSize);
+
+            angle_old = angle; //Update old angle before reading new angle
 
-int main() {
-    MPU6050raw::setup();
-    while(1) {
-        MPU6050raw::loop();
-    }
-}
\ No newline at end of file
+            // 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;
+
+
+            //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);
+
+
+        }
+
+
+        // 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