Diff: MotorControl.cpp

Revision:

9:219bfc575aa4

Parent:

8:10d6f6ad03c8

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MotorControl.cpp	Fri Oct 26 09:36:17 2018 +0000
@@ -0,0 +1,76 @@
+#include "mbed.h"
+#include "QEI.h"
+#include "MODSERIAL.h"
+#include "math.h"
+
+
+MODSERIAL pc(USBTX,USBRX);
+Ticker TickerReadPots;
+Ticker TickerGetCounts;
+QEI Encoder1(D10,D11,NC,32);
+QEI Encoder2(D12,D13,NC,32);
+InterruptIn PrintCounts(SW2);
+
+
+DigitalOut DirectionPin1(D4);
+DigitalOut DirectionPin2(D7);
+PwmOut PwmPin1(D5);
+PwmOut PwmPin2(D6);
+AnalogIn potmeter1(A1);
+AnalogIn potmeter2(A0);
+
+
+volatile float Duty1;
+volatile float Duty2;
+volatile float MotorSignal1;
+volatile float MotorSignal2;
+volatile int counts1;
+volatile int counts2;
+
+// function for reading potentiometers and set to dutycycle
+void ReadPots(void)
+{
+    Duty1 = potmeter1.read(); // read value potentiometer 1 
+    Duty2 = potmeter2.read(); // read value potentiometer 2
+ 
+    MotorSignal1 = 2*Duty1 - 1; //scaling potmeter to motor control signal [0 - 1] --> [(-1) - 1]
+    MotorSignal2 = 1 - 2*Duty2; 
+}
+
+// printing counts to pc
+void GetCounts(void)
+{
+ counts1 = Encoder1.getPulses();
+ counts2 = Encoder2.getPulses();
+
+ Encoder1.reset();
+ Encoder2.reset();
+}
+
+void Print()
+{
+ pc.printf("Number counts per second: motor1 = %i , motor2 = %i \r\n",counts1,counts2);
+}
+
+
+int main()
+{
+    pc.baud(115200);
+    pc.printf("Hello World!\r\n");
+    PwmPin1.period_us(60); // 16.66667 kHz (default period is too slow!)
+    TickerReadPots.attach(&ReadPots,0.05); // every 50 milli seconds.
+    TickerGetCounts.attach(&GetCounts,1); //Print amount of counts every second
+    PrintCounts.fall(&Print);
+    while (true) 
+    {
+    // motor 1
+    DirectionPin1 = MotorSignal1 > 0.0f; //either true or false, CW or CCW
+    PwmPin1 = fabs(MotorSignal1); //pwm duty cycle can only be positive, floating point absolute value
+    
+    // motor 2
+    DirectionPin2 = MotorSignal2 > 0.0f; 
+    PwmPin2 = fabs(MotorSignal2); 
+    
+    wait(0.01);  //Do while loop a hundred times per second.
+    }
+}
\ No newline at end of file