Diff: main.cpp

Revision:

5:0abb848bb2f9

Parent:

4:76146d1e248a

--- a/main.cpp	Fri Aug 04 04:08:02 2017 +0000
+++ b/main.cpp	Thu Sep 07 06:43:22 2017 +0000
@@ -6,13 +6,14 @@
 #define PEN_UP          90
 #define PEN_DOWN        70
 
+#define RTO_SPEED       500
 
 //stepMotor x_axis(PC_4 ,PB_13 ,PB_14 ,PB_15);
 stepMotor x_axis(PB_15 ,PB_14 ,PB_13 ,PC_4);
 //stepMotor y_axis(PB_1  ,PB_2  ,PB_12 ,PA_11);
 stepMotor y_axis(PA_11  ,PB_12  ,PB_2 ,PB_1);
 
-trapezoidalMotionCal motionCal(50, 50, 4000, 10000, -10000);
+trapezoidalMotionCal motionCal(50, 500, 4000, 10000, -10000);
 
 PwmOut servo(PC_6);
 PwmOut speaker(/*PC_8*/PA_15);
@@ -29,9 +30,6 @@
     int32_t y;
 } coordinate_t;
 
-int task = 0;
-int32_t pulse = 0;
-
 int job = 0;
 int nextMode = 0;
 int penMotion = 0; // 0: up 1: down
@@ -61,6 +59,8 @@
     
     char temp = pc.getc();
     
+    if(temp == 0xEA)exit(-1);
+    
     if(((temp >> 5) & 0x07) != RXCount){
         RXCount = 0;
     }
@@ -92,6 +92,16 @@
     job = 1;
 }
 
+void return2origin(){
+    int32_t RTO_stepPeriod = 1000000 / RTO_SPEED;
+    
+    while((x_originSW.read() == 0)||(y_originSW.read() == 0)){
+        wait_us(RTO_stepPeriod);
+        if(x_originSW.read() == 0)x_axis.oneStep(false);
+        if(y_originSW.read() == 0)y_axis.oneStep(false);
+    }
+    
+}
 
 void sendRequest(){
     pc.putc(0x01);
@@ -117,7 +127,7 @@
     
     pc.attach(pcIntr, Serial::RxIrq);
     
-    wait_ms(1000);
+    wait_ms(100);
     
     debugPort.printf("Hello Debug Port\r\n");
     
@@ -126,6 +136,10 @@
     
     driveServo(PEN_UP);
     
+    wait_ms(500);
+    
+    return2origin();
+    
     sendRequest();
     
     while(1) {
@@ -139,6 +153,7 @@
             linearInterpolation(currentPosition, runningPosition);
             currentPosition = runningPosition;
             wait_ms(10);
+            debugPort.printf("moved [%d,%d]\r\n", runningPosition.x, runningPosition.y);
         }
         else{ // pen mode
             runningPenMotion = penMotion;
@@ -147,13 +162,14 @@
             if(runningPenMotion != 0){
                 driveServo(PEN_DOWN);
                 wait_ms(100);
+                debugPort.printf("pen down\r\n");
             }
             else{
                 driveServo(PEN_UP);
                 wait_ms(100);
+                debugPort.printf("pen up\r\n");
             }
         }
-        
     }
 }
 
@@ -199,7 +215,6 @@
                 inc.y = 1;
             }
             else{
-                debugPort.printf("%d, %d : %d\r\n", cCurrent.x, cCurrent.y, error);
                 break;
             }
             period_us = motionCal.calStepDelay(cCurrent.y - cStart.y);
@@ -210,7 +225,6 @@
                 inc.x = 1;
             }
             else{
-                debugPort.printf("%d, %d : %d\r\n", cCurrent.x, cCurrent.y, error);
                 break;
             }
             period_us = motionCal.calStepDelay(cCurrent.x - cStart.x);
@@ -221,7 +235,6 @@
                 inc.y = 1;
             }
             else{
-                debugPort.printf("%d, %d : %d\r\n", cCurrent.x, cCurrent.y, error);
                 break;
             }
             error += delta.x;
@@ -237,7 +250,6 @@
                 inc.x = 1;
             }
             else{
-                debugPort.printf("%d, %d : %d\r\n", cCurrent.x, cCurrent.y, error);
                 break;
             }
             error += delta.y;
@@ -257,34 +269,6 @@
     }
 }
 
-void trapezoid(int32_t targSteps)
-{
-    int revMode = 0;
-
-    uint32_t period_us = 0;
-    uint32_t steps = 0;
-
-    if(targSteps < 0) {
-        revMode = 1;
-        targSteps *= -1;
-    }
-    
-    motionCal.setTarg(targSteps);
-
-    while (steps <= targSteps) {
-
-        period_us = motionCal.calStepDelay(steps);
-
-        steps += 1;
-
-        x_axis.oneStep(revMode != 1);
-
-        wait_us(period_us);
-
-    }
-}
-
-
 void beep(int T_us,int t_ms){
     
     if(T_us==0 || t_ms==0)return;