File content as of revision 6:0a3c58539a2a:

#include "mbed.h"
#include "SPM.h"
#include "trapezoidalMotionCal.h"

#define PEN_SAVEMODE    120
#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, 500, 4000, 10000, -10000);

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);
Serial debugPort(PA_9,PA_10);

typedef struct {
    int32_t x;
    int32_t y;
} coordinate_t;

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 == 0xEA)exit(-1);
    
    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 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);
}

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()
{
    coordinate_t runningPosition = {0};
    int runningPenMotion = 0;
    
    pc.baud(115200);
    debugPort.baud(115200);
    
    pc.attach(pcIntr, Serial::RxIrq);
    
    wait_ms(100);
    
    debugPort.printf("Hello Debug Port\r\n");
    
    x_axis.lock();
    y_axis.lock();
    
    driveServo(PEN_UP);
    
    wait_ms(500);
    
    return2origin();
    
    sendRequest();
    
    while(1) {
        
        while(job == 0);
        
        if(nextMode != 0){ // move mode
            runningPosition = nextPosition;
            job = 0;
            sendRequest();
            linearInterpolation(currentPosition, runningPosition);
            currentPosition = runningPosition;
            wait_ms(10);
            debugPort.printf("moved [%d,%d]\r\n", runningPosition.x, runningPosition.y);
        }
        else{ // pen mode
            runningPenMotion = penMotion;
            job = 0;
            sendRequest();
            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");
            }
        }
    }
}

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 == 0)&&(delta.y == 0)){
        return;
    }
    
    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){
            if(cCurrent.y < cTarg.y){
                cCurrent.y++;
                inc.y = 1;
            }
            else{
                break;
            }
            period_us = motionCal.calStepDelay(cCurrent.y - cStart.y);
        }
        else if(delta.y == 0){
            if(cCurrent.x < cTarg.x){
                cCurrent.x++;
                inc.x = 1;
            }
            else{
                break;
            }
            period_us = motionCal.calStepDelay(cCurrent.x - cStart.x);
        }
        else if(delta.y > delta.x) {
            if(cCurrent.y < cTarg.y){
                cCurrent.y++;
                inc.y = 1;
            }
            else{
                break;
            }
            error += delta.x;
            if(error >= delta.y) {
                error -= delta.y;
                cCurrent.x++;
                inc.x = 1;
            }
            period_us = motionCal.calStepDelay(cCurrent.y - cStart.y);
        } else {
            if(cCurrent.x < cTarg.x){
                cCurrent.x++;
                inc.x = 1;
            }
            else{
                break;
            }
            error += delta.y;
            if(error >= delta.x) {
                error -= delta.x;
                cCurrent.y++;
                inc.y = 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 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);
}