Diff: main.cpp

Revision:

1:3663d0f20136

Parent:

0:0eddebdc2c58

Child:

2:80eb19e9091e

--- a/main.cpp	Thu Aug 03 06:51:15 2017 +0000
+++ b/main.cpp	Thu Aug 03 14:13:41 2017 +0000
@@ -2,13 +2,18 @@
 #include "SPM.h"
 #include "trapezoidalMotionCal.h"
 
-//stepMotor x_axis(PC_4  ,PB_13 ,PB_14 ,PB_15);
+#define PEN_SAVEMODE    120
+#define PEN_UP          90
+#define PEN_DOWN        70
+
+
 stepMotor x_axis(PB_15 ,PB_14 ,PB_13 ,PC_4);
 stepMotor y_axis(PB_1  ,PB_2  ,PB_12 ,PA_11);
 
 trapezoidalMotionCal motionCal(200, 200, 4000, 40000, -40000);
 
 PwmOut servo(PC_6);
+PwmOut speaker(/*PC_8*/PA_15);
 
 DigitalIn x_originSW(PA_0);
 DigitalIn y_originSW(PA_1);
@@ -24,43 +29,69 @@
 int task = 0;
 int32_t pulse = 0;
 
-
+int job = 0;
+int nextMode = 0;
+int penMotion = 0; // 0: up 1: down
+coordinate_t nextPosition = {0};
 
-void pc_intr();
+coordinate_t currentPosition = {0};
+
+void pcIntr();
+void rcdataProcessing(int32_t *RXData);
+void sendRequest();
+
 void driveServo(int deg);
 void linearInterpolation(coordinate_t cStart, coordinate_t cTarg);
 void trapezoid(int32_t targSteps);
 
-// 受信割込みハンドラ
-void pc_intr()
-{
-    char temp = pc.getc();
-
-    switch(temp) {
+void beep(int T_us,int t_ms);
+void pi(int times);
+void q_sound(void);
+void ans_sound(void);
+void piroro(void);
 
-        case ',' :
-            if(task==0) {
-                task=1;
-                pulse=200;
-            }
-            break;
-        case '.' :
-            if(task==0) {
-                task=1;
-                pulse=-200;
-            }
-            break;
-        case 's' :
-            driveServo(120);
-            break;
-        case 'u' :
-            driveServo(100);
-            break;
-        case 'd' :
-            driveServo(60);
-            break;
+// 受信割込みハンドラ
+void pcIntr()
+{
+    static int32_t RXByte[8] = {0};
+    static int32_t RXCount   = 0;
+    
+    char temp = pc.getc();
+    
+    if(((temp >> 5) & 0x07) != RXCount){
+        RXCount = 0;
+    }
+    else{
+        RXByte[RXCount] = temp;
+        
+        if(RXCount < 7){
+            RXCount += 1;
+        }
+        else{ // all data received
+            RXCount = 0;
+            rcdataProcessing(RXByte);
+        }
+    }
+}
 
+
+void rcdataProcessing(int32_t *RXData){
+    
+    if((RXData[0] & 0x08) != 0){ // move mode
+        nextPosition.x = (RXData[1] & 0x0f) | ((RXData[2] & 0x0f) << 4) | ((RXData[3] & 0x0f) << 8);
+        nextPosition.y = (RXData[4] & 0x0f) | ((RXData[5] & 0x0f) << 4) | ((RXData[6] & 0x0f) << 8);
+        nextMode = 1;
     }
+    else{ // pen mode
+        nextMode = 0;
+        penMotion = RXData[0] & 0x01;
+    }
+    job = 1;
+}
+
+
+void sendRequest(){
+    pc.putc(0x01);
 }
 
 void driveServo(int deg){
@@ -73,55 +104,67 @@
     return;
 }
 
-void itp(int32_t x, int32_t y){
-    static coordinate_t cPast = {0,0};
-    coordinate_t cCurrent = {x,y};
-    
-    linearInterpolation(cPast, cCurrent);
-    
-    cPast.x = x;
-    cPast.y = y;
-}
-
 int main()
 {
-    pc.attach(pc_intr, Serial::RxIrq);
+    
+    pc.baud(115200);
+    
+    pc.attach(pcIntr, Serial::RxIrq);
 
     pc.printf("Hello\r\n");
     /*
     x_axis.lock();
     y_axis.lock();
     */
-    driveServo(120);
+    driveServo(PEN_UP);
+    /*
+    while(1){
+        nextPosition.x = 100; nextPosition.y = 200;
+        linearInterpolation(currentPosition, nextPosition);
+        currentPosition = nextPosition;
+        wait_ms(100);
+        nextPosition.x = 100; nextPosition.y = 0;
+        linearInterpolation(currentPosition, nextPosition);
+        currentPosition = nextPosition;
+        wait_ms(100);
+        nextPosition.x = 0; nextPosition.y = 200;
+        linearInterpolation(currentPosition, nextPosition);
+        currentPosition = nextPosition;
+        wait_ms(100);
+        nextPosition.x = 0; nextPosition.y = 0;
+        linearInterpolation(currentPosition, nextPosition);
+        currentPosition = nextPosition;
+        wait_ms(100);
+    }
+    */
+    
+    sendRequest();
     
     while(1) {
-
-        if(task==1) {
-
-            myled = 1;
-            
-            itp(400,  0);
-            wait_ms(10);
-            itp(400,400);
-            wait_ms(10);
-            itp(  0,400);
-            wait_ms(10);
-            itp(  0,  0);
+        
+        q_sound();
+        while(job == 0);
+        
+        if(nextMode != 0){ // move mode
+            linearInterpolation(currentPosition, nextPosition);
+            currentPosition = nextPosition;
             wait_ms(10);
-            itp(400,  0);
-            wait_ms(10);
-            itp(  0,400);
-            wait_ms(10);
-            itp(400,400);
-            wait_ms(10);
-            itp(  0,  0);
-            wait_ms(10);
-            
-            myled = 0;
-
-            task=0;
+            job = 0;
+            sendRequest();
         }
-
+        else{ // pen mode
+            if(penMotion != 0){
+                driveServo(PEN_DOWN);
+                wait_ms(50);
+            }
+            else{
+                driveServo(PEN_UP);
+                wait_ms(50);
+            }
+            job = 0;
+            sendRequest();
+        }
+        
     }
 }
 
@@ -143,7 +186,7 @@
         y_inv = true;
         cTarg.y = cStart.y * 2 - cTarg.y;
     }
-
+    
     delta.x = cTarg.x - cStart.x;
     delta.y = cTarg.y - cStart.y;
     
@@ -157,14 +200,32 @@
     while(true) {
         inc.x = 0;
         inc.y = 0;
-        if(delta.y > delta.x) {
+        if(delta.x == 0){
+            cCurrent.y++;
+            inc.y = 1;
+            if(cCurrent.y == cTarg.y){
+                break;
+            }
+            period_us = motionCal.calStepDelay(cCurrent.y - cStart.y);
+        }
+        else if(delta.y == 0){
+            cCurrent.x++;
+            inc.x = 1;
+            if(cCurrent.x == cTarg.x){
+                break;
+            }
+            period_us = motionCal.calStepDelay(cCurrent.x - cStart.x);
+        }
+        else if(delta.y >= delta.x) {
             error += delta.x;
             if(error > delta.y) {
                 error -= delta.y;
                 cCurrent.x++;
                 inc.x = 1;
             }
-            if(cCurrent.y == cTarg.y)break;
+            if(cCurrent.y == cTarg.y){
+                break;
+            }
             cCurrent.y++;
             inc.y = 1;
             period_us = motionCal.calStepDelay(cCurrent.y - cStart.y);
@@ -175,7 +236,9 @@
                 cCurrent.y++;
                 inc.y = 1;
             }
-            if(cCurrent.x == cTarg.x)break;
+            if(cCurrent.x == cTarg.x){
+                break;
+            }
             cCurrent.x++;
             inc.x = 1;
             period_us = motionCal.calStepDelay(cCurrent.x - cStart.x);
@@ -185,7 +248,7 @@
         if(inc.y != 0)y_axis.oneStep(y_inv == 0);
         
         wait_us(period_us);
-                
+        
     }
 
 }
@@ -215,5 +278,51 @@
         wait_us(period_us);
 
     }
+}
 
+
+void beep(int T_us,int t_ms){
+    
+    if(T_us==0 || t_ms==0)return;
+    
+    speaker.period_us(T_us);
+    speaker.write(0.10);
+    
+    wait_ms(t_ms);
+    
+    speaker.write(0.0);
+    speaker.period_us(100);
+    
+    return;
 }
+
+void pi(int times){
+    int count=0;
+    
+    for(count=0;count<times;count++){
+        beep(379,50);
+        wait_ms(50);
+    }
+    wait_ms(300);
+}
+
+void q_sound(void){
+    beep(478,100);
+    beep(379,100);
+}
+
+void ans_sound(void){
+    beep(379,100);
+    beep(478,100);
+}
+
+void piroro(void){
+    beep(379,100);
+    beep(426,100);
+    beep(478,100);
+}
+
+
+
+
+