Diff: main.cpp

Revision:

7:1d3eefa00e20

Parent:

6:9943f69ff668

Child:

8:83b2ab57cf0d

--- a/main.cpp	Thu Nov 02 13:09:51 2017 +0000
+++ b/main.cpp	Thu Nov 02 13:37:38 2017 +0000
@@ -19,9 +19,11 @@
 const float Ts = 0.1;                   // tickettijd/ sample time
 float e_prev = 0; 
 float e_int = 0;
+float error1;
 float PwmPeriod2 = 1.0/5000.0;           //set up of PWM periode (5000 Hz, want 5000 periodes in 1 seconde)
 float e_prev2 = 0; 
 float e_int2 = 0;
+float error2;
 
 double pi = 3.14159265359;
 double SetPx = 0.38;     //Setpoint position x-coordinate from changePosition (EMG dependent)
@@ -42,7 +44,7 @@
 double cc;
 double a;
 double aa;
-bool autodemo_done = false;      //automatische demo stand =0
+bool autodemo_done = 1;      //automatische demo stand =0
 
 //tweede motor
 AnalogIn potMeter1(A2);
@@ -65,12 +67,11 @@
     int maxwaarde = 4096;                   // = 64x64
     
     
-    Motor1Set = (q1/(2*pi))*maxwaarde;           //Calculate the desired motor1 angle from the desired joint positions
-    Motor2Set = -((pi-q2-q1)/(2*pi))*maxwaarde;   //Calculate the desired motor2 angle from the desired joint positions
+    Motor1Set = -(q1/(2*pi))*maxwaarde;           //Calculate the desired motor1 angle from the desired joint positions
+    Motor2Set = ((pi-q2-q1)/(2*pi))*maxwaarde;   //Calculate the desired motor2 angle from the desired joint positions
     
-    pc.printf("waarde p = %f, waarde pp = %f, a= %f, aa = %f, bb = %f, cc = %f \r\n",p,pp,a,aa,bb,cc);
+    //pc.printf("waarde p = %f, waarde pp = %f, a= %f, aa = %f, bb = %f, cc = %f \r\n",p,pp,a,aa,bb,cc);
     //pc.printf("q1 = %f, q2 = %f, Motor1Set = %f, Motor2Set = %f \r\n", q1, q2, Motor1Set, Motor2Set);
-    //pc.printf("Setpointx = %f, Setpointy = %f \r\n", SetPx, SetPy);
 }
 
 void SetpointRobot()
@@ -206,6 +207,26 @@
     return Huidigepositie2;             // huidige positie = current position
 }
 
+void MeasureAndControl(void)
+{
+     // RKI aanroepen
+    RKI();
+    
+    // hier the control of the control system
+    //float refP = GetReferencePosition(); 
+    float Huidigepositie = Encoder(); 
+    error1 = (Motor1Set - Huidigepositie);// make an error
+    float motorValue = FeedBackControl(error1, e_prev, e_int);
+    SetMotor1(motorValue);
+
+    // hier the control of the control system
+    //float refP2 = GetReferencePosition2(); 
+    float Huidigepositie2 = Encoder2(); 
+    error2 = (Motor2Set - Huidigepositie2);// make an error
+    float motorValue2 = FeedBackControl2(error2, e_prev2, e_int2);
+    SetMotor2(motorValue2);
+}
+
 void Autodemo_or_demo()
 {
     if (autodemo_done == 0)
@@ -232,28 +253,6 @@
     }
     
 }
-    
-    
-    
-void MeasureAndControl(void)
-{
-     // RKI aanroepen
-    RKI();
-    
-    // hier the control of the control system
-    //float refP = GetReferencePosition(); 
-    float Huidigepositie = Encoder(); 
-    float error = (Motor1Set - Huidigepositie);// make an error
-    float motorValue = FeedBackControl(error, e_prev, e_int);
-    SetMotor1(motorValue);
-
-    // hier the control of the control system
-    //float refP2 = GetReferencePosition2(); 
-    float Huidigepositie2 = Encoder2(); 
-    float error2 = (Motor2Set - Huidigepositie2);// make an error
-    float motorValue2 = FeedBackControl2(error2, e_prev2, e_int2);
-    SetMotor2(motorValue2);
-}
 
 
 int main()
@@ -268,6 +267,7 @@
     {
         //wait(0.2);
         float B = motor1.getPosition();
+        pc.printf("Setpointx = %f, Setpointy = %f, Motor1Set = %f, Motor2Set = %f \r\n, error1 = %f, error2 = %f", SetPx, SetPy, Motor1Set, Motor2Set,error1, error2);
         //float positie = B%4096;
         //pc.printf("pos: %d, speed %f, potmeter = %f V, \r\n",motor1.getPosition(), motor1.getSpeed(),(potMeter2.read()*3.3)); //potmeter uitlezen. tussen 0-1. voltage, dus *3.3V
         //pc.printf("q1 = %f, q2 = %f, Motor1Set = %f, Motor2Set = %f \r\n", q1, q2, Motor1Set, Motor2Set);