Diff: main.cpp

Revision:

2:ee6d40dd8fb6

Parent:

1:5ed21efc1d58

diff -r 5ed21efc1d58 -r ee6d40dd8fb6 main.cpp
--- a/main.cpp	Sat Apr 20 13:38:25 2019 +0000
+++ b/main.cpp	Sat Apr 20 17:03:37 2019 +0000
@@ -1,5 +1,7 @@
 #include "mbed.h"
 
+InterruptIn prox1(PA_13);
+InterruptIn prox2(PA_14);
 Ticker stepper;
 //Ticker stepper2;
 
@@ -8,8 +10,8 @@
 SPISlave slave(PA_7, PA_6, PA_5, PA_15); //MOSI MISO CLK CS
 
 //prox detects = 0
-DigitalIn prox2(PA_14);
-DigitalIn prox1(PA_13);
+//DigitalIn prox2(P/A_14);
+//DigitalIn prox1(PA_13);
 
 //Stepper Motor 1
 DigitalOut PUL_1(D3);
@@ -23,9 +25,10 @@
 float countq3 = 0, countq4 = 0;
 bool moveq3 = false, moveq4 = false;
 bool stop = false;
+float stepq3 = 0.0, stepq4 = 0.0;
+int t = 500;
 
 //drive stepper motor
-
 void drvStepper1(float angle){
     float step = angle - q3;
     step = step*44.4444444;
@@ -79,15 +82,42 @@
     }
 }
 
-void ISR_stepper(){
-    if(moveq3 == true){
-        drvStepper1(q3);
-        moveq3 = false;
+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*(1.0 + 1.0/9.0);
     }
+}
+
+void driveStepper(){
+    //if(moveq3 == true){
+//        stepq3 = stepq3 - q3;
+//        stepq3 = stepq3*(44.0 + 4.0/9.0);
+//        if(countq3 >= stepq3){
+//            moveq3 = false;
+//        }
+//        else{
+//            DR_1 = 1;
+//            PUL_1 = 1;
+//            countq3++;
+//        }
+//    }
+//    else{PUL_1 = 0;}
     if(moveq4 == true){
-        drvStepper2(q4);
-        moveq4 = false;
+        if(countq4 >= stepq4){
+            moveq4 = false;
+        }
+        else{
+            DR_2 = 1;
+            PUL_2 = 1;
+            countq4++;
+        }
     }
+    else{PUL_2 = 0;}
 }
 
 void setHome(){
@@ -109,19 +139,43 @@
     q3 = 0;
 }
 
+void stop_q3(){
+    PUL_1 = 0;
+}
+
+void stop_q4(){
+    PUL_2 = 0;
+}
+
 int main() {
- //   stepper.attach(&ISR_stepper, 0.01);
+    stepper.attach_us(&driveStepper, t);
 //    stepper2.attach_us(&drvStepper2, 20);
-    pc.baud(9600);
+    pc.baud(250000);
+    prox1.rise(&stop_q3);
+    prox2.rise(&stop_q4);
     setHome();
-    printf("q3 = %.2f, q4 = %.2f\n", countq3,countq4);
+    wait(2);
+    countq3 = 0;
+    countq4 = 0;
+    moveq4 = true;
+    convertStep(5.0, 4);
+//    q3 = 100;
     while(1){
-    moveq4 = true;
-    q4 = 10;
+//        printf("%.2f\n",countq3);
+        printf("stepq4 %.2f countq4 %.2f\n",stepq4,countq4);
+//        wait(0.1);
+//        moveq3 = true;
+//        q3 = 10;
+    }
+//    setHome();
+//    printf("q3 = %.2f, q4 = %.2f\n", countq3,countq4);
+//    while(1){
+//    moveq4 = true;
+//    q4 = 10;
 //    printf("done\n");
 //        moveq3 = false;
 //        break;
-    wait(5);    
-    }
+//    wait(5);    
+//    }
 //    drvStepper1(2000);
 }