Diff: Motor_tryout.cpp

Revision:

25:eb3204e45d33

Parent:

24:61d03692f92e

Child:

26:432d3519ba86

diff -r 61d03692f92e -r eb3204e45d33 Motor_tryout.cpp
--- a/Motor_tryout.cpp	Fri Oct 25 13:45:39 2019 +0000
+++ b/Motor_tryout.cpp	Tue Oct 29 08:48:48 2019 +0000
@@ -17,6 +17,9 @@
 QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING);
 QEI Encoder3(D2,D3,NC,64,QEI::X4_ENCODING);
 
+float steps1 = 0;
+float steps2 = 0;
+float steps3 = 0;
 int check;
 int quit;
 int limit_pos = 8400;
@@ -29,6 +32,9 @@
 int counts1 = 0;
 int counts2 = 0;
 int counts3 = 0;
+int count1old = 0;
+int countdiff = 0;
+int counting1 = 0;
 
 const float le = 15.0;
 const float f = 37.5;
@@ -60,12 +66,19 @@
 float oldtheta2=0;
 float oldtheta3=0;
 
-Ticker pulses;
-void getpulses()   {
+Ticker pulses1;
+void getpulses1()   {
         counts1 = Encoder1.getPulses();
+        }
+Ticker pulses2;
+void getpulses2()   {
         counts2 = Encoder2.getPulses();
+        }
+Ticker pulses3;
+void getpulses3()   {
         counts3 = Encoder3.getPulses();
         }
+      
         
 float delta_calcangleyz(float x00, float y00, float z00)   {
     float y2 = y00 + le;
@@ -132,9 +145,10 @@
     theta2 = theta2 - oldtheta2;
     theta3 = theta3 - oldtheta3;    
                 
-    float steps1 = anglestep(theta1);
-    float steps2 = anglestep(theta2);
-    float steps3 = anglestep(theta3);
+    steps1 = anglestep(theta1);
+    pc.printf("\n\rsteps1.1 %f", steps1);
+    steps2 = anglestep(theta2);
+    steps3 = anglestep(theta3);
     
     // Set the direction of the motors.   
     if (theta1 < 0) {
@@ -172,46 +186,76 @@
     check2 = true;
     check3 = true; 
     
-    counts1=0;
-    counts2=0;
-    counts3=0;
-    
-    pulses.attach(&getpulses, 1.0/1000);
-    for (int i=0; i<100; i++)  {
-        pc.printf("\n\rcounts1 %i", counts1);
-        wait(0.001);
-        }
+
+ //   for (int i=0; i<100; i++)  {
+ //       pc.printf("\n\rcounts1 %i", counts1);
+ //       wait(0.001);
+ //       }
     
-                
+         pc.printf("\n\rsteps3.1 %f", steps3);
+        pc.printf("\n\rcounts3.1 %i", counts3);
+
+           
     while (check1 || check2 || check3)    {
-    pc.printf("\n\r counts1 %i", counts1);
-    pulses.attach(&getpulses, 1.0/10000);
-         
-         wait(1/1000);
-        if(abs(counts1)>=abs(steps1)) {
- //           pc.printf("\n\r counts1 %i and steps1 %f", counts1, steps1);
-    pc.printf("\n 1 is false");
+        pc.printf("\n\rsteps1 %f, steps2 %f, steps3 %f",steps1, steps2, steps3);
+        pc.printf("\n\rcounts1 %i, counts2 %i, counts3 %i", counts1, counts2, counts3);
+
+    if (steps1 <= 0)  {
+        if(counts1<=steps1) {
+            pulses1.detach();
+            pc.printf("\n 1 is false");
+
             motor1_pwm.write(0);
             check1=false;
-            counts1=0;
             }
-        if (abs(counts2)>=abs(steps2))   {
+        if (counts2<=steps2)   {
+                            pulses2.detach();
             pc.printf("\n 2 is false");
             motor2_pwm.write(0);
             check2=false;
-            counts2=0;
             }
-        if (abs(counts3)>=abs(steps3))  {
+        if (counts3>=abs(steps3))  {
+                            pulses3.detach();
             pc.printf("\n 3 is false");
             motor3_pwm.write(0);
             check3=false;
             //pc.printf("\n\rsteps %f, counts %f", steps3, counts3);
-            counts3=0;
+                    pc.printf("\n\rsteps3 %f", steps3);
+        pc.printf("\n\rcounts3 %i", counts3);
+            }
+        }
+        if (steps1 >= 0)  {
+        if(counts1>=steps1) {
+            pc.printf("\n 1 is false");
+            motor1_pwm.write(0);
+            check1=false;
             }
+        if (counts2>=steps2)   {
+            pc.printf("\n 2 is false");
+            motor2_pwm.write(0);
+            check2=false;
+            }
+        if (counts3<=-steps3)  {
+            pc.printf("\n 3 is false");
+            motor3_pwm.write(0);
+            check3=false;
+            }
+        }
+
     }
+    //Encoder1.reset();
+            wait(1.0);
+        pc.printf("\n\rcounts1 %i, counts2 %i, counts3 %i", counts1, counts2, counts3);
 }
 
 int main(void)  {    
+    pulses1.attach(&getpulses1, 1.0/1000);
+    pulses2.attach(&getpulses2, 1.0/1000);
+    pulses3.attach(&getpulses3, 1.0/1000);
+    Encoder1.reset();
+    Encoder2.reset();
+    Encoder3.reset();
+    
     pc.baud(115200);
         
     char cc = pc.getc();