![](/media/cache/profiles/1129cb8ba4665189d5a9bd861a2e28b9.jpg.50x50_q85.jpg)
4年工学基礎研究(前期)で作成したペンプロッタの制御プログラム
Dependencies: SPM_simple mbed trapezoidalMotionCal
main.cpp
- Committer:
- Akito914
- Date:
- 2017-08-03
- Revision:
- 0:0eddebdc2c58
- Child:
- 1:3663d0f20136
File content as of revision 0:0eddebdc2c58:
#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); } }