4年工学基礎研究(前期)で作成したペンプロッタの制御プログラム
Dependencies: SPM_simple mbed trapezoidalMotionCal
main.cpp
- Committer:
- Akito914
- Date:
- 2017-08-03
- Revision:
- 1:3663d0f20136
- Parent:
- 0:0eddebdc2c58
- Child:
- 2:80eb19e9091e
File content as of revision 1:3663d0f20136:
#include "mbed.h" #include "SPM.h" #include "trapezoidalMotionCal.h" #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); DigitalOut myled(LED1); Serial pc(USBTX,USBRX); typedef struct { int32_t x; 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 coordinate_t nextPosition = {0}; 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 beep(int T_us,int t_ms); void pi(int times); void q_sound(void); void ans_sound(void); void piroro(void); // 受信割込みハンドラ 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){ const static int servo_offset =830; const static float servo_gain =10.0; deg=deg*servo_gain+servo_offset; servo.pulsewidth_us(deg); return; } int main() { pc.baud(115200); pc.attach(pcIntr, Serial::RxIrq); pc.printf("Hello\r\n"); /* x_axis.lock(); y_axis.lock(); */ 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) { q_sound(); while(job == 0); if(nextMode != 0){ // move mode linearInterpolation(currentPosition, nextPosition); currentPosition = nextPosition; wait_ms(10); 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(); } } } 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.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; } 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); } } 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); }