Diff: main.cpp

Revision:

9:31c8de7ac6fe

Parent:

8:83b2ab57cf0d

Child:

10:37698a6a1ea7

--- a/main.cpp	Thu Nov 02 15:22:38 2017 +0000
+++ b/main.cpp	Thu Nov 02 15:31:35 2017 +0000
@@ -71,9 +71,7 @@
     
     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("q1 = %f, q2 = %f, Motor1Set = %f, Motor2Set = %f \r\n", q1, q2, Motor1Set, Motor2Set);
+
 }
 
 void SetpointRobot()
@@ -83,19 +81,19 @@
     bool knop1 = Knopje1;
     bool knop2 = Knopje2;
 
-    if (Potmeterwaarde2>0.6) {
-        SetPx += 0.001;    // hoe veel verder gaat hij? 1 cm? 10 cm?
+    if  (knop1 == false){        //(Potmeterwaarde2>0.6) {
+        SetPx += 0.01;    // hoe veel verder gaat hij? 1 cm? 10 cm?
     }
-    else if (Potmeterwaarde2<0.4) {
-        SetPx -= 0.001;
+    else if  (knop1 == true){  //(Potmeterwaarde2<0.4) {
+                                //SetPx -= 0.001;
     }
     else
     {}
-    if (Potmeterwaarde1>0.6) {
-        SetPy += 0.001;
+    if (knop2 == false){         //(Potmeterwaarde1>0.6) {
+        SetPy += 0.01;
     }
-    else if (Potmeterwaarde1<0.4) {
-        SetPy -= 0.001;
+    else if (knop2 ==true){    //(Potmeterwaarde1<0.4) {
+                                //SetPy -= 0.001;
     }
     else
     {}
@@ -271,7 +269,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);
+        pc.printf("Setpointx = %f, Setpointy = %f, Motor1Set = %f, Motor2Set = %f, error1 = %f, error2 = %f \r\n", 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);