Diff: main.cpp

Revision:

27:3430dfb4c9fb

Parent:

26:b48708ed51ff

Child:

28:aec0d9bdb949

--- a/main.cpp	Mon Oct 29 18:22:56 2018 +0000
+++ b/main.cpp	Tue Oct 30 09:13:59 2018 +0000
@@ -7,8 +7,8 @@
 #include "BiQuad.h"
 
 // Input
-AnalogIn pot1(A1);
-AnalogIn pot2(A2);
+AnalogIn    pot1(A1);
+AnalogIn    pot2(A2);
 InterruptIn button1(D0);
 InterruptIn button2(D1);
 InterruptIn emergencybutton(SW2);  /* This is not yet implemented! 
@@ -16,12 +16,12 @@
 everything will abort as soon as possible
 */
 
-DigitalIn pin8(D8);     // Encoder 1 B
-DigitalIn pin9(D9);     // Encoder 1 A
-DigitalIn pin10(D10);   // Encoder 2 B
-DigitalIn pin11(D11);   // Encoder 2 A
-DigitalIn pin12(D12);   // Encoder 3 B
-DigitalIn pin13(D13);   // Encoder 3 A
+DigitalIn   pin8(D8);     // Encoder 1 B
+DigitalIn   pin9(D9);     // Encoder 1 A
+DigitalIn   pin10(D10);   // Encoder 2 B
+DigitalIn   pin11(D11);   // Encoder 2 A
+DigitalIn   pin12(D12);   // Encoder 3 B
+DigitalIn   pin13(D13);   // Encoder 3 A
 
 // Output
 DigitalOut  pin2(D2);    // Motor 3 direction
@@ -42,83 +42,108 @@
 QEI         Encoder3(D13,D12,NC,4200);      // Counterclockwise motor rotation is the positive direction
 Ticker      motor;
 Ticker      encoders;
+HIDScope    scope(2);
+Ticker      HIDTicker;
+
 
 // Global variables
 const float     pi = 3.14159265358979;
 float           u3 = 0.0;         // Normalised variable for the movement of motor 3
 const float     fCountsRad = 4200.0;
-const double    dt = 0.001;
+const double    dt = 0.5;
 const double    Kp = 17.5;    // given value is 17.5
 const double    Ki = 1.02;  // given value is 1.02
 //const double    Ts = 0.0025; // Sample time in seconds
 
+volatile float  x;
+volatile float  xprev = 0;
+volatile float  y;
+
 // Functions
-void turn1()    // main function, all below functions with an extra tab are called
+void ShowHIDScope()
+{   x = pot1;
+    scope.set(0,x);
+    y = (xprev+x)/2.0;
+    scope.set(1,y);
+    xprev = x;
+    
+    scope.send(); 
+}
+
+/*    //Subfunctions
+    int Counts1input()
+    {   int     counts1;
+        counts1 = Encoder1.getPulses();
+        return  counts1;
+    }
+    int Counts2input()
+    {   int     counts2;
+        counts2 = Encoder2.getPulses();
+        return  counts2;
+    }
+    int Counts3input()
+    {   int     counts3;
+        counts3 = Encoder3.getPulses();
+        return  counts3;
+    }
+
+    float   CurrentAngle(float counts)
+    {   float   angle = ((float)counts*2.0*pi)/fCountsRad;
+        while (angle > pi)
+        {   angle = angle - 2.0*pi;
+        }
+        while (angle < pi)
+        {   angle = angle + 2.0*pi;
+        }
+        return  angle;
+    }
+    
+    double  ErrorCalc(double yref,double CurAngle)
+    {   double  error = yref - CurAngle;
+        return error;
+    }
+            
+    double  Pcontroller(double yref,double CurAngle)
+    {   double  error = ErrorCalc(yref,CurAngle);
+        //double Kp = 50.0*pot1;    // Normalised variable for value of potmeter 1
+        // Proportional part:
+        double  u_k = Kp * error;
+        // Sum all parts and return it
+        //pc.printf("error: %d",error);
+        return  u_k;
+    }
+
+    double  PIcontroller(double yref,double CurAngle)
+    {   double  error = yref - CurAngle;    
+        static double   error_integral = 0; 
+        // Proportional part:
+        double  u_k = Kp * error;
+        // Integral part
+        error_integral = error_integral + error * dt;
+        double  u_i = Ki * error_integral;
+        // Sum all parts and return it
+        return  u_k + u_i;
+    }
+    
+
+void    turn1()    // main function, all below functions with an extra tab are called
 {   
-    double refvalue1 = pi/4;
-    int counts1 = Counts1input();
-    float angle1 = CurrentAngle(counts1);
-    /*if (refvalue1 - angle1 < 0)
+    double  refvalue1 = pi/4;
+    int     counts1 = Counts1input();
+    float   angle1 = CurrentAngle(counts1);
+    double  PIcontr = PIcontroller(refvalue1,angle1);
+    double  error = ErrorCalc(refvalue1,angle1);
+     
+    pin6 = fabs(PIcontr)/pi;
+    if  (error > 0)
     {   pin7 = true;
     }
-    else if(refvalue1 - angle1 > 0)
+    else if (error < 0)
     {   pin7 = false;    
-    }*/
-    pin6 = Pcontroller(refvalue1,angle1); 
+    }
     //pc.printf("%i\r\n",refvalue1);
     //pc.printf("Counts1,2,3: %i       Angle1,2,3: %f  \r\n",counts1,angle1);
 }
-
-    //Subfunctions
-    int Counts1input()
-    {   int counts1;
-        counts1 = Encoder1.getPulses();
-        return counts1;
-    }
-    int Counts2input()
-    {   int counts2;
-        counts2 = Encoder2.getPulses();
-        return counts2;
-    }
-    int Counts3input()
-    {   int counts3;
-        counts3 = Encoder3.getPulses();
-        return counts3;
-    }
-
-    float   CurrentAngle(float counts)
-    {   float angle = ((float)counts*2.0*pi)/fCountsRad;
-        while (angle > 2.0*pi)
-        {   angle = angle - 2.0*pi;
-        }
-        while (angle < -2.0*pi)
-        {   angle = angle + 2.0*pi;
-        }
-        return angle;
-    }
-
-    double  Pcontroller(double yref,double CurAngle)
-    {   double error = yref - CurAngle;
-        //double Kp = 50.0*pot1;    // Normalised variable for value of potmeter 1
-        // Proportional part:
-        double u_k = Kp * error;
-        // Sum all parts and return it
-        return u_k;
-    }
-
-    double PIcontroller(double yref,double CurAngle)
-    {   double error = yref - CurAngle;
-    
-        static double error_integral = 0; 
-        // Proportional part:
-        double u_k = Kp * error;
-        // Integral part
-        error_integral = error_integral + error * dt;
-        double u_i = Ki * error_integral;
-        // Sum all parts and return it
-        return u_k + u_i;
-    }
-
    
 /*double RefVelocity(float pot)
 {   // Returns reference velocity in rad/s. 
@@ -167,11 +192,11 @@
     pin5.period(0.1);
     pin6.period(0.1);
     motor.attach(turn1,dt);
-
+    HIDTicker.attach(ShowHIDScope,dt);
     emergencybutton.rise(Emergency);              //If the button is pressed, stop program
             
     while   (true)
     {   
-        
+        // Nothing here
     }
 }
\ No newline at end of file