Diff: main.cpp

Revision:

4:28d71b0a29aa

Parent:

3:2d45e3d0b0f0

Child:

5:54ce02ad7a50

diff -r 2d45e3d0b0f0 -r 28d71b0a29aa main.cpp
--- a/main.cpp	Fri Oct 04 12:21:53 2019 +0000
+++ b/main.cpp	Fri Oct 04 12:58:04 2019 +0000
@@ -23,15 +23,17 @@
 FastPWM motor1Power(D6);
 
 Ticker motorTicker;
+Ticker encoderTicker;
 
 const float maxVelocity = 7.958701; // 76 RPM to rad/s
-const double PWM_freq = 1e-6;
+const float PWM_period = 1e-6;
 
-int counts; // Encoder counts
-float degreein; // Angle of DC motor shaft input (before gearbox)
-float factorin= 360/64; // Convert encoder counts to angle in degrees
-float degreeout; // Angle of motor shaft output (after gearbox)
-float gearratio= 131.25; // Gear ratio of gearbox
+volatile int counts; // Encoder counts
+volatile int countsPrev = 0;
+volatile int deltaCounts;
+
+float factorin = 6.23185/64; // Convert encoder counts to angle in rad
+float gearratio = 131.25; // Gear ratio of gearbox
 
 float getRefVelocity()
 {
@@ -48,7 +50,15 @@
 void motorControl()
 {
     float potValue = potmeter.read();
-    motor1Power.pulsewidth(potValue * PWM_freq);
+    motor1Power.pulsewidth(potValue * PWM_period);
+}
+
+void readEncoder()
+{
+    counts = encoder.getPulses();
+    deltaCounts = counts - countsPrev;
+
+    countsPrev = counts;
 }
 
 int main()
@@ -56,24 +66,26 @@
     pc.baud(115200);
     pc.printf("\r\nStarting...\r\n\r\n");
 
-    motor1Power.period(PWM_freq);
+    motor1Power.period(PWM_period);
 
     motorTicker.attach(motorControl, 0.01);
+    
+    float T_encoder = 0.1;
+    encoderTicker.attach(readEncoder, T_encoder);
 
     motor1Direction = 0;
 
     while (true) {
-        counts = encoder.getPulses(); // Get encoder pulses
-        degreein = counts*factorin;  // Convert encoder data to angle
-        degreeout = degreein/gearratio; // Derived output angle
-        //pc.printf("%f \r\n", degreein); //draaigraden van inputgear en zo ook encoder.
-        pc.printf("%f \r\n", degreeout); // Angle of output
 
         float potValue = potmeter.read(); // Read potmeter
-        pc.printf("Potmeter: %f \r\n", potValue);
+        
+        float angle = counts * factorin / gearratio; // Angle of motor shaft in rad
+        float omega = deltaCounts / T_encoder * factorin / gearratio; // Angular velocity of motor shaft in rad/s
         
-        // motor1Power.pulsewidth(potValue * PWM_freq);
-
+        pc.printf("Potmeter: %f \r\n", potValue);
+        pc.printf("Counts: %i   DeltaCounts: %i\r\n", counts, deltaCounts);
+        pc.printf("Angle:  %f   Omega:       %f\r\n", angle, omega);
+        
         wait(0.5);
     }
 }