Test to control motors

Dependencies:   HIDScope_friso MODSERIAL QEI mbed

Revision:
0:42272569c45a
Child:
2:8fb37a4587f1
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Oct 19 12:48:20 2016 +0000
@@ -0,0 +1,80 @@
+#include "mbed.h"
+#include "MODSERIAL.h"
+#define SERIAL_BAUD 115200
+#include "QEI.h"
+#include "HIDScope.h"
+
+MODSERIAL pc(USBTX, USBRX);
+QEI Encoder(D12,D13,NC,64,QEI::X4_ENCODING);
+DigitalOut motor1_direction(D4);
+PwmOut motor1_pwm(D5);
+InterruptIn button1(D2);
+AnalogIn potmeter1(A0);
+
+HIDScope scope(1);
+Ticker scopeTimer;
+
+const float timer = 0.01;
+const float timer1 = 0.1;
+const float countspr = 8400;
+const float max_velocity = 8.4;
+const float pi = 3.14159265359;
+int counts;
+int counts_p;
+int pwmvalue;
+float potmeter;
+float angle;
+float referencevelocity;
+float velocity;
+
+void print(){
+    pc.printf("%i\r\n", counts);
+    pc.printf("velocity: %3.3f%\n", velocity);
+    pc.printf("angle: %3.3f%\n", angle);
+}
+
+void flipdirection()    {
+    motor1_direction = !motor1_direction;
+}
+
+void controlmotor()    {
+    potmeter = potmeter1.read();
+    referencevelocity = potmeter*max_velocity;
+    motor1_pwm.pulsewidth(referencevelocity/max_velocity*0.020);
+}
+
+void calculateangle()   {
+    angle = fmod(counts,countspr)/countspr*2*pi;
+}
+
+void derivative()   {
+    velocity = (counts-counts_p)/countspr*2*pi/timer;
+    counts_p = counts;
+}
+
+void scopeSend()
+{
+    scope.set(0,velocity);
+    scope.send();
+}
+    
+int main()
+{
+    motor1_direction = 1;
+    pc.baud(SERIAL_BAUD);
+    Ticker time;
+    Ticker time1;
+    Ticker time2;
+    Ticker time3;
+    time.attach(print, timer1);
+    time1.attach(controlmotor, timer);
+    time2.attach(calculateangle, timer);
+    time3.attach(derivative, timer);
+    button1.rise(&flipdirection);
+    
+    scopeTimer.attach_us(&scopeSend, 1e4);
+    motor1_pwm.period(0.020f); // set pwm period to 20ms
+    while (true) {
+        counts = Encoder.getPulses();
+    }
+}
\ No newline at end of file