Diff: main.cpp

Revision:

3:2d45e3d0b0f0

Parent:

2:6f5f300f0569

Child:

4:28d71b0a29aa

diff -r 6f5f300f0569 -r 2d45e3d0b0f0 main.cpp
--- a/main.cpp	Thu Oct 03 14:56:22 2019 +0000
+++ b/main.cpp	Fri Oct 04 12:21:53 2019 +0000
@@ -9,7 +9,7 @@
 DigitalOut ledg(LED_GREEN);
 DigitalOut ledb(LED_BLUE);
 PwmOut led1(D10);
-DigitalIn button1(D2);
+DigitalIn button1(D11);
 AnalogIn potmeter(A0);
 DigitalIn sw(SW2);
 MODSERIAL pc(USBTX, USBRX);
@@ -17,10 +17,15 @@
 DigitalIn encB(D12);
 QEI encoder(D13,D12,NC,64,QEI::X4_ENCODING);
 
-DigitalOut motor1Direction(D4);
-FastPWM motor1Power(D5);
-DigitalOut motor2Direction(D7);
-FastPWM motor2Power(D6);
+DigitalOut motor2Direction(D4);
+FastPWM motor2Power(D5);
+DigitalOut motor1Direction(D7);
+FastPWM motor1Power(D6);
+
+Ticker motorTicker;
+
+const float maxVelocity = 7.958701; // 76 RPM to rad/s
+const double PWM_freq = 1e-6;
 
 int counts; // Encoder counts
 float degreein; // Angle of DC motor shaft input (before gearbox)
@@ -28,25 +33,47 @@
 float degreeout; // Angle of motor shaft output (after gearbox)
 float gearratio= 131.25; // Gear ratio of gearbox
 
+float getRefVelocity()
+{
+    float refVelocity;
+
+    if (button1) {
+        refVelocity = potmeter.read() * maxVelocity;
+    } else {
+        refVelocity = potmeter.read() * maxVelocity * -1;
+    }
+    return refVelocity;
+}
+
+void motorControl()
+{
+    float potValue = potmeter.read();
+    motor1Power.pulsewidth(potValue * PWM_freq);
+}
+
 int main()
 {
     pc.baud(115200);
     pc.printf("\r\nStarting...\r\n\r\n");
-    
-    motor1Power.period(1e-3);
-    
+
+    motor1Power.period(PWM_freq);
+
+    motorTicker.attach(motorControl, 0.01);
+
+    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);
         
-        motor1Power.pulsewidth(potValue * 1e-3);
-        
+        // motor1Power.pulsewidth(potValue * PWM_freq);
+
         wait(0.5);
     }
 }