File content as of revision 0:187c4874bf41:

#include "mbed.h"
#include "EC.h"
#include "SpeedController.h"
#define RESOLUTION 500

DigitalOut led(LED1);
CAN can1(PB_5,PB_13);
/*
0 start/stop
1 to 4 , 6 to 9
5 stop
10 clock move
11 not clock move
12 speed change
*/
Ec2multi ec[]= {
    Ec2multi(PC_5,PB_2,RESOLUTION),
    Ec2multi(PA_11,PB_1,RESOLUTION),
    Ec2multi(PB_12,PB_15,RESOLUTION),
    Ec2multi(PC_4,PB_14,RESOLUTION)
};  //2逓倍用class

Ec2multi ecXY[]= {
    Ec2multi(PC_6,PB_8,RESOLUTION),
    Ec2multi(PC_8,PB_9,RESOLUTION)
};

SpeedControl motor[]= {
    SpeedControl(PA_5,PC_7,50,ec[0]),
    SpeedControl(PC_9,PA_1,50,ec[1]),
    SpeedControl(PA_10,PB_4,50,ec[2]),
    SpeedControl(PA_9,PA_7,50,ec[3])
};

DigitalIn button(USER_BUTTON);

class CAN_ticker
{
public:
    CAN_ticker():x(0)
    {
        data[0]=0;
    }
    void canmsg_read()
    {
        CANMessage msg;
        if(can1.read(msg)) {
            if(msg.id == 1) {
                x=(short)(msg.data[0]);
            }
        }
    }
    int get_xCAN()
    {
        return x;
    }
private:
    char data[0];
    int x;
};
//手動出力
double canOmega[4]= {
    0,0,0,0
};
void ledturn()
{
    led=1;
    wait(0.04);
    led=0;
}
void calOmega_CAN(int canx)
{
    static double a=0.1;
    static int count=0;
    switch(canx) {
        case 1:
            canOmega[0]=0;
            canOmega[1]=a;
            canOmega[2]=0;
            canOmega[3]=-a;
            ledturn();
            break;
        case 2:
            canOmega[0]=a*1.41;
            canOmega[1]=a*1.41;
            canOmega[2]=-a*1.41;
            canOmega[3]=-a*1.41;
            ledturn();
            break;
        case 3:
            canOmega[0]=a;
            canOmega[1]=0;
            canOmega[2]=-a;
            canOmega[3]=0;
            ledturn();
            break;
        case 4:
            canOmega[0]=-a*1.41;
            canOmega[1]=a*1.41;
            canOmega[2]=a*1.41;
            canOmega[3]=-a*1.41;
            ledturn();
            break;
        case 5:
            for(int i=0; i<4; i++) {
                canOmega[i]=0;
            }
            break;
        case 6:
            canOmega[0]=a*1.41;
            canOmega[1]=-a*1.41;
            canOmega[2]=-a*1.41;
            canOmega[3]=a*1.41;
            ledturn();
            break;
        case 7:
            canOmega[0]=0;
            canOmega[1]=-a;
            canOmega[2]=0;
            canOmega[3]=a;
            ledturn();
            break;
        case 8:
            canOmega[0]=-a*1.41;
            canOmega[1]=-a*1.41;
            canOmega[2]=a*1.41;
            canOmega[3]=a*1.41;
            ledturn();
            break;
        case 9:
            canOmega[0]=-a;
            canOmega[1]=0;
            canOmega[2]=a;
            canOmega[3]=0;
            ledturn();
            break;
        case 10:
            for(int i=0; i<4; i++) {
                canOmega[i]=a;
            }
            break;
        case 11:
            for(int i=0; i<4; i++) {
                canOmega[i]=-a;
            }
            break;
        case 12:
            wait(0.2);
            if(count==1) {
                a=0.1;
                count=0;
            } else {
                a=0.05;
                count=1;
            }
            ledturn();
            break;

    }
    for(int i=0; i<4; i++) {
        if(canOmega[i]>0) {
            canOmega[i]=canOmega[i]*0.8;
        }
        motor[i].turn(canOmega[i]);
    }
}

//ticker に入れる関数
Ticker canTicker;

CAN_ticker canx;
void ticker_CanRead()
{
    canx.canmsg_read();
}

int main()
{
    can1.frequency(1000000);
    printf("start\r\n");
    motor[0].setEquation(0.008031,-0.022300,-0.008839,-0.016290);
    motor[1].setEquation(0.008878,-0.016622,-0.009702,-0.015806);
    motor[2].setEquation(0.008637,-0.016537,-0.009397,-0.012159);
    motor[3].setEquation(0.008096,-0.014822,-0.008801,-0.016645);

    motor[0].setDutyLimit(0.4);
    motor[1].setDutyLimit(0.4);
    motor[2].setDutyLimit(0.4);
    motor[3].setDutyLimit(0.4);

    motor[0].setPDparam( 0.01790, 0.00560);
    motor[1].setPDparam( 0.01705, 0.00620);
    motor[2].setPDparam( 0.01790, 0.00620);
    motor[3].setPDparam( 0.01680, 0.00560);

    int canX=20;//can変数
    canTicker.attach(&ticker_CanRead,0.05);//can読み込み
    while(1) {
        printf("waiting\r\n");
        canX=canx.get_xCAN();//０で手\動化
        if(canX==13) {//手動化
            wait(1);
            canX=5;
            while(1) {
                printf("go\r\n");
                calOmega_CAN(canX);
                if(canX==13) {
                    wait(0.5);
                    break;
                }
                if(canX==14) {
                    printf("reset\r\n");
                    NVIC_SystemReset();
                }
                canX=canx.get_xCAN();
            }
        }
    }
}