Diff: main.cpp

Revision:

9:d9776d8ce47c

Parent:

6:ae3e6aefe908

--- a/main.cpp	Fri Nov 11 19:20:41 2016 +0000
+++ b/main.cpp	Wed Nov 16 00:14:11 2016 +0000
@@ -67,6 +67,12 @@
 float max_target_angle = MAX_TARGET_ANGLE;
 int robot_pos = 0; //Robots position
 bool fallen = true;
+float d_angle = 0;
+float d_anglemax = 0;
+float angle_old2 = 0;
+float angle_old3 = 0;
+float angle_correction = 0;
+float refAngle = 0;
 
 Timer timer;
 int timer_value; 
@@ -166,6 +172,12 @@
     //Enable Motors
     enable = ENABLE;
 
+
+    float integral1 = 0;
+    float integral2 = 0;
+    int16_t motor1Old = 0;
+    int16_t motor2Old = 0;
+    float lastError = 0;
     while(1) {
         //Led 4 to indicate if robot it STANDING or FALLEN
         led4 = !fallen;
@@ -195,39 +207,86 @@
             throttle = jstick_v;
             steering = jstick_h;
 
-            /**** Update Values DO NOT MODIFY ********/
-            loop_counter++;
-            slow_loop_counter++;
-            medium_loop_counter++;
-            dt = (timer_value - timer_old);
-            timer_old = timer_value;
-            angle_old = angle;
-            /*****************************************/
 
+            float error = 0;
             //STANDING: Motor Control Enabled
             if(((angle < 45) && (angle > -45)) && (fallen == false)) {
 
+                //wait_ms(1);
                 //Enable Motor
                 enable = ENABLE;
                 
-                /* This is where you want to impliment your controlers 
-                * Start off with a simple P controller.
-                *
-                * float error = angle - 0; //should be balanced at 0
-                * motor1 = error * Kp; 
-                * motor2 = error * Kp;        */
+                //controls
+                error = angle - 0;//angle_correction; //should be balanced at 0
+
+                float Ki = 0.5;
+                
+                d_angle = angle-(angle_old+angle_old2+angle_old3)/3.0;
+               
+                /*
+                if (d_angle > d_anglemax) d_anglemax = d_angle;
+                if (d_angle > 2.0) d_angle = 2.0;
+                if (d_angle < -2.0) d_angle = -2.0;*/
+                if (d_angle < 3.0 && d_angle > -3.0) d_angle = 0;
+                
+                motor1 = error * Kp1/10.0 + Kd1/25.0 * d_angle;
+                motor2 = error * Kp2/10.0 + Kd2/25.0 * d_angle;        
+                
+                /*
+                if (d_angle == 0){
+                     if (motor1 < -5) angle_correction += 0.002;
+                     else if (motor1 > 5) angle_correction -= 0.002;
+                }
+                */
+                /*long positionError = pos_M1 - 0;
+                refAngle -= (double)positionError/500000.0; //initially 0
+                    
+                refAngle -= (double)motor1Old/500000.0;
+                    
+                if (refAngle < -5) {
+                    
+                    refAngle = -5;
+            
+                }
+                else if (refAngle > 5) {
+                    
+                    refAngle = 5;
+            
+                }
+                
+                error = (angle - refAngle)/10.0;
+                double P = Kp1 * error;
+                //I += 0.0001 * error;
+                double D = Kd1 * (error - lastError);
+                
+                lastError = error;
+                motor1 = error * P/10.0 + D/25.0 ;
+                motor2 = error * P/10.0 + D/25.0 ; 
+                */
                 
                 //Calculate motor inputs
-                motor1 = int16_t(throttle/2 + steering/8);
-                motor2 = int16_t(throttle/2 - steering/8);
+                motor1 += 0.1 * int16_t(throttle/2 + steering/8);
+                motor2 += 0.1 * int16_t(throttle/2 - steering/8);
                 
                 //Cap the max and min values [-100, 100]
                 motor1 = CAP(motor1, MAX_CONTROL_OUTPUT);
                 motor2 = CAP(motor2, MAX_CONTROL_OUTPUT);
                 
+                //Update speed if motor changes are above threshold
+                int thresh = 1;
+                if ((motor1 - motor1Old >= thresh) || (motor1-motor1Old <= -thresh)) {
+                    setMotor1Speed(motor1);
+                    motor1Old = motor1;
+                }
+                //motor1Old = motor1;
+                if ((motor2 - motor2Old >= thresh) || (motor2-motor2Old <= -thresh)) {
+                    setMotor2Speed(motor2);
+                    motor2Old = motor2;
+                }
+                //motor2Old = motor2;
                 //Set Motor Speed here
-                setMotor1Speed(motor1);
-                setMotor2Speed(motor2);
+                //setMotor1Speed(motor1);
+                //setMotor2Speed(motor2);
 
             } else { //FALLEN: Motor Control Disabled
                 //Disable Motors
@@ -235,6 +294,12 @@
 
                 //Set fallen flag
                 fallen = true;
+                
+                integral1 = 0;
+                integral2 = 0;
+                angle_correction = 0;
+                lastError = 0;
+                d_anglemax = 0;
             }
     
             /* Here are some loops that trigger at different intervals, this 
@@ -244,7 +309,7 @@
             //Fast Loop: Good for printing to serial monitor
             if(loop_counter >= 5) {
                 loop_counter = 0;
-                pc.printf("angle:%d Kp1:%0.3f Kd1:%0.2f Kp2:%0.2f Kd2:%0.3f pos_M1:%d pos_M2:%d \r\n", (int)angle, Kp1, Kd1, Kp2, Kd2, pos_M1, pos_M2, robot_pos);
+                //pc.printf("angle:%0.3f Kp1:%0.3f Kd1:%0.2f Kp2:%0.2f Kd2:%0.3f pos_M1:%d pos_M2:%d dt:%0.2f, %0.2f\r\n", angle, Kp1, Kd1, Kp2, Kd2, pos_M1, pos_M2, robot_pos, throttle, steering);
             }
 
             //Meduim Loop: Good for sending and receiving 
@@ -264,16 +329,26 @@
             //Slow Loop: Good for sending if speed is not an issue 
             if(slow_loop_counter >= 99) {
                 slow_loop_counter = 0;
-
-                /* Send Data To Controller goes here *
-                 *                                   */
+                sprintf(txBuffer, "M1: %d, M2: %d, error: %0.2f, angle: %0.3f, dangle: %0.2f correction: %0.3f, Kp: %0.2f, Kd: %0.2f,  motor1: %d", \
+                pos_M1, pos_M2, error, angle, d_angle, d_anglemax, Kp1, Kd1, motor1);
+                rf_send(txBuffer, strlen(txBuffer)+1);
             } //End of Slow Loop
 
             //Reattach interupt
             checkpin.rise(&dmpDataReady);
         } //END WHILE
 
-
+        /**** Update Values DO NOT MODIFY ********/
+        loop_counter++;
+        slow_loop_counter++;
+        medium_loop_counter++;
+        dt = (timer_value - timer_old);
+        timer_old = timer_value;
+        angle_old3 = angle_old2;
+        angle_old2 = angle_old;
+        angle_old = angle;
+        /*****************************************/
+        
         /********************* All IMU Handling DO NOT MODIFY *****************/
         //Disable IRQ
         checkpin.rise(NULL);