Diff: main.cpp

Revision:

2:75b2f713161c

Parent:

1:b862262a9d14

Child:

3:4f281c336a8b

--- a/main.cpp	Wed Sep 04 15:30:13 2019 +0000
+++ b/main.cpp	Mon Oct 07 12:25:49 2019 +0000
@@ -1,23 +1,92 @@
 #include "mbed.h"
-//#include "HIDScope.h"
-//#include "QEI.h"
 #include "MODSERIAL.h"
-//#include "BiQuad.h"
-//#include "FastPWM.h"
+#include "FastPWM.h"
+
+DigitalIn button1(D12);
+AnalogIn pot2(A0);
+Ticker john;
+DigitalOut motor1Direction(D6);
+FastPWM motor1Velocity(D7);
+Serial pc(USBTX, USBRX);
+volatile double frequency = 17000.0;
+volatile double period_signal = 1.0/frequency;
+
+//get the measured velocity
+double getmeasuredvelocity()
+{   
+    float measuredvelocity;
+    measuredvelocity = pot2.read();
+    return measuredvelocity;
+}
+
+//get the reference of the velocity: positive or negative
+double getreferencevelocity()
+{
+    //const float maxvelocity = 8.4;
+    /*volatile float referencevelocity;
+    if (button1.read() == 0)
+    {
+        referencevelocity = pot2.read()*maxvelocity;
+    }
+    else
+    {
+        referencevelocity = -1*pot2.read()*maxvelocity;
+    }*/
+    float referencevelocity = -1.0 + 2.0*pot2.read();
+    //pc.printf("The reference velocity is %f\r\n", referencevelocity);
+    return referencevelocity;
+}   
 
-DigitalOut led(LED_RED);
+float vel = 0.0f;
+//send value to motor
+void sendtomotor(float motorvalue)
+{   
+    /*
+    pc.printf("The motor value is %f\r\n", motorvalue);
+    if (motorvalue >= 0)
+    {
+        motor1Direction = 1;
+        pc.printf("hello");
+    }
+    else 
+    {
+        motor1Direction = 0;
+        pc.printf("wow");
+    }
+    if (fabs(motorvalue)>1)
+    {
+        motor1Velocity = 1;
+    }
+    else
+    {
+        motor1Velocity = fabs(motorvalue);
+    }*/
+    vel = fabs(motorvalue);
+     vel = vel > 1.0f ? 1.0f : vel;   
+    motor1Velocity = vel;
 
-MODSERIAL pc(USBTX, USBRX);
+    motor1Direction = (motorvalue > 0.0f);
+    //pc.printf("The motor value is %f\r\n", motorvalue);
+    //pc.printf("The motor direction is %s\r\n", motor1Direction ? "links" : "rechts");
+}
 
+// function to call reference velocity, measured velocity and controls motor with feedback
+void measureandcontrol(void)
+{
+    float referencevelocity = getreferencevelocity();
+    float measuredvelocity = getmeasuredvelocity();
+//    float motorvalue = feedbackcontrol(referencevelocity - measuredvelocity);
+    sendtomotor(referencevelocity);
+}
 int main()
 {
     pc.baud(115200);
-    pc.printf("\r\nStarting...\r\n\r\n");
-    
-    while (true) {
-        
-        led = !led;
-        
-        wait_ms(500);
+    pc.printf("Starting...\r\n");
+    motor1Velocity.period(period_signal);
+    john.attach(measureandcontrol, 0.001f);
+    while(true)
+    {
+        wait(0.5);
+        pc.printf("%f\r\n", vel);
     }
-}
+}
\ No newline at end of file