Diff: main.cpp

Revision:

8:7dab565a208e

Parent:

5:2ae500da8fe1

Child:

9:e8cc37a94fec

--- a/main.cpp	Mon Oct 07 14:22:05 2019 +0000
+++ b/main.cpp	Mon Oct 14 14:00:10 2019 +0000
@@ -1,35 +1,31 @@
 #include "mbed.h"
+#include "HIDScope.h"
+#include "BiQuad.h"
 #include "MODSERIAL.h"
 #include "FastPWM.h"
 #include "QEI.h"
 #include <math.h>
 
+#include "Motor_Control.h"
+
 DigitalIn button1(D12);
 AnalogIn pot2(A0);
-Ticker john;
+Ticker measurecontrol;
 DigitalOut motor1Direction(D7);
 FastPWM motor1Velocity(D6);
-Serial pc(USBTX, USBRX);
+MODSERIAL pc(USBTX, USBRX);
 QEI Encoder(D8,D9,NC,8400);
-volatile double frequency = 17000.0;
-volatile double period_signal = 1.0/frequency;
-float vel = 0.0f;
-float referencevelocity;
-float motorvalue2;
-double Kp = 17.5;
-int counts;
-float position1 = 0;
-float position2;
+
+float PI = 3.1415926535897932384626433832795028841971693993;
 float timeinterval = 0.001;
-float measuredvelocity;
 
 // README
-// positive referencevelocity corresponds to a counterclockwise motion
 
 
 //P control implementation (behaves like a spring)
 double P_controller(double error)
 {
+    double Kp = 17.5;
     //Proportional part:
     double u_k = Kp * error;
     
@@ -37,56 +33,25 @@
     return u_k;
 }
 
-//get the measured velocity
-double getmeasuredvelocity()
-{   
-    counts = Encoder.getPulses();
-    counts = counts % 8400;
-    position2 = counts/8400*2*3.141592;
-    measuredvelocity = (position2-position1)/timeinterval; 
-    position1 = position2;
+void nothing(){// Do nothing
 }
 
-//get the reference of the velocity: positive or negative
-double getreferencevelocity()
-{
-    referencevelocity = -1.0 + 2.0*pot2.read();
-    return referencevelocity;
-}   
-
-//send value to motor
-void sendtomotor(float motorvalue)
-{   
-    motorvalue2 = motorvalue;
-    vel = fabs(motorvalue);
-    vel = vel > 1.0f ? 1.0f : vel;   
-    motor1Velocity = vel;
-
-    motor1Direction = (motorvalue > 0.0f);
-}
-
-// function to call reference velocity, measured velocity and controls motor with feedback
-void measureandcontrol(void)
-{
-    float referencevelocity = getreferencevelocity();
-    float measuredvelocity = getmeasuredvelocity();
-    sendtomotor(referencevelocity);
-}
 int main()
 {
+    double frequency = 17000.0;
+    double period_signal = 1.0/frequency;
     pc.baud(115200);
-    pc.printf("Starting...\r\n");
+    pc.printf("Starting...\r\n\r\n");
     motor1Velocity.period(period_signal);
-    john.attach(measureandcontrol, timeinterval);
-    while(true)
+    measurecontrol.attach(measureandcontrol, timeinterval);
+    bool aa = true;
+    while(aa)
     {
-        wait(0.5);
-        //pc.printf("velocity = %f\r\n", vel);
-        //pc.printf("motor1Direction = %i\r\n", (int)motor1Direction);
-        //pc.printf("motorvalue2 = %f\r\n", motorvalue2);
-        pc.printf("number of counts %i\r\n", counts);
-        pc.printf("measured velocity is %f\r\n", measuredvelocity);
-        pc.printf("position1 = %f\r\n",position1);
-        pc.printf("position2 = %f\r\n",position2);
+        char c = pc.getcNb();
+        aa = c == 'c' ? false : true;
     }
-}
\ No newline at end of file
+    pc.printf("Programm stops running\r\n");
+    sendtomotor(0);
+    measurecontrol.attach(nothing,10000);
+    return 0;
+}