4年工学基礎研究(前期)で作成したペンプロッタの制御プログラム
Dependencies: SPM_simple mbed trapezoidalMotionCal
Diff: main.cpp
- Revision:
- 0:0eddebdc2c58
- Child:
- 1:3663d0f20136
diff -r 000000000000 -r 0eddebdc2c58 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu Aug 03 06:51:15 2017 +0000 @@ -0,0 +1,219 @@ +#include "mbed.h" +#include "SPM.h" +#include "trapezoidalMotionCal.h" + +//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); + +trapezoidalMotionCal motionCal(200, 200, 4000, 40000, -40000); + +PwmOut servo(PC_6); + +DigitalIn x_originSW(PA_0); +DigitalIn y_originSW(PA_1); + +DigitalOut myled(LED1); +Serial pc(USBTX,USBRX); + +typedef struct { + int32_t x; + int32_t y; +} coordinate_t; + +int task = 0; +int32_t pulse = 0; + + + +void pc_intr(); +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) { + + 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 driveServo(int deg){ + const static int servo_offset =830; + const static float servo_gain =10.0; + + deg=deg*servo_gain+servo_offset; + servo.pulsewidth_us(deg); + + 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.printf("Hello\r\n"); + /* + x_axis.lock(); + y_axis.lock(); + */ + driveServo(120); + + 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); + 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; + } + + } +} + +void linearInterpolation(coordinate_t cStart, coordinate_t cTarg) +{ + coordinate_t delta; + int32_t error = 0; + bool x_inv = false; + bool y_inv = false; + coordinate_t cCurrent = {cStart.x, cStart.y}; + coordinate_t inc = {0, 0}; + uint32_t period_us = 0; + + if(cStart.x > cTarg.x) { + x_inv = true; + cTarg.x = cStart.x * 2 - cTarg.x; + } + if(cStart.y > cTarg.y) { + y_inv = true; + cTarg.y = cStart.y * 2 - cTarg.y; + } + + delta.x = cTarg.x - cStart.x; + delta.y = cTarg.y - cStart.y; + + if(delta.x < delta.y){ + motionCal.setTarg(delta.y); + } + else{ + motionCal.setTarg(delta.x); + } + + while(true) { + inc.x = 0; + inc.y = 0; + 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; + cCurrent.y++; + inc.y = 1; + period_us = motionCal.calStepDelay(cCurrent.y - cStart.y); + } else { + error += delta.y; + 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); + } + + if(inc.x != 0)x_axis.oneStep(x_inv == 0); + if(inc.y != 0)y_axis.oneStep(y_inv == 0); + + wait_us(period_us); + + } + +} + +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); + + } + +}