Diff: main.cpp

Revision:

11:2553f5798f84

Parent:

10:48f640a54401

Child:

12:16d1a5390022

--- a/main.cpp	Tue Jan 10 21:10:39 2017 +0000
+++ b/main.cpp	Thu Jan 19 19:18:54 2017 +0000
@@ -24,6 +24,7 @@
 
 //BroBot Begin
 #include "cmsis_os.h"
+#include "rtos_definations.h"
 #include "pin_assignments.h"
 #include "I2Cdev.h"
 #include "JJ_MPU6050_DMP_6Axis.h"
@@ -76,10 +77,11 @@
 #define GPIOC p23
 #define GPIOD p24
 
-//*********** Thread Definations BEGIN ***********//
-void imu_update_thread(void const *args);
-osThreadDef(imu_update_thread, osPriorityNormal, DEFAULT_STACK_SIZE);
-//*********** Thread Definations END *************//
+#define THROTTLE_DAMPNER 1000
+
+
+
+
 
 //*********** Local Function Definations BEGIN **************//
 void init_system();
@@ -116,8 +118,8 @@
 int timer_old; //maybe make this a long
 int dt;
 
-uint8_t slow_loop_counter;
-uint8_t medium_loop_counter;
+//uint8_t slow_loop_counter;
+//uint8_t medium_loop_counter;
 uint8_t loop_counter;
 
 Serial pc(USBTX, USBRX);
@@ -140,14 +142,36 @@
 //Used to set angle upon startup
 bool initilizeAngle = true;
 
+//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_old1, robot_pos_old2;
+float velocity;
+bool fallen = true;
+
 // ================================================================
 // ===                      IMU Thread                          ===
 // ================================================================
 void imu_update_thread(void const *args)
 {
-    while (true) {
-        gpioMonitorA = 1;
-        if(mpuInterrupt){
+    pc.printf("Starting IMU Thread..\r\n");
+    while (1) {
+        osSignalWait(IMU_UPDATE_SIGNAL,osWaitForever);
+        //gpioMonitorA = 1;
+        if(mpuInterrupt) {
             mpuInterrupt = false;
             led3 = !led3;
             /********************* All IMU Handling DO NOT MODIFY *****************/
@@ -193,14 +217,179 @@
                     pc.printf("\t\t\t Delta Angle too Large: %d. Ignored \r\n",dAngle);
                 }
             }
-            gpioMonitorA = 0;
+            //gpioMonitorA = 0;
             /********************* All IMU Handling DO NOT MODIFY *****************/
         }//End of if(mpuInterrupt) loop
+        osSignalClear(imu_update_thread_ID,IMU_UPDATE_SIGNAL);
         osThreadYield(); // Yield to a running thread
     }
 }
 
+// ================================================================
+// ===                      PID Thread                          ===
+// ================================================================
+void pid_update_thread(void const *args)
+{
+    pc.printf("Starting PID Thread..\r\n");
+    while(1) {
+        gpioMonitorA = 1;
+        //Get the time stamp as soon as it enters the loop
+        timer_value = timer.read_us();
+        //led1 = led1^1;
+        led4 = !fallen;
+        led2 = button;
 
+        if(jstick_v > 80)
+            led3 = 1;
+        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
+        //knob1 = 42.157;
+        //knob2 = 41.135;
+        //knob3 = 8.82;
+        //knob4 = 20.68;
+
+        //Set Gainz with knobs
+        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  /THROTTLE_DAMPNER;
+        steering = (float)jstick_h / 10.0;
+
+        //Calculate the delta time
+        dt = (timer_value - timer_old);
+        timer_old = timer_value;
+        angle_old = angle;
+        
+        //loopcounter
+        loop_counter++;
+
+        // STANDING: Motor Control Enabled
+        //******************** PID Control BEGIN ********************** //
+        if(((angle < 45) && (angle > -45)) && (fallen == false)) {
+
+            //CS Pd Target Angle Contoller Goes Here
+
+            //Robot Position
+            robot_pos = (pos_M1 + pos_M2) / 2;
+            target_pos += throttle/2;
+
+            //Robot Current Velocity
+            velocity = (motor1 + motor2) / 2;
+
+            //CS ***************** Velocity Controller *********************
+            //float target_velocity = 0;
+            //velocity_error = target_velocity - velocity;
+            //target_angle = -velocity_error * Kp2 * 100;
+            //CS ***************** Position Controller *********************
+            pos_error = robot_pos - target_pos; //robot_pos - target_pos;
+
+            //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 = ((-Kd2 * change_in_target_pos) + (-Kd2*change_in_pos)) /dt;
+            kd_pos_term = ((Kd2 * change_in_target_pos) + (Kd2*velocity));
+            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;
+
+            //CS ************ PD Stability CONTROLLER HERE ****************
+            error = target_angle - angle;
+            kp_term = Kp1 * error;
+
+            change_in_target_angle = target_angle - target_angle_old; //add
+            change_in_angle = angle - angle_old2; //add
+            kd_term = ((Kd1 * change_in_target_angle) - (Kd1*change_in_angle)) / dt;
+            //kd_term = ((Kd1 * change_in_target_angle) - (Kd1*velocity));
+
+            //pc.printf("dAngle:%f\r\n", angle-angle_old1);
+
+            //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 = 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);
+            //setMotor1Speed(20);
+            //setMotor2Speed(40);
+            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 { //[FALLEN}
+            //Disable Motors
+            enable = DISABLE;
+
+            //Set fallen flag
+            fallen = true;
+        }
+        
+         if(loop_counter >= 5) {
+            gpioMonitorC = 1;
+            loop_counter = 0;
+            //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);
+            //wait_us(200);
+            gpioMonitorC = 0;
+            
+        }
+        gpioMonitorA = 0;
+    } //end main loop
+
+}
+
+// ================================================================
+// ===                Communication Thread                      ===
+// ================================================================
+void communication_update_thread(void const *args)
+{
+    pc.printf("Starting Communication Thread..\r\n");
+    while(1) {
+        //Recieve Data
+        //gpioMonitorC = 1;
+        rxLen = rf_receive(rxBuffer, 128);
+        if(rxLen > 0) {
+            led1 = led1^1;
+            //Process data with our protocal
+            communication_protocal(rxLen);
+        }
+        osDelay(30);
+        //gpioMonitorC = 0;
+    }
+}
 
 
 // ================================================================
@@ -279,199 +468,31 @@
     gpioMonitorB = 0;
     gpioMonitorC = 0;
     gpioMonitorD = 0;
+    
+    loop_counter = 0;
 
+    enable = ENABLE; //Enable Motors
 }
 
 // ================================================================
 // ===                    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_old1, robot_pos_old2;
-float velocity;
-bool fallen = true;
+
 
 int main()
 {
 
-
-
     init_system();
-    enable = ENABLE; //Enable Motors
 
     //Create IMU Thread
-    osThreadCreate(osThread(imu_update_thread), NULL);
-
-    while(1) {
-
-        //Get the time stamp as soon as it enters the loop
-        timer_value = timer.read_us();
-        //led1 = led1^1;
-        led4 = !fallen;
-        led2 = button;
+    imu_update_thread_ID = osThreadCreate(osThread(imu_update_thread), NULL);
 
-        if(jstick_v > 80)
-            led3 = 1;
-        else
-            led3 = 0;
+    //Create PID Thread
+    pid_update_thread_ID = osThreadCreate(osThread(pid_update_thread), NULL);
 
-        //Button Press on the remote initilizes the robot position.
-        if(button) {
-            pos_M1 = 0;
-            pos_M2 = 0;
-            target_pos = 0;
-            fallen = false;
-        }
+    //Create Communication Thread
+    communication_update_thread_ID = osThreadCreate(osThread(communication_update_thread), NULL);
 
 
-        //Preset Knob Values for Green Balance Bot
-        knob1 = 42.157;
-        knob2 = 41.135;
-        knob3 = 8.82;
-        knob4 = 20.68;
 
-        //Set Gainz with knobs
-        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;
-        steering = (float)jstick_h / 10.0;
-
-        //Increment the loop counters
-        loop_counter++;
-        slow_loop_counter++;
-        medium_loop_counter++;
-
-        //Calculate the delta time
-        dt = (timer_value - timer_old);
-        timer_old = timer_value;
-        angle_old = angle;
-
-        // STANDING: Motor Control Enabled
-        //******************** PID Control BEGIN ********************** //
-        if(((angle < 45) && (angle > -45)) && (fallen == false)) {
-            gpioMonitorB = 1;
-
-            //CS Pd Target Angle Contoller Goes Here
-
-            //Robot Position
-            robot_pos = (pos_M1 + pos_M2) / 2;
-            target_pos += throttle/2;
-
-            //Robot Current Velocity
-            velocity = motor1 + motor2 / 2;
-
-            //CS ***************** Velocity Controller *********************
-            //float target_velocity = 0;
-            //velocity_error = target_velocity - velocity;
-            //target_angle = -velocity_error * Kp2 * 100;
-            //CS ***************** Position Controller *********************
-            pos_error = robot_pos - target_pos; //robot_pos - target_pos;
-
-            //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 = ((-Kd2 * change_in_target_pos) + (-Kd2*change_in_pos)) /dt;
-            kd_pos_term = ((Kd2 * change_in_target_pos) + (Kd2*velocity));
-            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;
-
-            //CS ************ PD Stability CONTROLLER HERE ****************
-            error = target_angle - angle;
-            kp_term = Kp1 * error;
-
-            change_in_target_angle = target_angle - target_angle_old; //add
-            change_in_angle = angle - angle_old2; //add
-            kd_term = ((Kd1 * change_in_target_angle) - (Kd1*change_in_angle)) / dt;
-            //kd_term = ((Kd1 * change_in_target_angle) - (Kd1*velocity));
-
-            //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));
-            motor2 = (int16_t)(control_output - (steering));
-            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;
-            gpioMonitorB = 0;
-            //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 { //[FALLEN}
-            //Disable Motors
-            enable = DISABLE;
-
-            //Set fallen flag
-            fallen = true;
-        }
-        //******************** PID Control END ********************** //
-
-        //Fast Loop
-        if(loop_counter >= 5) {
-            loop_counter = 0;
-            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);
-        }
-
-        //Meduim Loop
-        if (medium_loop_counter >= 10) {
-            medium_loop_counter = 0; // Read  status
-
-            //Recieve Data
-            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 To Controller goes here *
-             *                                    */
-        } //End of Slow Loop
-
-        //Reattach interupt
-        //checkpin.rise(&dmpDataReady);
-        
-        gpioMonitorB = 0;
-    } //end main loop
 } //End Main()
\ No newline at end of file