Diff: main.cpp

Revision:

15:65f295a49a4b

Parent:

14:54cbd8f0efe4

Child:

16:000f2ebbd16c

--- a/main.cpp	Thu Nov 02 15:43:46 2017 +0000
+++ b/main.cpp	Thu Nov 02 17:43:05 2017 +0000
@@ -38,13 +38,13 @@
 
 // Constants EMG ----------------------- Start
 double X;
-double X_maxTreshold = 480;
+double X_maxTreshold = 450;
 double X_minTreshold = 20;
 const double X_Callibrate  = 300;
 
 double Y;
-double Y_maxTreshold = 480;
-double Y_minTreshold = 20;
+double Y_maxTreshold = 450;
+double Y_minTreshold = 0;
 const double Y_Callibrate  = 300;
 
 
@@ -52,9 +52,8 @@
 const double pi = 3.1415926535897;
 float   Pwmperiod = 0.001f;
 int     potmultiplier = 600;    // Multiplier for the pot meter reference which is normally between 0 and 1
-const float gearM1 = 6.2;
-const double   gainM1 = 1/38.17;       // encoder pulses per degree theta
-const double   gainM2 = 1/100.8;       // pulses per mm
+const double   gainM1 = 1/29.17;       // encoder pulses per degree theta
+const double   gainM2 = 1/105.0;       // pulses per mm
 
 double motor1;
 double motor2;
@@ -149,6 +148,7 @@
    // X = potMeter1 * potmultiplier;  //--------- for Potmerter use
     
 // -- Potmeter use ----------------------------------- 
+   
     if (potMeter1 < 0.3)              
             {
                 X = X-0.5;
@@ -161,9 +161,9 @@
             {
                 X = X;
             }
-
+    
     
-    /*
+    ///*
     double in1 = emg1.read();
     double in2 = emg2.read();
     
@@ -176,13 +176,13 @@
             }
         else if (RA > 1.5)
             {
-                X = X-0.1;
+                X = X-0.4;
             }
         else
             {
-                X = X+0.1;
+                X = X+0.4;
             }
-    */
+   // */
     
     if (X >= X_maxTreshold){
         X = X_maxTreshold;
@@ -213,6 +213,7 @@
     //Y = potMeter2 * potmultiplier; //--------- for Potmerter use
  
 // ---- Potmeter Use--------------------------   
+    
     if (potMeter2 < 0.3)
             {
                 Y = Y-0.5;
@@ -226,8 +227,8 @@
                 Y = Y;
             }
 
-    
-    /*
+   
+    // /*
     double in3 = emg3.read();
     double in4 = emg4.read();
     
@@ -240,13 +241,13 @@
             }
         else if (LA > 1.5)
             {
-                Y = Y-0.1;
+                Y = Y-0.4;
             }
         else
             {
-                Y = Y+0.1;
+                Y = Y+0.4;
             }
-    */        
+   // */        
     if (Y >= Y_maxTreshold){
         Y = Y_maxTreshold;
         }
@@ -290,7 +291,7 @@
     double countM1 = Encoder1.getPosition();
     
     pc.baud(115200);
-    pc.printf("\r counts encoder: %f\n",countM1);
+    //pc.printf("\r counts encoder: %f\n",countM1);
     
     return pos_M1;       
 }
@@ -309,8 +310,9 @@
         }
         
     double pos_M2 = gainM2 *Encoder2.getPosition(); // current position for the radius;
+    double countM2 = Encoder2.getPosition();
     pc.baud(115200);
-   // pc.printf("\r R1 = %f, pos_m2 = %f\n", R1,pos_m2);
+    //pc.printf("\r counts encoder: %f\n",countM2);
     return pos_M2;
 }
 //-----------------------------------------------------END
@@ -336,6 +338,7 @@
 
     pc.baud(115200);
     //pc.printf("\r DesPosition(X,Y):(%f,%f), posError(dTheta, dError):(%f,%f), (delta1,delta2):(%f,%f)\n",x,y, dTheta ,dRadius,delta1, delta2);
+    pc.printf("\r DesPosition(X,Y):(%f,%f)\n", x,y);
     //pc.printf("\r pos(M1,M2):(%f,%f)\n", pos_M1, pos_M2);
     
     //motor1PWM = motor1;
@@ -365,8 +368,8 @@
     }
     
     motor1 = abs(delta1)/1000.0;
-        if(motor1 >= 0.20) {
-        motor1 = 0.20;
+        if(motor1 >= 0.10) {
+        motor1 = 0.10;
       //pc.baud(115200);
       //pc.printf("\r val motor1: %f\n", motor1);
     }
@@ -399,7 +402,7 @@
         motor2 = 0.50;
     }
     
-    motor1PWM = motor1 + 0.80;
+    motor1PWM = motor1 + 0.90;
     motor2PWM = motor2 + 0.50;
     
     //pc.printf("\r delta(1,2):(%f,%f)\n", delta1,delta2);