Diff: main.cpp

Revision:

3:53fb8bd0a448

Parent:

2:58ec7347245e

Child:

4:93c4e826d11d

--- a/main.cpp	Wed Oct 31 08:04:10 2018 +0000
+++ b/main.cpp	Wed Oct 31 11:47:36 2018 +0000
@@ -3,7 +3,9 @@
 #include "MODSERIAL.h"
 #include "QEI.h"
 #include "HIDScope.h"
+#include "BiQuad.h"
 
+InterruptIn buttoncal(D2);
 DigitalOut gpo(D0);
 DigitalOut led(LED_RED);
 AnalogIn pot1(A4);                  //POORTEN VERANDEREN
@@ -24,8 +26,8 @@
 MODSERIAL pc(USBTX, USBRX);
 
 //HIDSCOPE          //Oppassen waar we HIDSCOPE aanroepen want nu voor meerdere dingen
-HIDScope    scope(4);
-Ticker      scopeTimer;
+//HIDScope    scope(4);
+//Ticker      scopeTimer;
 
 Ticker      EMGTicker;
 
@@ -57,8 +59,8 @@
 int n = 100;            // Zelfde waarde als PrevErrorarray
 double q1Ref = 1.0;     //VOOR TESTEN
 double q2Ref;
-double xRef;
-double yRef;
+double xRef = 40.0;
+double yRef = 40.0;
 double q1Pos;
 double q2Pos;
 double PIDerrorq1;
@@ -79,18 +81,30 @@
 double EMGdata2;
 double EMGdata3;
 double EMGdata4;
-int   count;
+int   count = 0;
 double EMG_filt1;
 double EMG_filt2;
 double EMG_filt3;
 double EMG_filt4;
-double unit_vx;   
+
+double EMG_max1 = 10000.0; 
+double EMG_max2 = 10000.0;
+double EMG_max3 = 10000.0;
+double EMG_max4 = 10000.0;
+
+double unit_vX;   
 double unit_vY;
 
 Ticker Kikker;
 int counter = 0;
 int countstep = 0;
 
+//Demo variabelen
+double vxMax = 3.0;
+double vyMax = 3.0;
+int DemoStage = 0;
+
+
 
 //EMGDINGEN
 
@@ -217,8 +231,8 @@
     unit_vX = filt2kin (EMG_filt1, EMG_filt2, EMG_max1, EMG_max2);
     unit_vY   = filt2kin (EMG_filt3, EMG_filt4, EMG_max3, EMG_max4);
 
-    scope.set(0, unit_Vx);
-    scope.set(1, unit_Vy);
+    //scope.set(0, unit_Vx);
+    //scope.set(1, unit_Vy);
     //scope.set(2, EMG_filt3);
     //scope.set(3, EMG_filt4);
     
@@ -239,14 +253,14 @@
 
 //InverseKinematics
 
-void inverse(double X1, double Y1, double & thetaM1, double & thetaM2)
+void inverse()
 {
     double L1 = 40.0; // %define length of arm 1 attached to gear
-    double L3 = sqrt(pow(X1,2.0)+pow(Y1,2.0));
-    double q3 = atan(Y1/X1);
+    double L3 = sqrt(pow(xRef,2.0)+pow(yRef,2.0));
+    double q3 = atan(yRef/xRef);
     double q4 = 2.0*asin(0.5*L3/L1);
-    thetaM1 = (0.5*3.1416-0.5*q4+q3)*9.0;
-    thetaM2 = (3.1416-thetaM1/9.0-q4)*4.0;
+    q1Ref = (0.5*3.1416-0.5*q4+q3)*9.0;
+    q2Ref = (3.1416-q1Ref/9.0-q4)*4.0;
     }
 
 
@@ -259,23 +273,17 @@
     double deltaX = TickerPeriod*V_max*unit_vX; // imput to delta
     double deltaY = TickerPeriod*V_max*unit_vY;
     
-    static double X1;
-    static double Y1;
-    X1 += deltaX;
-    Y1 += deltaY;
+    xRef += deltaX;
+    yRef += deltaY;
     
-    double THETA1;
-    double THETA2;
+    inverse();
     
-    inverse(X1, Y1, THETA1, THETA2);
-    q1Ref = THETA1;
-    q2Ref = THETA2;
 }
 
 
 void ReadCurrentPosition()  //Update the coordinates of the end-effector q1Pos, q2Pos
 {    
-    q1Pos = encoder1.getPulses()/32/131.25*2*3.1416;        //Position motor 1 in rad
+    q1Pos = -encoder1.getPulses()/32/131.25*2*3.1416     + 3.1416/2.0*9.0;        //Position motor 1 in rad
     q2Pos = encoder2.getPulses()/32/131.25*2*3.1416;        //Position motor 2 in rad 
 }   
 
@@ -350,57 +358,142 @@
 {
         //Motor 1 
         //Keep signal within bounds
-        if (PIDerrorq1 > 1.0){
-            PIDerrorq1 = 1.0;
+        if (PIDerrorq1 > 0.6){  //tijdelijk 0.6, hoort 1.0 te zijn
+            PIDerrorq1 = 0.6;
             }
-        else if (PIDerrorq1 < -1.0){
-            PIDerrorq1 = -1.0;
+        else if (PIDerrorq1 < -0.6){
+            PIDerrorq1 = -0.6;
             }     
         //Direction    
         if (PIDerrorq1 <= 0){
-            Motor1Direction = 0;
+            Motor1Direction = 1;
             Motor1PWM.write(-PIDerrorq1); //write Duty cycle 
         }        
         if (PIDerrorq1 >= 0){
-            Motor1Direction = 1;
+            Motor1Direction = 0;
             Motor1PWM.write(PIDerrorq1); //write Duty cycle 
         }    
         
         //Motor 2 
         //Keep signal within bounds
-        if (PIDerrorq2 > 1.0){
-            PIDerrorq2 = 1.0;
+        if (PIDerrorq2 > 0.6){
+            PIDerrorq2 = 0.6;
             }
-        else if (PIDerrorq2 < -1.0){
-            PIDerrorq2 = -1.0;
+        else if (PIDerrorq2 < -0.6){
+            PIDerrorq2 = -0.6;
             }     
         //Direction           
           
         if (PIDerrorq2 <= 0){
-            Motor2Direction = 0;
+            Motor2Direction = 1;
             Motor2PWM.write(-PIDerrorq2); //write Duty cycle 
         }        
         if (PIDerrorq2 >= 0){
-            Motor2Direction = 1;
+            Motor2Direction = 0;
             Motor2PWM.write(PIDerrorq2); //write Duty cycle 
         }    
 }
 
+
+void HomingSystem()     //Manually place the arm in position q1 = 90degrees, q2 = 0 degrees
+{
+    q1Pos = 3.1416 / 2.0;
+    q2Pos = 0.0;
+    q1Ref = 3.1416 / 2.0;
+    q2Ref = 0.0;
+}
+
+void DemonstrationPath()
+{
+    
+    // Also think about how to move from whatever position to (40,5)
+    if (DemoStage == 0)    //From (40,40) to (40,-5)
+    {
+        if (yRef >0)
+        {
+            yRef = yRef - vyMax * TickerPeriod;
+        }
+        else
+        {
+            DemoStage = 1;
+        }
+    }
+    else if (DemoStage == 1) //From (40,-5) to (65,-5)
+    {
+        if (xRef > 30)
+        {
+            xRef = xRef - vxMax * TickerPeriod;
+        }
+        else
+        {
+            DemoStage = 2;
+        }
+    }
+    else if (DemoStage == 2)
+    {
+        if (yRef < 10)      //From (65,-5) to (65, 10)
+        {
+            yRef = yRef + TickerPeriod;
+        }
+        else
+        {
+            DemoStage = 3;
+        }
+    }
+    else if (DemoStage == 3) //From (65,10) to (40,10)
+    {
+        if (xRef < 40)
+        {
+            xRef = xRef + vxMax * TickerPeriod;
+        }
+        else
+        {
+            DemoStage = 0;  // Repeat moving in the square pattern
+        }
+    }
+}
+
+
+
+void CalibrationModus()
+{
+    EMG();
+    //EMGCalibration();
+}
+
+void DemoModus()   // The ticker should call this function (in the switch statement)
+{    
+    DemonstrationPath();
+    inverse();
+    ReadCurrentPosition();
+    UpdateError();
+    PIDControl();
+    MotorControl();
+    
+    count++;
+    if (count ==400)
+    {
+    pc.printf("q1Pos = %lf, q2Pos = %lf, q1Ref = %lf, q2Ref = %lf,   xRef = %lf, yRef = %lf \n\r", q1Pos, q2Pos, q1Ref, q2Ref, xRef, yRef); 
+    count = 0;
+    }
+}
+
+
 void NormalOperatingModus()
 {
-     EMG()
+     EMG();
      InverseKinematics();
      ReadCurrentPosition();
      UpdateError();
      PIDControl();
      MotorControl();
      
-     scope.set(0, q1Pos);
-     scope.set(1, q1Ref);
+     //scope.set(0, q1Pos);
+     //scope.set(1, q1Ref);
      
-     GainP1 = pot3.read() * 10;
+     GainP1 = 3.0; //pot3.read() * 10;
      GainI1 = 0; //pot2.read() * 10;
-     GainD1 = pot1.read() ;
+     GainD1 = 0.235;//pot1.read() ;
           
      countstep++;
      counter++;
@@ -417,12 +510,40 @@
          
     
 }
+
+//enum states {WaitModus, EMGCalibration, NormalOperatingModus, DemonstrationModus};
  
+/*void StateMachine()
+{
+    switch(State)
+    {
+        case WaitModus: //aanmaken
+                EMG();
+                break;
+        case EMGCalibration:
+             EMGCalibration();           
+                break;
+        case NormalOperatingModus:
+                NormalOperatingModus();
+                break;
+        case DemoModus:
+             DemoModus();      //Goede naam geven..
+                break;
+        default :        
+    }
+}    
+*/                                         
+         
  
 int main()
 {
         pc.baud(115200);
         
+        pc.printf("nog 10 seconden \n\r");
+        wait(7.0);
+        pc.printf("nog 3 seconden \n\r");
+        wait(3.0);
+        
         //BiQuad chains
         bqc1.add( &HP_emg1 ).add( &Notch_emg1 );
         bqc2.add( &HP_emg2 ).add( &Notch_emg2 );
@@ -439,12 +560,14 @@
         Motor1PWM.period(1.0/frequency_pwm); // T = 1/f
         Motor2PWM.period(1.0/frequency_pwm); // T = 1/f     
         
-        Kikker.attach(NormalOperatingModus, TickerPeriod);
-        scopeTimer.attach_us(&scope, &HIDScope::send, 2e4);
+        HomingSystem();
+        
+        Kikker.attach(DemoModus, TickerPeriod);
+        //scopeTimer.attach_us(&scope, &HIDScope::send, 2e4);
         
         //Onderstaande stond in EMG deel, kijken hoe en wat met tickers!!
         // Attach the HIDScope::send method from the scope object to the timer at 50Hz
-        scopeTimer.attach_us(&scope, &HIDScope::send, 5e3);
+        //scopeTimer.attach_us(&scope, &HIDScope::send, 5e3);
         //EMGTicker.attach_us(EMG_filtering, 5e3);
         //.