Diff: main.cpp

Revision:

19:f08b5cd2b7ce

Parent:

17:616ce7bc1f96

Child:

20:4ce3fb543a45

--- a/main.cpp	Thu Sep 21 11:53:09 2017 +0000
+++ b/main.cpp	Fri Oct 06 05:29:52 2017 +0000
@@ -9,7 +9,6 @@
 
 QEI Encoder(D12,D13,NC,64, QEI::X4_ENCODING);
 MODSERIAL pc(USBTX, USBRX);
-HIDScope scope(2);
 
 // Defining outputs
 DigitalOut motor1_direction(D4);
@@ -31,24 +30,67 @@
 
 // Initializing encoder
 Ticker encoderTicker;
+Ticker controllerTicker;
 volatile int counts = 0;
 volatile float revs = 0.00;
 
-void readEncoder(){
-    counts = Encoder.getPulses();
-    revs = counts/(131*64.0f);
-    pc.printf("%0.2f revolutions \r\n", revs);
-    
-    // Displaying revs in HIDscope
-    x = revs;   // Capture data
-    scope.set(0, x);   // store data in first element of scope memory
-    y = (x_prev + x)/2.0;   // averaging filter
-    scope.set(1, y);  // store data in second element of scope memory
-    x_prev = x; // Prepare for next round
-    
-    scope.send(); // send what's in scope memory to PC
+// MOTOR CONTROL PART    
+
+// 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
+    double r;
+    if(motor1_direction = false){
+        // Clockwise rotation yields positive reference
+        r = maxVelocity*pot.read();
+        }
+    if(motor1_direction = true){
+        // Counterclockwise rotation yields negative reference
+        r = -1*maxVelocity*pot.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
+    return motorValue;
     }
     
+// setMotor1 sets the direction and magnitude pins of motor1 depending on the given motorValue. Negative motorValue means clockwise rotation
+void setMotor1(float motorValue){
+    switch(currentState){
+                case KILLED:
+                    motor1_pwm.write(0.0);
+                    break;
+                case ACTIVE:
+                    // Set motor direction
+                    if (motorValue >=0){
+                        motor1_direction = 0;
+                        }
+                    else {
+                        motor1_direction = 1;
+                        }
+                    // Set motor speed
+                    if (fabs(motorValue)>1){ 
+                        motor1_pwm = 1;
+                        }
+                    else {
+                        motor1_pwm.write(fabs(motorValue));
+                        }      
+                    break;
+            }
+    }
+    
+void measureAndControl(){
+    float referenceVelocity = getReferenceVelocity();
+    float motorValue = motorController(referenceVelocity);
+    pc.printf("Motorvalue is %.2f \r \n", motorValue);
+    pc.printf("Motor direction is %d \r \n", motor1_direction);
+    setMotor1(motorValue);
+    }
+
 void killSwitch(){
     pc.printf("Motors turned off");
     currentState = KILLED;
@@ -64,28 +106,27 @@
     motor1_direction = !motor1_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()
     {
-    motor1_direction = false;
+    pc.printf("Main function");
+    motor1_direction = false; // False = clockwise rotation
     motor1_pwm.period(pwmPeriod);//T=1/f 
     sw2.fall(&killSwitch);
     sw3.fall(&turnMotorsOn);
     button1.rise(&M1switchDirection);
     pc.baud(115200);
-    encoderTicker.attach(readEncoder, 0.1);
+    encoderTicker.attach(readEncoder, 2);
+    controllerTicker.attach(measureAndControl, 0.1);
       
-    pc.printf("Encoder ticker attached and baudrate set");
-    
-    while(true){
-            switch(currentState){
-                case KILLED:
-                    motor1_pwm.write(0.0);
-                    break;
-                case ACTIVE:
-                    motor1_pwm.write(pot.read()); // Motor speed proportional to pot meter                
-                    break;
-            }  
-        }
-    
+    pc.printf("Encoder ticker attached and baudrate set");    
     }