Diff: main.cpp

Revision:

15:d6d7623a17f8

Parent:

14:94c65d1c8bad

Child:

16:3a85662fb740

--- a/main.cpp	Tue Jan 24 23:09:10 2017 +0000
+++ b/main.cpp	Thu Jan 26 21:37:24 2017 +0000
@@ -37,6 +37,7 @@
 //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
+#define THETA_OFFSET 165
 #define MRF_CHANNEL 2
 
 //For RF Communication
@@ -104,6 +105,8 @@
 Timer timer;
 int timer_value = 0;
 int timer_old = 0;
+float angle = 0;
+float Theta = 0;
 float velocitySamples[VELOCITY_SAMPLES + 1] = {0.0};
 
 Serial pc(USBTX, USBRX);
@@ -131,7 +134,12 @@
 // ===                      IMU Thread                          ===
 // ================================================================
 void imu_update_thread(void const *args)
-{
+{   
+    float dAngle = 0;
+    float new_angle = 0;
+    long long timeOut = 0;
+    
+   
     pc.printf("Starting IMU Thread..\r\n");
     while (1) {
         osSignalWait(IMU_UPDATE_SIGNAL,osWaitForever);
@@ -158,7 +166,14 @@
                 //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();
+                timeOut = 0;
+                while (fifoCount < packetSize) {
+                    timeOut++;
+                    fifoCount = mpu.getFIFOCount();
+                    if(timeOut > 100000000){
+                        break;
+                    }
+                }
 
                 //read a packet from FIFO
                 mpu.getFIFOBytes(fifoBuffer, packetSize);
@@ -168,7 +183,12 @@
                 fifoCount -= packetSize;
 
                 //Read new angle from IMU
-                new_angle = (float)(dmpGetPhi() - ANGLE_OFFSET);
+                dmpGetReadings(&new_angle,&Theta);
+                new_angle = (float)(new_angle - ANGLE_OFFSET);
+                Theta = float(Theta + THETA_OFFSET);
+                pc.printf("Angle: %f\r\n",new_angle);
+                pc.printf("Theta: %f\r\n",Theta);
+                
                 //Calculate the delta angle
                 dAngle = new_angle - angle;
 
@@ -179,9 +199,14 @@
                 } else if( ((dAngle < 15) && (dAngle > -15))) {
                     angle = new_angle;
                 } else {
+                    enable = DISABLE;
                     pc.printf("\t\t\t Delta Angle too Large: %d. Ignored \r\n",dAngle);
                 }
             }
+            else{
+                pc.printf("\t\t\t IMU Error!!\r\n");
+                
+            }
             //gpioMonitorA = 0;
             /********************* All IMU Handling DO NOT MODIFY *****************/
         }//End of if(mpuInterrupt) loop
@@ -233,11 +258,24 @@
     
     float runningSumReplaceVal = 0;
     float newVelocity = 0;
+    float angle_old = 0;
     
     float velocitySum = 0;
+    float theta_error = 0;
+    float target_theta = 0;
     memset(velocitySamples,0,sizeof(velocitySamples));
     pc.printf("Starting PID Thread..\r\n");
     while(1) {
+         //Button Press on the remote initilizes the robot position.
+        if(button) {
+            pos_M1 = 0;
+            pos_M2 = 0;
+            target_pos = 0;
+            motor1 = 0;
+            motor2 = 0;
+            control_output = 0;
+            fallen = false;
+        }
         gpioMonitorA = 1;
         //Get the time stamp as soon as it enters the loop
         timer_value = timer.read_us();
@@ -250,16 +288,7 @@
         else
             led3 = 0;
 
-        //Button Press on the remote initilizes the robot position.
-        if(button) {
-            pos_M1 = 0;
-            pos_M2 = 0;
-            target_pos = 0;
-            motor1 = 0;
-            motor2 = 0;
-            control_output = 0;
-            fallen = false;
-        }
+       
 
 
         //Preset Knob Values for Green Balance Bot
@@ -269,10 +298,10 @@
         //knob4 = 17.07;
 
         //Set Gainz with knobs
-        Kp1 = ((float)knob1) / 100.0;
-        Kd1 = ((float)knob2) / 1.0;
-        Kp2 = ((float)knob3) / 1000.0;
-        Kd2 = ((float)knob4) / 100.0;
+        Kp1 = ((float)knob1);// / 100.0;
+        Kd1 = ((float)knob2);// / 1.0;
+        Kp2 = ((float)knob3);// / 1000.0;
+        Kd2 = ((float)knob4);// / 100.0;
 
         //Joystick control
         throttle = (float)jstick_v / THROTTLE_DAMPNER;
@@ -305,39 +334,45 @@
             velocitySum = velocitySum - runningSumReplaceVal + newVelocity;
             velocity = velocitySum/VELOCITY_SAMPLES;
             
-            //CS ***************** Position Controller *********************
+            // ***************** Angular Controller *********************
+            float Kp1A = Kp1/100;
+            target_theta = 0;
+            theta_error = Theta - target_theta;
+            steering = -Kp1A * theta_error;
+            
+            // ***************** Position Controller *********************
+            float Kp1P = Kp1/100;
+            float Kd1P = Kd1*0;
+            Kp1P = 5.73/100;
             //target_pos = 0;
             pos_error = robot_pos - target_pos; //robot_pos - target_pos;
 
             //KP Term
-            kp_pos_term = Kp1 * pos_error;
+            kp_pos_term = Kp1P * pos_error;
 
             //KD Term
             change_in_target_pos = target_pos - target_pos_old;
             //kd_pos_term = ((-Kd2 * change_in_target_pos) + (-Kd2*change_in_pos)) /dt;
-            kd_pos_term = ((Kd1 * change_in_target_pos) + (Kd1*velocity));
+            kd_pos_term = ((Kd1P * change_in_target_pos) + (Kd1P*velocity));
             target_velocity = kp_pos_term; // + kd_pos_term;
             target_velocity = CAP(target_velocity, 100);
 
             //CS ***************** Velocity Controller *********************
+            float Kp2V = Kp2/1000;
+            float Kd2V = Kd2/100;
+            Kp2V = 98.51/1000;
             //target_velocity = throttle;
             velocity_error = target_velocity - velocity;
             change_in_velocity = velocity - velocitySamples[(velocitySamplesCounter + 500)%VELOCITY_SAMPLES];
-            d_velocity = (Kd2 * change_in_velocity)/dt;
+            d_velocity = (Kd2V * change_in_velocity)/dt;
             
-            /*
-            if(target_velocity <= 3 && target_velocity >= -3) {
-                target_angle = 0;    
-            }
-            else{
-                
-            }
-            */
-            target_angle = (-velocity_error * Kp2 * 50); //+ d_velocity;    
+            target_angle = (-velocity_error * Kp2V); //+ d_velocity;    
             //CS ************ PD Stability CONTROLLER HERE ****************
-            //Knob1 7.373/1000.0 Knob2 50.986/1.0
-            float Kp1S = 7.373/1000.0;
-            float Kd1S = 50.986/1.0;
+            float Kp1S = Kp1/1000; //7.373/1000.0;
+            float Kd1S = Kd1; //50.986/1.0;
+            Kp1S = 3.48/1000;
+            Kd1S = 44.5;
+            
             error = target_angle - angle;
             kp_term = Kp1S * error;
 
@@ -380,7 +415,7 @@
         if(loop_counter >= 20) {
             gpioMonitorC = 1;
             loop_counter = 0;
-            pc.printf("Target Vel: %f\r\n",target_velocity);            
+            //pc.printf("Target Vel: %f\r\n",target_velocity);            
             //pc.printf("angle:%d Kp1: %0.3f Kd1: %0.2f  Kp2: %0.2f Kd2: %0.3f  tang: %0.2f dt:%d rob_pos: %d VE: %f\r\n", (int)angle, Kp1, Kd1, Kp2, Ki2, target_angle, dt, robot_pos, velocity_error);
             //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);
             //pc.printf("CAngle: %d, TAngle: %d,Pos: %d, dt: %d\r\n",(int)angle,target_angle,robot_pos,dt);
@@ -426,7 +461,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]default
+    mpu.setRate(1);   // 0=1khz 1=500hz, 2=333hz, 3=250hz [4=200hz]default
     mpu.setSleepEnabled(false);
     wait_ms(500);