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