4年工学基礎研究(前期)で作成したペンプロッタの制御プログラム
Dependencies: SPM_simple mbed trapezoidalMotionCal
Diff: main.cpp
- Revision:
- 5:0abb848bb2f9
- Parent:
- 4:76146d1e248a
diff -r 76146d1e248a -r 0abb848bb2f9 main.cpp --- 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;