Diff: main.cpp

Revision:

0:6c0f87177797

Child:

2:684d5cf2f683

diff -r 000000000000 -r 6c0f87177797 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Oct 16 13:51:22 2017 +0000
@@ -0,0 +1,51 @@
+#include "mbed.h"
+#include "QEI.h"
+AnalogIn PotMeter(A0);
+InterruptIn Button(PTA4);
+ 
+PwmOut MotorThrottle1(D5);
+PwmOut MotorThrottle2(D6);
+DigitalOut MotorDirection1(D4);
+DigitalOut MotorDirection2(D7);
+
+QEI Encoder(D12,D13,NC,32);
+
+Serial pc(PTB17,PTB16);
+
+void ButtonPress(){
+    MotorDirection1 = !MotorDirection1;
+}
+
+
+int main() {
+    pc.baud(115200);
+    float speed = 0.0f;
+    int pos = 0;
+    int pos_ref = 0;
+    int temp_ref = 0;
+    int pos_prev = 0;
+    pc.printf("startup");
+    Button.rise(&ButtonPress);
+    
+    while(true){
+        //pc.printf("PWM: %f, POS: %i, REF: %i\r\n",speed,pos,pos_ref);
+        pos = Encoder.getPulses();
+        temp_ref =(int) 10000*PotMeter.read();
+        if(fabsf(temp_ref-pos_ref)>50){
+            pos_ref = temp_ref;
+        }
+        speed = ((float)(pos-pos_ref))/10000.0f;
+        if (speed<0.0f){
+            MotorDirection1=1;
+            speed = -speed;
+        }
+        else{
+            MotorDirection1=0;
+        }
+        if(speed<0.01f){
+            speed=0;
+        }
+        MotorThrottle1.write(speed);
+        pos_prev=pos;
+    }
+}