Diff: main.cpp

Revision:

26:bfb1ae203c11

Parent:

25:1c17ab3967c4

Child:

27:2d9f172c66ad

--- a/main.cpp	Fri Nov 03 12:17:04 2017 +0000
+++ b/main.cpp	Mon Nov 06 09:57:31 2017 +0000
@@ -167,7 +167,7 @@
 double Rsx = 0.38;       //Reference x-component of the setpoint radius
 double Rsy = 0.30;       //Reference y-component of the setpoint radius
 double refP = 0;      //Reference position motor 1
-double refP2 = 0.5*pi; //Reference position motor 2
+double refP2 = 0; //Reference position motor 2
 double Rex = cos(q1)*L1 - sin(q2)*L2;   //The x-component of the end-effector radius 
 double Rey = sin(q1)*L1 + cos(q2)*L2;   //The y-component of the end-effector radius
 double R1x = 0;                         //The x-component of the joint 1 radius
@@ -278,7 +278,7 @@
         RESTMEANRT = emgSUMRT/Timescalibration; //determine the mean rest value
         RESTMEANLT = emgSUMLT/Timescalibration; //determine the mean rest value
     }
-    if(Timescalibration>2000 && Timescalibration<6000)
+    if(Timescalibration>2000 && Timescalibration<3000)
     {
         pc.printf("maximum linker biceps \r\n");
         led = 1;
@@ -304,7 +304,7 @@
     }    
     }
     
-    if(Timescalibration>6000 && Timescalibration<10000)
+    if(Timescalibration>3000 && Timescalibration<4000)
     {
         pc.printf(" maximum rechter biceps \r\n");
         /*led = 1;
@@ -334,7 +334,7 @@
     }    
     }
     
-    if(Timescalibration>10000 && Timescalibration<14000)
+    if(Timescalibration>4000 && Timescalibration<5000)
     {
         pc.printf("maximum linker triceps \r\n");
         led = 1;
@@ -368,7 +368,7 @@
     }    
     }
     
-    if(Timescalibration>14000 && Timescalibration<18000)
+    if(Timescalibration>5000 && Timescalibration<6000)
     {
         pc.printf("maximum rechter triceps");
     emgNotchRT = NFRT.step(emgRT.read() );  
@@ -382,7 +382,7 @@
     }    
     }
     
-    if(Timescalibration>18000)
+    if(Timescalibration>6000)
     {
          pc.printf("calibratie afgelopen");
          State = SelectDevice; 
@@ -410,7 +410,7 @@
     
     int maxwaarde = 4096;                   // = 64x64
     refP = (((0.5*pi) - q1)/(2*pi))*maxwaarde;
-    refP2 = (((-pi) + q1 - q2)/(2*pi))*maxwaarde;    //Get reference positions was eerst 0.5*pi
+    refP2 = (( q1 + q2)/(2*pi))*maxwaarde;    //Get reference positions was eerst 0.5*pi
 }
 
 void SetpointRobot()
@@ -551,15 +551,19 @@
     if (RBF>0.6) {
         Rsx +=0.001;    // hoe veel verder gaat hij? 1 cm? 10 cm?
     }
+    else {}
     if (RTF>0.6) {
         Rsx -=0.001;
     }
+    else {}
     if (LBF>0.6) {
         Rsy +=0.001;
     }
+    else {}
     if (LTF>0.6) {
         Rsy -=0.001;
     }
+    else {}
     pc.printf("goalx = %i, goaly = %i\r\n",goalx, goaly);
 } 
 
@@ -609,7 +613,7 @@
                 State=EMG;
             }
             if (button==0) {
-                State=Demo;
+                State=Demonstration;
             }*/
             break;
         case EMG: //Aansturen met EMG
@@ -619,7 +623,7 @@
             //RKI --> output refP van motor
             MeasureAndControl();
             break;
-         case Demo: // Aansturen met toetsenbord
+         case Demonstration: // Aansturen met toetsenbord
             break;
         }
 }