4年工学基礎研究(前期)で作成したペンプロッタの制御プログラム
Dependencies: SPM_simple mbed trapezoidalMotionCal
Diff: main.cpp
- Revision:
- 1:3663d0f20136
- Parent:
- 0:0eddebdc2c58
- Child:
- 2:80eb19e9091e
diff -r 0eddebdc2c58 -r 3663d0f20136 main.cpp --- 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); +} + + + + +