Diff: main.cpp

Revision:

22:2e473e9798c0

Parent:

21:d266d1e503ce

Child:

23:917179f72762

--- a/main.cpp	Fri Oct 06 08:34:14 2017 +0000
+++ b/main.cpp	Fri Oct 06 10:43:24 2017 +0000
@@ -11,6 +11,12 @@
 MODSERIAL pc(USBTX, USBRX);
 
 // Defining outputs
+
+// Leds can be used to indicate status
+DigitalOut ledG(LED_GREEN);
+DigitalOut ledR(LED_RED);
+DigitalOut ledB(LED_BLUE);
+
 DigitalOut motor1_direction(D4);
 PwmOut motor1_pwm(D5);
 
@@ -36,10 +42,10 @@
 volatile float revs = 0.00;
 
 // MOTOR CONTROL PART    
-
 bool m1_direction = false;
 int posRevRange = 5; // describes the ends of the position range in complete motor output shaft revolutions in both directions
 const float maxAngle = 2*3.14*posRevRange; // max angle in radians
+const float Ts = 0.1;
 
 // Function getReferencePosition returns reference angle within range
 float getReferencePosition(){
@@ -55,20 +61,28 @@
     return r;
     }
 
+// Initializing position and integral errors
+float posError = 0;
+float integralError = 0;
+float totalError = posError + integralError;
+
 // readEncoder reads counts and revs and logs results to serial window
-float getError(){
+void getError(float &posError, float &integralError){
     counts = Encoder.getPulses();
     double motor1Position = 2*3.14*(counts/(131*64.0f));
     pc.printf("%0.2f revolutions \r\n", motor1Position);
-    float posError = getReferencePosition() - motor1Position;
-    pc.printf("Error is %0.2f \r \n", posError);
-    return posError;
+    posError = getReferencePosition() - motor1Position;
+    
+    integralError = integralError + Ts*totalError;
+    totalError = posError + integralError;
+    pc.printf("Error is %0.2f \r \n", totalError);
     }
 
 // motorController sets a motorValue based on the position error. Sign indicates direction and magnitude indicates speed. Right now this is a simple feedforward
-float motorController(float posError){
+float motorController(float posError, float integralError){
     float k_p = 5*pot2.read(); // use potmeter 2 to adjust k_p within range 0 to 1
-    double motorValue = (posError*k_p)/maxAngle + 0.35; // motorValue is the normalized voltage applied to motor magnitude pin between -1 and 1 added 0.35 as motorvalue of 0.35 does nothing
+    float k_i = 0.0000000001;
+    double motorValue = (posError*k_p)/maxAngle + 0.35 + k_i*integralError; // motorValue is the normalized voltage applied to motor magnitude pin between -1 and 1 added 0.35 as motorvalue of 0.35 does nothing
     return motorValue;
     }
     
@@ -77,6 +91,9 @@
     switch(currentState){
                 case KILLED:
                     motor1_pwm.write(0.0);
+                    ledR = 0;
+                    ledG = 1;
+                    ledB = 1;
                     break;
                 case ACTIVE:
                     // Set motor direction
@@ -92,17 +109,22 @@
                         }
                     else {
                         motor1_pwm.write(fabs(motorValue));
-                        }      
+                        }
+                    ledR = 1;
+                    ledG = 1;
+                    ledB = 0;      
                     break;
             }
     }
     
 void measureAndControl(){
-    float posError = getError();
-    float motorValue = motorController(posError);
-    pc.printf("Motorvalue is %.2f \r \n", motorValue);
+    getError(posError, integralError);
+    float motorValue = motorController(posError, integralError);
     pc.printf("Position error is %.2f \r \n", posError);
-    pc.printf("Motor direction is %d \r \n", motor1_direction);
+    pc.printf("Total error is %.2f \r \n", totalError);
+    pc.printf("Integral error is %2f", integralError);
+    //pc.printf("Motorvalue is %.2f \r \n", motorValue);
+    //pc.printf("Motor direction is %d \r \n", motor1_direction);
     setMotor1(motorValue);
     }
 
@@ -119,6 +141,7 @@
     
 void M1switchDirection(){
     m1_direction = !m1_direction;
+    pc.printf("Switched direction!");
     }
 
 
@@ -132,7 +155,7 @@
     sw3.fall(&turnMotorsOn);
     button1.rise(&M1switchDirection);
     pc.baud(115200);
-    controllerTicker.attach(measureAndControl, 0.1);
+    controllerTicker.attach(measureAndControl, Ts);
       
     pc.printf("Encoder ticker attached and baudrate set");    
     }