Diff: main.cpp

Revision:

16:3a85662fb740

Parent:

15:d6d7623a17f8

Child:

17:8e2824f64b91

--- a/main.cpp	Thu Jan 26 21:37:24 2017 +0000
+++ b/main.cpp	Wed Feb 08 22:33:21 2017 +0000
@@ -33,11 +33,12 @@
 #include "stepper_motors.h"
 #include "MRF24J40.h"
 #include "communication.h"
+#include "waypoint.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
-#define THETA_OFFSET 165
+#define THETA_OFFSET 0//165
 #define MRF_CHANNEL 2
 
 //For RF Communication
@@ -79,20 +80,16 @@
 #define GPIOD p24
 
 #define THROTTLE_DAMPNER 100
-
+#define STREEING_DAMPNER 10000
 #define VELOCITY_SAMPLES 1000
 
-
-
-
-
 //*********** Local Function Definations BEGIN **************//
 void init_system();
 void init_imu();
 //*********** Local Function Definations END **************//
 
 
-
+WAYPOINTS w1;
 
 
 
@@ -106,9 +103,14 @@
 int timer_value = 0;
 int timer_old = 0;
 float angle = 0;
-float Theta = 0;
+float theta = 0;
+float totalTheta = 0;
+float deltaTheta = 0;
 float velocitySamples[VELOCITY_SAMPLES + 1] = {0.0};
 
+float target_pos = 0;
+ float pos_error = 0;
+
 Serial pc(USBTX, USBRX);
 
 // LEDs
@@ -128,6 +130,7 @@
 
 //Used to set angle upon startup
 bool initilizeAngle = true;
+bool initilizeTheta = true;
 
 
 // ================================================================
@@ -136,7 +139,9 @@
 void imu_update_thread(void const *args)
 {   
     float dAngle = 0;
+    float dTheta = 0;
     float new_angle = 0;
+    float new_theta = 0;
     long long timeOut = 0;
     
    
@@ -183,14 +188,15 @@
                 fifoCount -= packetSize;
 
                 //Read new angle from IMU
-                dmpGetReadings(&new_angle,&Theta);
+                dmpGetReadings(&new_angle,&new_theta);
                 new_angle = (float)(new_angle - ANGLE_OFFSET);
-                Theta = float(Theta + THETA_OFFSET);
+                new_theta = float(new_theta + THETA_OFFSET);
                 pc.printf("Angle: %f\r\n",new_angle);
-                pc.printf("Theta: %f\r\n",Theta);
+                pc.printf("Theta: %f\r\n",new_theta);
                 
                 //Calculate the delta angle
                 dAngle = new_angle - angle;
+                dTheta = new_theta - theta;
 
                 //Filter out angle readings larger then MAX_ANGLE_DELTA
                 if(initilizeAngle) {
@@ -198,10 +204,24 @@
                     initilizeAngle = false;
                 } else if( ((dAngle < 15) && (dAngle > -15))) {
                     angle = new_angle;
+                    theta = new_theta;
                 } else {
-                    enable = DISABLE;
                     pc.printf("\t\t\t Delta Angle too Large: %d. Ignored \r\n",dAngle);
                 }
+                
+                if(initilizeTheta) {
+                    theta = new_theta;
+                    initilizeTheta = false;
+                } else if( ((dTheta < 15) && (dTheta > -15))) {
+                    theta = new_theta;
+                } else {
+                    pc.printf("\t\t\t Delta Theta too Large: %d. Ignored \r\n",dTheta);
+                    theta = new_theta;
+                    dTheta = dTheta > 0 ? -1 : 1;
+                    dTheta = 1;
+                }
+                 totalTheta += dTheta;
+                  //pc.printf("Total Theta: %f\r\n",totalTheta);
             }
             else{
                 pc.printf("\t\t\t IMU Error!!\r\n");
@@ -215,6 +235,24 @@
     }
 }
 
+
+// ================================================================
+// ===                      Way Point Thread                    ===
+// ================================================================
+/*
+void pid_update_thread(void const *args){
+    
+     while(1) {
+         target_pos = w1.targetPosition;
+         if()
+         
+         
+         
+         
+    }
+}
+*/
+
 // ================================================================
 // ===                      PID Thread                          ===
 // ================================================================
@@ -235,6 +273,7 @@
     float kd_term = 0;
     float throttle = 0;
     float steering = 0;
+    float steering_output = 0;
     float control_output = 0;
     int robot_pos = 0;
     int loop_counter = 0;
@@ -245,11 +284,11 @@
     bool fallen = true;
     
     // --- For Position controller  --- //
-    float pos_error = 0;
+   
     float kp_pos_term = 0;
     float kd_pos_term = 0;
     float change_in_target_pos = 0;
-    float target_pos = 0;
+    
     float target_pos_old = 0;
     float target_velocity = 0;
     float velocity_error = 0;
@@ -274,6 +313,9 @@
             motor1 = 0;
             motor2 = 0;
             control_output = 0;
+            totalTheta = 0;
+            target_theta = 0;
+            steering_output = 0;
             fallen = false;
         }
         gpioMonitorA = 1;
@@ -288,9 +330,6 @@
         else
             led3 = 0;
 
-       
-
-
         //Preset Knob Values for Green Balance Bot
         //knob1 = 1.132;
         //knob2 = 47.284;
@@ -304,8 +343,8 @@
         Kd2 = ((float)knob4);// / 100.0;
 
         //Joystick control
-        throttle = (float)jstick_v / THROTTLE_DAMPNER;
-        steering = (float)jstick_h / 10.0;
+        //throttle = (float)jstick_v / THROTTLE_DAMPNER;
+        //steering = (float)jstick_h / STREEING_DAMPNER;
 
         //Calculate the delta time
         dt = (timer_value - timer_old);
@@ -321,7 +360,7 @@
 
             //Robot Position
             robot_pos = (pos_M1 + pos_M2) / 2;
-            target_pos += throttle/2;
+            //target_pos += throttle/2;
     
             //Velocity Computation
             velocitySamplesCounter++;
@@ -335,10 +374,11 @@
             velocity = velocitySum/VELOCITY_SAMPLES;
             
             // ***************** Angular Controller *********************
+            
             float Kp1A = Kp1/100;
-            target_theta = 0;
-            theta_error = Theta - target_theta;
-            steering = -Kp1A * theta_error;
+            target_theta += steering;
+            theta_error =  totalTheta - target_theta;
+            steering_output = -Kp1A * theta_error;
             
             // ***************** Position Controller *********************
             float Kp1P = Kp1/100;
@@ -386,8 +426,8 @@
             //Control Output
             control_output += kp_term + kd_term; //-100 to 100
             control_output = CAP(control_output, MAX_CONTROL_OUTPUT); // Limit max output from control
-            motor1 = (int16_t)(control_output + (steering));
-            motor2 = (int16_t)(control_output - (steering));
+            motor1 = (int16_t)(control_output + (steering_output));
+            motor2 = (int16_t)(control_output - (steering_output));
             motor1 = CAP(motor1, MAX_CONTROL_OUTPUT);
             motor2 = CAP(motor2, MAX_CONTROL_OUTPUT);
 
@@ -404,6 +444,11 @@
             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);
+            
+            if(abs(motor1) == MAX_CONTROL_OUTPUT || abs(motor2) == MAX_CONTROL_OUTPUT) {
+                fallen = true;
+            }
+            
         } else { //[FALLEN}
             //Disable Motors
             enable = DISABLE;
@@ -495,6 +540,11 @@
 void init_system()
 {
     initilizeAngle = true;
+    totalTheta = 0;
+    w1.targetPosition = 1000;
+    w1.targetAngle = 90;
+    target_pos = 0;
+    pos_error = 0;
     //Set the Channel. 0 is default, 15 is max
     mrf.SetChannel(MRF_CHANNEL);
     enable = DISABLE; //Disable Motors
@@ -547,6 +597,9 @@
 
     //Create Communication Thread
     communication_update_thread_ID = osThreadCreate(osThread(communication_update_thread), NULL);
+    
+    //Create waypoint Thread
+    //waypoint_update_thread_ID = osThreadCreate(osThread(waypoint_update_thread), NULL);