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);

    }

}