Revision 21:d266d1e503ce, committed 2017-10-06

Comitter:

tvlogman

Date:

Fri Oct 06 08:34:14 2017 +0000

Parent:

20:4ce3fb543a45

Child:

22:2e473e9798c0

Commit message:

Working P-controller implemented. Note that setting motor magnitude pin to anything under 0.35 doesn't seem to do much - that needs a fix.

Changed in this revision

--- a/main.cpp	Fri Oct 06 07:23:26 2017 +0000
+++ b/main.cpp	Fri Oct 06 08:34:14 2017 +0000
@@ -18,7 +18,8 @@
 InterruptIn sw2(SW2);
 InterruptIn sw3(SW3);
 InterruptIn button1(D2);
-AnalogIn pot(A0);
+AnalogIn pot1(A0);
+AnalogIn pot2(A1);
 
 // PWM settings
 float pwmPeriod = 1.0/5000.0;
@@ -37,26 +38,37 @@
 // 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
 
-// Function getReferenceVelocity returns reference r between -8.4 and 8.4 rad/s
-float getReferenceVelocity(){
-    const float maxVelocity = 8.4; // max velocity in rad/s
+// Function getReferencePosition returns reference angle within range
+float getReferencePosition(){
     double r;
     if(m1_direction == false){
         // Clockwise rotation yields positive reference
-        r = maxVelocity*pot.read();
+        r = maxAngle*pot1.read();
         }
     if(m1_direction == true){
         // Counterclockwise rotation yields negative reference
-        r = -1*maxVelocity*pot.read();
+        r = -1*maxAngle*pot1.read();
         }
     return r;
     }
 
-// motorController sets a motorValue based on the reference. Sign indicates direction and magnitude indicates speed. Right now this is a simple feedforward
-float motorController(float reference){
-    const float motorGain = 8.4; // max power of the motor should normalize to a motorValue magnitude of 1
-    double motorValue = reference/motorGain; // motorValue is the normalized voltage applied to motor magnitude pin between -1 and 1
+// readEncoder reads counts and revs and logs results to serial window
+float getError(){
+    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;
+    }
+
+// 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 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
     return motorValue;
     }
     
@@ -86,9 +98,10 @@
     }
     
 void measureAndControl(){
-    float referenceVelocity = getReferenceVelocity();
-    float motorValue = motorController(referenceVelocity);
+    float posError = getError();
+    float motorValue = motorController(posError);
     pc.printf("Motorvalue is %.2f \r \n", motorValue);
+    pc.printf("Position error is %.2f \r \n", posError);
     pc.printf("Motor direction is %d \r \n", motor1_direction);
     setMotor1(motorValue);
     }
@@ -108,14 +121,7 @@
     m1_direction = !m1_direction;
     }
 
-// readEncoder reads counts and revs and logs results to serial window
-void readEncoder(){
-    counts = Encoder.getPulses();
-    revs = counts/(131*64.0f);
-    pc.printf("%0.2f revolutions \r\n", revs);
-    float reference = getReferenceVelocity();
-    pc.printf("Reference velocity %0.2f \r\n", reference);
-    }
+
 
 int main()
     {
@@ -126,7 +132,6 @@
     sw3.fall(&turnMotorsOn);
     button1.rise(&M1switchDirection);
     pc.baud(115200);
-    encoderTicker.attach(readEncoder, 2);
     controllerTicker.attach(measureAndControl, 0.1);
       
     pc.printf("Encoder ticker attached and baudrate set");