Diff: main.cpp

Revision:

22:02a9b5914cc7

Parent:

21:9307dc9be4f7

Child:

23:08255478f6cd

--- a/main.cpp	Fri Nov 03 03:14:33 2017 +0000
+++ b/main.cpp	Fri Nov 03 04:28:04 2017 +0000
@@ -6,7 +6,7 @@
 #include "MODSERIAL.h"
 
 //State Machine en calibratie
-enum States {Cal1, Cal2, CalEMG, SelectDevice, EMG, Rest, Demo};
+enum States {Cal1, Cal2, CalEMG, SelectDevice, EMG, Rest, Demo, STOP};
 States State;
 int Counter;
 bool Position_controller_on;
@@ -14,6 +14,7 @@
 double value;
 double home1;
 DigitalIn button (D1);
+bool automode=false;
 
 //Encoder/motor
 double Huidigepositie1;
@@ -385,7 +386,7 @@
     if(Timescalibration>6000)
     {
          pc.printf("calibratie afgelopen");
-         State = SelectDevice; 
+         State = Demo; 
     }
    // pc.printf("maxi waarde = %f emgfinal = %f \r\n",maxi,emgfinal);
    //}
@@ -413,69 +414,6 @@
     refP2 = (((-pi) + q1 - q2)/(2*pi))*maxwaarde;    //Get reference positions was eerst 0.5*pi
 }
 
-void changePosition ()    // DIT MOET NOG HEEL ERG GETUNED WORDEN !!!
-{
-    if (RBF>0.5) {
-        //goalx++;    // hoe veel verder gaat hij? 1 cm? 10 cm?
-        Rsx += 0.001;
-    }
-    else if (RTF>0.5) {
-        //goalx--;
-        Rsx -= 0.001;
-    }
-    else {}
-    if (LBF>0.5) {
-        //goaly++;
-        Rsy +=0.001;
-    }
-    else if (LTF>0.5) {
-        //goaly--;
-        Rsy -= 0.001;
-    }
-    else {}
-    pc.printf("goalx = %f, goaly = %f\r\n",Rsx, Rsy);
-} 
-
-void SetpointRobot()
-{    
-    double Potmeterwaarde2 = potMeter2.read();
-    double Potmeterwaarde1 = potMeter1.read();
-    
-    if  (Potmeterwaarde2>0.6) {
-        Rsx += 0.001;                    //het gaat telkens 1 mm verder wanneer de potmeter boven de 0.6 staat
-    }
-    else if  (Potmeterwaarde2<0.4) {
-        Rsx -= 0.001;                    //het gaat telkens 1 mm terug wanneer de potmeter onder de 0.4 staat
-    }
-    else {                               //de x-waarde van de setpoint verandert niet
-        }
-
-    if (Potmeterwaarde1>0.6) {           //het gaat telkens 1 mm verder wanneer de potmeter boven de 0.6 staat
-        Rsy += 0.001;
-    }
-    else if (Potmeterwaarde1<0.4) {      //het gaat telkens 1 mm terug wanneer de potmeter onder de 0.4
-        Rsy -= 0.001;                       
-    }
-    else {                               //de y-waarde van de setpoint verandert niet
-        }
-}
-
-double GetReferencePosition() 
-{
-    double Potmeterwaarde = potMeter2.read(); //naam moet universeel worden
-    int maxwaarde = 4096;                   // = 64x64
-    double refP = Potmeterwaarde*maxwaarde;
-    return refP;                            // value between 0 and 4096 
-}
-
-double GetReferencePosition2() 
-{
-    double potmeterwaarde2 = potMeter1.read();
-    int maxwaarde2 = 4096;                   // = 64x64
-    double refP2 = potmeterwaarde2*maxwaarde2;
-    return refP2;                            // value between 0 and 4096 
-}
-   
 double FeedBackControl(double error, double &e_prev, double &e_int)   // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking)
 {
     double kp = 0.0015;                               // kind of scaled.
@@ -549,6 +487,197 @@
     }
 }
 
+void changePosition ()    // DIT MOET NOG HEEL ERG GETUNED WORDEN !!!
+{
+    if (RBF>0.5) {
+        //goalx++;    // hoe veel verder gaat hij? 1 cm? 10 cm?
+        Rsx += 0.001;
+    }
+    else if (RTF>0.5) {
+        //goalx--;
+        Rsx -= 0.001;
+    }
+    else {}
+    if (LBF>0.5) {
+        //goaly++;
+        Rsy +=0.001;
+    }
+    else if (LTF>0.5) {
+        //goaly--;
+        Rsy -= 0.001;
+    }
+    else {}
+    pc.printf("goalx = %f, goaly = %f\r\n",Rsx, Rsy);
+}
+
+void DemoPotmeter ()
+{
+     double Potmeterwaarde2 = potMeter2.read();
+        double Potmeterwaarde1 = potMeter1.read();
+    
+        if  (Potmeterwaarde2>0.6) {
+            Rsx += 0.001;                    //het gaat telkens 1 mm verder wanneer de potmeter boven de 0.6 staat
+        }
+        else if  (Potmeterwaarde2<0.4) {
+            Rsx -= 0.001;                    //het gaat telkens 1 mm terug wanneer de potmeter onder de 0.4 staat
+        }
+        else {                               //de x-waarde van de setpoint verandert niet
+        }
+
+        if (Potmeterwaarde1>0.6) {           //het gaat telkens 1 mm verder wanneer de potmeter boven de 0.6 staat
+            Rsy += 0.001;
+        }
+        else if (Potmeterwaarde1<0.4) {      //het gaat telkens 1 mm terug wanneer de potmeter onder de 0.4
+            Rsy -= 0.001;                       
+        }
+        else {                               //de y-waarde van de setpoint verandert niet
+        }
+         RKI();
+    // hier the control of the 1st control system
+    //double refP = GetReferencePosition();                    //KOMT UIT RKI
+    double Huidigepositie = motor1.getPosition(); 
+    double error = (refP - Huidigepositie);// make an error
+    double motorValue = FeedBackControl(error, e_prev, e_int);
+    SetMotor1(motorValue);
+    // hier the control of the 2nd control system
+    //double refP2 = GetReferencePosition2(); 
+    double Huidigepositie2 = motor2.getPosition(); 
+    double error2 = (refP2 - Huidigepositie2);// make an error
+    double motorValue2 = FeedBackControl2(error2, e_prev2, e_int2);
+    SetMotor2(motorValue2);
+    pc.printf("refP = %f, huidigepos = %f, motorvalue = %f, refP2 = %f, huidigepos2 = %f, motorvalue2 = %f \r\n", refP, Huidigepositie, motorValue, refP2, Huidigepositie2, Huidigepositie2);
+}
+
+void SetpointRobot()
+{   
+    if (automode == 0)
+    {
+        Treecko.detach();
+        for (int n = 0; n < 100; n++)
+        {
+            Rsy -= 0.0002;
+            RKI();
+            
+            // hier the control of the control system
+            double Huidigepositie = motor1.getPosition(); 
+            double error = (refP - Huidigepositie);// make an error
+            double motorValue = FeedBackControl(error, e_prev, e_int);
+            SetMotor1(motorValue);
+
+            double Huidigepositie2 = motor2.getPosition(); 
+            double error2 = (refP2 - Huidigepositie2);// make an error
+            double motorValue2 = FeedBackControl2(error2, e_prev2, e_int2);
+            SetMotor2(motorValue2);
+            //pc.printf("encoder 2 = %f\r\n",Huidigepositie2);
+            pc.printf("refP2 = %f, Huidigepositie2 = %f, error = %f, motorValue2 = %f \r\n", refP2, Huidigepositie2, error2, motorValue2);
+        }
+        
+        for (int n = 0; n < 100; n++)
+        {
+            Rsy += 0.0002;
+            RKI();
+            // hier the control of the control system
+            double Huidigepositie = motor1.getPosition(); 
+            double error = (refP - Huidigepositie);// make an error
+            double motorValue = FeedBackControl(error, e_prev, e_int);
+            SetMotor1(motorValue);
+
+            double Huidigepositie2 = motor2.getPosition(); 
+            double error2 = (refP2 - Huidigepositie2);// make an error
+            double motorValue2 = FeedBackControl2(error2, e_prev2, e_int2);
+            SetMotor2(motorValue2);
+            //pc.printf("encoder 2 = %f\r\n",Huidigepositie2);
+            pc.printf("refP2 = %f, Huidigepositie2 = %f, error = %f, motorValue2 = %f \r\n", refP2, Huidigepositie2, error2, motorValue2);
+        }
+        
+        for (int n = 0; n < 100; n++)
+        {
+            Rsx += 0.0002;
+            RKI();
+            // hier the control of the control system
+            double Huidigepositie = motor1.getPosition(); 
+            double error = (refP - Huidigepositie);// make an error
+            double motorValue = FeedBackControl(error, e_prev, e_int);
+            SetMotor1(motorValue);
+
+            double Huidigepositie2 = motor2.getPosition(); 
+            double error2 = (refP2 - Huidigepositie2);// make an error
+            double motorValue2 = FeedBackControl2(error2, e_prev2, e_int2);
+            SetMotor2(motorValue2);
+            //pc.printf("encoder 2 = %f\r\n",Huidigepositie2);
+            pc.printf("refP2 = %f, Huidigepositie2 = %f, error = %f, motorValue2 = %f \r\n", refP2, Huidigepositie2, error2, motorValue2);
+        }
+    
+        for (int n = 0; n < 100; n++)
+        {
+            Rsx -= 0.0002;
+            RKI();
+            // hier the control of the control system
+            double Huidigepositie = motor1.getPosition(); 
+            double error = (refP - Huidigepositie);// make an error
+            double motorValue = FeedBackControl(error, e_prev, e_int);
+            SetMotor1(motorValue);
+
+            double Huidigepositie2 = motor2.getPosition(); 
+            double error2 = (refP2 - Huidigepositie2);// make an error
+            double motorValue2 = FeedBackControl2(error2, e_prev2, e_int2);
+            SetMotor2(motorValue2);
+            //pc.printf("Motor2.getposition() 2 = %f\r\n",Huidigepositie2);
+            pc.printf("refP2 = %f, Huidigepositie2 = %f, error = %f, motorValue2 = %f \r\n", refP2, Huidigepositie2, error2, motorValue2);
+        }
+    
+        automode = 1;
+    }
+    
+    else
+    {
+        Treecko.attach(DemoPotmeter,Ts);
+    } 
+ 
+}
+
+/*void SetpointRobot()
+{    
+    double Potmeterwaarde2 = potMeter2.read();
+    double Potmeterwaarde1 = potMeter1.read();
+    
+    if  (Potmeterwaarde2>0.6) {
+        Rsx += 0.001;                    //het gaat telkens 1 mm verder wanneer de potmeter boven de 0.6 staat
+    }
+    else if  (Potmeterwaarde2<0.4) {
+        Rsx -= 0.001;                    //het gaat telkens 1 mm terug wanneer de potmeter onder de 0.4 staat
+    }
+    else {                               //de x-waarde van de setpoint verandert niet
+        }
+
+    if (Potmeterwaarde1>0.6) {           //het gaat telkens 1 mm verder wanneer de potmeter boven de 0.6 staat
+        Rsy += 0.001;
+    }
+    else if (Potmeterwaarde1<0.4) {      //het gaat telkens 1 mm terug wanneer de potmeter onder de 0.4
+        Rsy -= 0.001;                       
+    }
+    else {                               //de y-waarde van de setpoint verandert niet
+        }
+} */
+
+double GetReferencePosition() 
+{
+    double Potmeterwaarde = potMeter2.read(); //naam moet universeel worden
+    int maxwaarde = 4096;                   // = 64x64
+    double refP = Potmeterwaarde*maxwaarde;
+    return refP;                            // value between 0 and 4096 
+}
+
+double GetReferencePosition2() 
+{
+    double potmeterwaarde2 = potMeter1.read();
+    int maxwaarde2 = 4096;                   // = 64x64
+    double refP2 = potmeterwaarde2*maxwaarde2;
+    return refP2;                            // value between 0 and 4096 
+}
+   
+
+
 void MeasureAndControl(void)
 {
     //SetpointRobot(); 
@@ -628,6 +757,8 @@
             CalibrationEMG();   //calculates average EMGFiltered at rest and measures max signal EMGFiltered.
             break;    
         case SelectDevice: //Looks at the difference between current position and home. Select aansturen EMG or buttons
+            wait(2.0);
+            pc.printf("besturingsmethode kiezen \r\n");
             if (button==1) {
                 State=EMG;
             }
@@ -643,6 +774,13 @@
             MeasureAndControl();
             break;
          case Demo: // Aansturen met toetsenbord
+         pc.printf("Automatische DEMO \r\n");
+            SetpointRobot();
+            State=STOP;
+            break;
+        case STOP: // Demo runt zichtzelf
+            SetMotor1(0);
+            SetMotor2(0);
             break;
         }
 }