Diff: main.cpp

Revision:

2:6f5f300f0569

Parent:

1:68f74b2ceb7d

Child:

3:2d45e3d0b0f0

--- a/main.cpp	Fri Sep 20 13:56:03 2019 +0000
+++ b/main.cpp	Thu Oct 03 14:56:22 2019 +0000
@@ -3,35 +3,50 @@
 #include "QEI.h"
 #include "MODSERIAL.h"
 //#include "BiQuad.h"
-//#include "FastPWM.h"
+#include "FastPWM.h"
 
 DigitalOut ledr(LED_RED);
 DigitalOut ledg(LED_GREEN);
 DigitalOut ledb(LED_BLUE);
 PwmOut led1(D10);
 DigitalIn button1(D2);
-AnalogIn potmeter(A1);
+AnalogIn potmeter(A0);
 DigitalIn sw(SW2);
 MODSERIAL pc(USBTX, USBRX);
 DigitalIn encA(D13);
 DigitalIn encB(D12);
 QEI encoder(D13,D12,NC,64,QEI::X4_ENCODING);
 
-int counts;
-float degreein;
-float factorin= 5.625;
-float degreeout;
-float gearratio= 131.25;
+DigitalOut motor1Direction(D4);
+FastPWM motor1Power(D5);
+DigitalOut motor2Direction(D7);
+FastPWM motor2Power(D6);
+
+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
+
 int main()
 {
     pc.baud(115200);
     pc.printf("\r\nStarting...\r\n\r\n");
+    
+    motor1Power.period(1e-3);
+    
     while (true) {
-        counts= encoder.getPulses();
-        degreein= counts*factorin;
-        degreeout= degreein/gearratio;
+        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);
-        wait(0.5); 
+        pc.printf("%f \r\n", degreeout); // Angle of output
+        
+        float potValue = potmeter.read(); // Read potmeter
+        pc.printf("Potmeter: %f \r\n", potValue);
+        
+        motor1Power.pulsewidth(potValue * 1e-3);
+        
+        wait(0.5);
     }
 }