Diff: main.cpp

Revision:

2:80eb19e9091e

Parent:

1:3663d0f20136

Child:

3:ddb5ca27e94b

Child:

4:76146d1e248a

--- a/main.cpp	Thu Aug 03 14:13:41 2017 +0000
+++ b/main.cpp	Fri Aug 04 00:53:48 2017 +0000
@@ -7,10 +7,12 @@
 #define PEN_DOWN        70
 
 
+//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(PB_1  ,PB_2  ,PB_12 ,PA_11);
+stepMotor y_axis(PA_11  ,PB_12  ,PB_2 ,PB_1);
 
-trapezoidalMotionCal motionCal(200, 200, 4000, 40000, -40000);
+trapezoidalMotionCal motionCal(50, 50, 4000, 10000, -10000);
 
 PwmOut servo(PC_6);
 PwmOut speaker(/*PC_8*/PA_15);
@@ -20,6 +22,7 @@
 
 DigitalOut myled(LED1);
 Serial pc(USBTX,USBRX);
+Serial debugPort(PA_9,PA_10);
 
 typedef struct {
     int32_t x;
@@ -106,16 +109,22 @@
 
 int main()
 {
+    coordinate_t runningPosition = {0};
+    int runningPenMotion = 0;
     
     pc.baud(115200);
+    debugPort.baud(115200);
     
     pc.attach(pcIntr, Serial::RxIrq);
-
+    
+    wait_ms(1000);
+    
     pc.printf("Hello\r\n");
-    /*
+    debugPort.printf("Hello Debug Port\r\n");
+    
     x_axis.lock();
     y_axis.lock();
-    */
+    
     driveServo(PEN_UP);
     /*
     while(1){
@@ -142,27 +151,28 @@
     
     while(1) {
         
-        q_sound();
         while(job == 0);
         
         if(nextMode != 0){ // move mode
-            linearInterpolation(currentPosition, nextPosition);
-            currentPosition = nextPosition;
-            wait_ms(10);
+            runningPosition = nextPosition;
             job = 0;
             sendRequest();
+            linearInterpolation(currentPosition, runningPosition);
+            currentPosition = runningPosition;
+            wait_ms(10);
         }
         else{ // pen mode
-            if(penMotion != 0){
+            runningPenMotion = penMotion;
+            job = 0;
+            sendRequest();
+            if(runningPenMotion != 0){
                 driveServo(PEN_DOWN);
-                wait_ms(50);
+                wait_ms(100);
             }
             else{
                 driveServo(PEN_UP);
-                wait_ms(50);
+                wait_ms(100);
             }
-            job = 0;
-            sendRequest();
         }
         
     }
@@ -190,6 +200,10 @@
     delta.x = cTarg.x - cStart.x;
     delta.y = cTarg.y - cStart.y;
     
+    if((delta.x == 0)&&(delta.y == 0)){
+        return;
+    }
+    
     if(delta.x < delta.y){
         motionCal.setTarg(delta.y);
     }
@@ -201,46 +215,58 @@
         inc.x = 0;
         inc.y = 0;
         if(delta.x == 0){
-            cCurrent.y++;
-            inc.y = 1;
-            if(cCurrent.y == cTarg.y){
+            if(cCurrent.y < cTarg.y){
+                cCurrent.y++;
+                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);
         }
         else if(delta.y == 0){
-            cCurrent.x++;
-            inc.x = 1;
-            if(cCurrent.x == cTarg.x){
+            if(cCurrent.x < cTarg.x){
+                cCurrent.x++;
+                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);
         }
-        else if(delta.y >= delta.x) {
+        else if(delta.y > delta.x) {
+            if(cCurrent.y < cTarg.y){
+                cCurrent.y++;
+                inc.y = 1;
+            }
+            else{
+                debugPort.printf("%d, %d : %d\r\n", cCurrent.x, cCurrent.y, error);
+                break;
+            }
             error += delta.x;
-            if(error > delta.y) {
+            if(error >= delta.y) {
                 error -= delta.y;
                 cCurrent.x++;
                 inc.x = 1;
             }
-            if(cCurrent.y == cTarg.y){
+            period_us = motionCal.calStepDelay(cCurrent.y - cStart.y);
+        } else {
+            if(cCurrent.x < cTarg.x){
+                cCurrent.x++;
+                inc.x = 1;
+            }
+            else{
+                debugPort.printf("%d, %d : %d\r\n", cCurrent.x, cCurrent.y, error);
                 break;
             }
-            cCurrent.y++;
-            inc.y = 1;
-            period_us = motionCal.calStepDelay(cCurrent.y - cStart.y);
-        } else {
             error += delta.y;
-            if(error > delta.x) {
+            if(error >= delta.x) {
                 error -= delta.x;
                 cCurrent.y++;
                 inc.y = 1;
             }
-            if(cCurrent.x == cTarg.x){
-                break;
-            }
-            cCurrent.x++;
-            inc.x = 1;
             period_us = motionCal.calStepDelay(cCurrent.x - cStart.x);
         }
         
@@ -250,7 +276,6 @@
         wait_us(period_us);
         
     }
-
 }
 
 void trapezoid(int32_t targSteps)