Revision 5:d86a0c29dd29, committed 2019-04-21

Comitter:

pbdt1997

Date:

Sun Apr 21 14:06:22 2019 +0000

Parent:

4:fb0905390ebc

Commit message:

just pray

Changed in this revision

diff -r fb0905390ebc -r d86a0c29dd29 main.cpp
--- a/main.cpp	Sat Apr 20 19:32:22 2019 +0000
+++ b/main.cpp	Sun Apr 21 14:06:22 2019 +0000
@@ -17,111 +17,35 @@
 DigitalOut PUL_2(D4);
 DigitalOut DR_2(PC_1);
 
-float q3 = 0, q4 = 0;
-float countq3 = 0, countq4 = 0;
+
+//globals
+double q3 = 0, q4 = 0;
+double q3_count = 0, q4_count = 0;
+double q3_speed = 1, q4_speed = 1;
+double q3_step = 0, q4_step = 0;
 bool moveq3 = false, moveq4 = false;
 bool stop = false;
-float stepq3 = 0.0, stepq4 = 0.0;
-float t = 10000.0;
+double t = 10000.0;
+
+//Functions
 
-//drive stepper motor
-void drvStepper1(float angle){
-    float step = angle - q3;
-    step = step*44.4444444;
-    if(step >= 0){
-        for(int i=0; i<(int)step; i++){
-            DR_1 = 0; //up
-            PUL_1 = 1;
-            wait_ms(1);
-            PUL_1 = 0;
-            wait_ms(1);
-//            countq3++;
-        }
+//conversion funcs
+void convertStep(double angle, char joint){
+    if(joint == 3){
+        q3_step = angle - q3;
+        q3_step = abs(q3_step*(44.444444));
     }
-    else if(step < 0){
-        step = -step;
-        for(int i=0; i<(int)step; i++){
-            DR_1 = 1; //down
-            PUL_1 = 1;
-            wait_ms(1);
-            PUL_1 = 0;
-            wait_ms(1);
-//            countq3--;
-        }
+    else if(joint == 4){
+        q4_step = angle - q4;
+//        q4_step = q4_step*(10.0 + 1.0/9.0);
+        q4_step = abs(q4_step*(11.1111111));
     }
 }
 
-void drvStepper2(float angle){
-   // printf("hello");
-    float step = angle - q4;
-    step = step*11.1111111;
-    if(step >= 0){
-        for(int i=0; i<(int)step; i++){
-            DR_2 = 0; //up
-            PUL_2 = 1;
-            wait_ms(1);
-            PUL_2 = 0;
-            wait_ms(1);
-//            countq4++;
-        }
-    }
-    else if(step < 0){
-        step = -step;
-        for(int i=0; i<(int)step; i++){
-            DR_2 = 1; //down
-            PUL_2 = 1;
-            wait_ms(1);
-            PUL_2 = 0;
-            wait_ms(1);
-//            countq4--;
-        }
-    }
-}
 
-void convertStep(double angle, char joint){
-    if(joint == 3){
-        stepq3 = angle - q3;
-        stepq3 = stepq3*(44.0 + 4.0/9.0);
-    }
-    else if(joint == 4){
-        stepq4 = angle - q4;
-        stepq4 = stepq4*(10.0 + 1.0/9.0);
-    }
-}
+//ISRs//
 
-void driveStepper(){
-    if(moveq4 == true){
-        if(countq4 >= stepq4){
-            moveq4 = false;
-            countq4 = 0;
-            PUL_2 = 0;
-        }
-        else{
-            PUL_2 = !PUL_2;
-            countq4++;
-        }
-    }
-}
-
-void setHome(){
-    while(prox2 == 1){
-        drvStepper2(1);
-    }
-    wait(0.5);
-    drvStepper2(-138.51);
-    wait(0.5);
-    countq4 = 0;
-    q4 = 0;
-    while(prox1 == 1){
-        drvStepper1(-1);
-    }
-    wait(0.5);
-    drvStepper1(18);
-    wait(0.5);
-    countq3 = 0;
-    q3 = 0;
-}
-
+//Proximity sensors
 void stop_q3(){
     PUL_1 = 0;
 }
@@ -130,38 +54,139 @@
     PUL_2 = 0;
 }
 
-void veloControl4(float angle, float speed){
-    stepper.detach();
-    stepper.attach_us(&driveStepper, 1000000/(speed));
+//
+void driveStepper(){
+    if(moveq4 == true){
+        if(q4_count >= q4_step){
+            moveq4 = false;
+            if(DR_2 == 0){
+                q4 += q4_count/11.1111111;
+                }else{
+                q4 -= q4_count/11.1111111;
+                }
+            q4_count = 0;
+            PUL_2 = 0;
+
+        }
+        else{
+            if((uint8_t)q4_count%(uint8_t)q4_speed==0){
+            PUL_2 = !PUL_2;
+            }
+            q4_count++;
+
+        }
+    }
+    if(moveq3 == true){
+        if(q3_count >= q3_step){
+            moveq3 = false;
+            if(DR_1 == 0){
+                q3 += q3_count/44.4444444;
+            }else{
+                q3 -= q3_count/44.4444444;
+            }
+            q3_count = 0;
+            PUL_1 = 0;
+        }
+        else{
+            if((uint8_t)q3_count%(uint8_t)q3_speed==0){
+            PUL_1 = !PUL_1;
+            }
+            q3_count++;
+
+        }
+    }
+}
+
+void drvStepper1(double angle, double speed){
+    q3_speed = speed;
+    if(angle - q3 >= 0){
+        DR_1 = 0;
+    }
+    else{
+        DR_1 = 1;
+    }
+        moveq3 = true;
+    convertStep(angle, 3);
+}
+
+void drvStepper2(double angle, double speed){
+//    stepper.detach();
+//    stepper.attach_us(&driveStepper, 1000000/(speed));
+    q4_speed = speed;
     if(angle - q4 >= 0){
         DR_2 = 0;
-        }
+    }
     else{
         DR_2 = 1;
-        }
+    }
     moveq4 = true;
     convertStep(angle, 4);
-    }
+}
+
+void veloControl(double q3_trgt, double q4_trgt, double v3, double v4){
+    //wait till previous movement stopped
+    while(moveq4||moveq3){
+        }
+    drvStepper1(q3_trgt, v3);
+    drvStepper2(q4_trgt, v4);
+    moveq3 = true;
+    moveq4 = true;
+}
+
+void setHome(){
+    //while(prox2 == 1){
+//        drvStepper2(1, 1000.0);
+//    }
+//    wait(0.5);
+//    drvStepper2(-138.51);
+//    wait(0.5);
+//    q4 = 0;
+//    q4 = 0;
+//    while(prox1 == 1){
+//        drvStepper1(-1);
+//    }
+//    wait(0.5);
+//    drvStepper1(18);
+//    wait(0.5);
+//    q3 = 0;
+//    q3 = 0;
+}
+
+
+
     
 
 int main() {
-    stepper.attach_us(&driveStepper, 10000.0);
+    stepper.attach_us(&driveStepper, 100.0);
     pc.baud(250000);
     //prox1.rise(&stop_q3);
 //    prox2.rise(&stop_q4);
     moveq4 = false;
-    setHome();
-    wait(2);
-    countq3 = 0;
-    countq4 = 0;
+    moveq3 = false;
+//    setHome();
+//    wait(2);
+    q3 = 0;
+    q4 = 0;
     
-    veloControl4(60.0, 100);
-    wait(5);
-    veloControl4(30.0, 1000);
-    while(1){
+    veloControl(10,10,10,10);
+    printf("q3 = %.2f q4 = %.2f\n", q3, q4);
+    while(moveq4||moveq3){
+        printf("numba 1 q3 %.2f q4 %.2f\n",q3_count,q4_count);
+        }
+    printf("q3 = %.2f q4 = %.2f\n", q3, q4);
+    veloControl(-10,-15,10,10);
 
-        printf("stepq4 %.2f countq4 %.2f\n",stepq4,countq4);
-
-    }
+       while(moveq4||moveq3){
+        printf("numba 2 q3 %.2f q4 %.2f\n",q3_count,q4_count);
+        }
+    printf("q3 = %.2f q4 = %.2f\n", q3, q4);
+ //   while(1){
+////        drvStepper2(60.0, 1000);
+////        wait(5);
+////        drvStepper2(30.0, 1000);
+//        printf("q4_count %.2f q4 %.2f\n",q4_count,q4);
+//        wait_ms(10);
+//
+//    }
 
 }