Diff: main.cpp

Revision:

4:836d7f9ac0ca

Parent:

3:120fbef23c17

Child:

5:9651c3f7602b

--- a/main.cpp	Thu Nov 02 10:00:46 2017 +0000
+++ b/main.cpp	Thu Nov 02 11:29:50 2017 +0000
@@ -24,12 +24,12 @@
 float e_int2 = 0;
 
 double pi = 3.14159265359;
-double SetPx = 38;     //Setpoint position x-coordinate from changePosition (EMG dependent)
-double SetPy = 30;     //Setpoint position y-coordinate from changePosition (EMG dependent)
+double SetPx = 0.38;     //Setpoint position x-coordinate from changePosition (EMG dependent)
+double SetPy = 0.30;     //Setpoint position y-coordinate from changePosition (EMG dependent)
 double q1 = 0;          //Reference position q1 from calibration (only the first time)
 double q2 = (pi/2);       //Reference position q2 from calibration (only the first time)
-const double L1 = 30;  //Length arm 1
-const double L2 = 38;  //Length arm 2
+const double L1 = 0.30;  //Length arm 1
+const double L2 = 0.38;  //Length arm 2
 double K = 1;           //Spring constant for movement end-joint to setpoint
 double B1 = 1;          //Friction coefficient for motor 1
 double B2 = 1;          //Friction coefficient for motot 2
@@ -42,7 +42,7 @@
 double cc;
 double a;
 double aa;
-
+bool autodemo_done = false;      //automatische demo stand =0
 
 //tweede motor
 AnalogIn potMeter1(A2);
@@ -50,6 +50,7 @@
 DigitalOut M2D(D4);
 Encoder motor2(D9,D8,true);
 
+
 void RKI()
 {
     p=sin(q1)*L1;
@@ -78,18 +79,18 @@
     double Potmeterwaarde1 = potMeter1.read();
 
     if (Potmeterwaarde2>0.6) {
-        SetPx++;    // hoe veel verder gaat hij? 1 cm? 10 cm?
+        SetPx += 0.001;    // hoe veel verder gaat hij? 1 cm? 10 cm?
     }
     else if (Potmeterwaarde2<0.4) {
-        SetPx--;
+        SetPx -= 0.001;
     }
     else
     {}
     if (Potmeterwaarde1>0.6) {
-        SetPy++;
+        SetPy += 0.001;
     }
     else if (Potmeterwaarde1<0.4) {
-        SetPy--;
+        SetPy -= 0.001;
     }
     else
     {}
@@ -205,10 +206,34 @@
     return Huidigepositie2;             // huidige positie = current position
 }
 
+void Autodemo_or_demo()
+{
+    if (autodemo_done == 0)
+    {
+    AutoSetpointRobotForward ();       //verander de se
+    MeasureAndControl ();
+    AutoSetpointRobotHome ();
+    MeasureAndControl ();   
+    AutoSetpointRobotDown ();
+    MeasureAndControl ();
+    AutoSetpointRobotHome ();
+    MeasureAndControl ();
+    autodemo_done = true;
+    }
+        
+    else if (autodemo_done == 1)
+    {
+    SetpointRobot();
+    MeasureAndControl ();
+    }
+    
+}
+    
+    
+    
 void MeasureAndControl(void)
 {
-    // RKI aanroepen
-    SetpointRobot();
+     // RKI aanroepen
     RKI();
     
     // hier the control of the control system
@@ -230,7 +255,7 @@
 int main()
 {
     M1E.period(PwmPeriod);
-    Treecko.attach(&MeasureAndControl, Ts);   //Elke 1 seconde zorgt de ticker voor het runnen en uitlezen van de verschillende 
+    Treecko.attach(&Autodemo_or_demo, Ts);   //Elke 1 seconde zorgt de ticker voor het runnen en uitlezen van de verschillende 
                                             //functies en analoge signalen. Veranderingen worden elke 1 seconde doorgevoerd.
     pc.baud(115200);