File content as of revision 3:012e5d7bbfd5:

#include "mbed.h"
#include "ros.h"
#include "std_msgs/Float32MultiArray.h"
#include "library/ScrpSlave.hpp"
#include "library/RotaryInc.hpp"
#include "library/GY521.hpp"

#define MAXPWM 0.98
#define Moter_NUM 4
#define PERIOD 2048

#define PI2_3 2.0943951023931954923084289221863
#define PI_3 1.0471975511965977461542144610932
#define PI_90 0.034906585
#define PI_4 0.785398163
#define Moter_L 100 //駆動輪と計測輪のそれぞれの回転中心とタイヤの距離
#define Rotary_L 100

#define kp 0.00002 //モーター
#define ki 0.003
#define kd 0.0000007

const PinName pwm_pin[7][3] = {
    {PB_1 ,PA_11,PC_6 },
    {PB_13,PB_14,PB_2 },
    {PB_4 ,PB_5 ,PB_15},
    {PC_8 ,PC_9 ,PA_10},
    {PB_6 ,PB_7 ,PB_12},
    {PB_8 ,PB_9 ,PC_7 },
    {PA_0 ,PA_1 ,PB_0 }
};

const PinName rotary_pin[8][2] = {
    {PC_10,PC_11},
    {PC_4 ,PA_13},
    {PA_14,PA_15},
    {PC_2 ,PC_3 },
    {PA_12,PC_5 },
    {PC_0 ,PC_1 },
    {PA_6 ,PA_7 },
    {PA_8 ,PA_9 }
};

//ScrpSlave slave(PC_12,PD_2,PH_1,0x0807ffff);

DigitalOut led(PA_5);
DigitalIn button(PC_13);
Timer motertimer;

PwmOut* Moter[Moter_NUM][2];
DigitalOut* Led[Moter_NUM];
RotaryInc* Speed[Moter_NUM];
RotaryInc* Place[Moter_NUM];
GY521 *gy;
bool ok = false;
bool able_move = true;
double X = 0,Y = 0,T = 0,Yaw = 0;//オドメトリ
double Vx = 0,Vy = 0,Omega = 0;//速度
double driveS[Moter_NUM],nowV[Moter_NUM];

ros::NodeHandle nh;

inline double constrain(double x,double a,double b){
    return (x < a ? a : (x > b ? b : x));
}

void safe(){
    Vx = 0;
    Vy = 0;
    Omega = 0;
    for(int i = 0;i < Moter_NUM;i++){
        Moter[i][0]->write(0);
        Moter[i][1]->write(0);
        Led[i]->write(0);
    }
}

void Reset(){
    if(!ok){
        ok = true;
    }else{
        led.write(0);
        safe();
        gy->reset(0);
        X = 0;
        Y = 0;
        T = 0;
        led.write(1);
    }
}    

inline bool Drive(int id,double pwm){//モーターを回す
    pwm = constrain(pwm,-MAXPWM,MAXPWM);
    if(!pwm){
        Moter[id][0]->write(0);
        Moter[id][1]->write(0);
        Led[id]->write(0);
    }else if(0 < pwm){
        Moter[id][0]->write(pwm);
        Moter[id][1]->write(0);
        Led[id]->write(1);
    }else{
        Moter[id][0]->write(0);
        Moter[id][1]->write(-pwm);
        Led[id]->write(1);
    }
    return true;
}

inline void move(){
    static double diff[Moter_NUM],errer[Moter_NUM],diffV[Moter_NUM],lastV[Moter_NUM],driveV[Moter_NUM],now_t,cos_yaw,sin_yaw;
    static int j;
#if Moter_NUM == 3
    driveV[0] = Vx*cos(Yaw)         + Vy*sin(Yaw)         + Omega*Moter_L;
    driveV[1] = Vx*cos(Yaw + PI2_3) + Vy*sin(Yaw + PI2_3) + Omega*Moter_L;
    driveV[2] = Vx*cos(Yaw - PI2_3) + Vy*sin(Yaw - PI2_3) + Omega*Moter_L;
#elif Moter_NUM == 4
    cos_yaw = cos(Yaw);
    sin_yaw = sin(Yaw);
    driveV[0] =  Vx*cos_yaw + Vy*sin_yaw + Omega*Moter_L;
    driveV[1] = -Vx*sin_yaw + Vy*cos_yaw + Omega*Moter_L;
    driveV[2] = -Vx*cos_yaw - Vy*sin_yaw + Omega*Moter_L;
    driveV[3] =  Vx*sin_yaw - Vy*cos_yaw + Omega*Moter_L;
#endif
    now_t = motertimer.read();
    motertimer.reset();
    for(j = 0;j < Moter_NUM;j++){
        nowV[j] = Speed[j]->getSpeed();
        diff[j] = driveV[j] - nowV[j];
        if(nowV[j] == 0 && driveV[j] == 0 && errer[j] != 0){
            errer[j] = 0;
        }else{
            errer[j] += diff[j] * now_t;
        }
        diffV[j] = (nowV[j] - lastV[j]) / now_t;
        lastV[j] = nowV[j];
        driveS[j] = 0.0003 * driveV[j] + diff[j] * kp + errer[j] * ki - diffV[j] * kd;
        Drive(j,driveS[j]);
    }
}

void getData(const std_msgs::Float32MultiArray &msgs){
    if(!ok && (int)msgs.data[0] != -1)return;
    switch((int)msgs.data[0]){
        case -1:
            able_move = true;
            Reset();
            break;
        case 0:
            able_move = true;
            Vx = msgs.data[1];
            Vy = msgs.data[2];
            Omega = msgs.data[3];
            break;
        case 1:
            able_move = true;
            safe();
            break;
        case 2:
            able_move = false;
            Drive(0,msgs.data[1]/255);
            Drive(1,msgs.data[2]/255);
            break;
        case 3:
            able_move = false;
            Drive(2,msgs.data[1]/255);
            Drive(3,msgs.data[2]/255);
            break;
    }
}

std_msgs::Float32MultiArray now;
ros::Publisher place("place",&now);
ros::Subscriber<std_msgs::Float32MultiArray> sub("moter",&getData);

int main(){
    nh.getHardware()->setBaud(115200);
    nh.initNode();
    nh.advertise(place);
    nh.subscribe(sub);
    now.data_length = 7;
    now.data = (float*)malloc(sizeof(float)*now.data_length);
    int i;
    bool flag = false;
    double diff[Moter_NUM],Pspeed[Moter_NUM];
    double nowVx,nowVy,nowVt;
    double cos_yaw_2,sin_yaw_2;
    for(i = 0;i < Moter_NUM;i++){
        Led[i] = new DigitalOut(pwm_pin[i][2]);
        Moter[i][0] = new PwmOut(pwm_pin[i][0]);
        Moter[i][1] = new PwmOut(pwm_pin[i][1]);
        Moter[i][0]->period_us(PERIOD);
        Moter[i][1]->period_us(PERIOD);
        Speed[i] = new RotaryInc(rotary_pin[i][0],rotary_pin[i][1],2*50.8*M_PI,256,2);
        Place[i] = new RotaryInc(rotary_pin[i+Moter_NUM][0],rotary_pin[i+Moter_NUM][1],2*25.4*M_PI,256,2);
    }
    I2C i2c(PB_3,PB_10);
    Timer loop;
    loop.start();
    while(button && !ok){
        nh.spinOnce();
        if(loop.read_ms() > 250){
            led = !led;
            loop.reset();
        }
    }
    led.write(0);
    ok = true;
    GY521 gyro(i2c);
    gy = &gyro;
    motertimer.start();
    led.write(1);
    while(true){
        if(able_move){
            move();
        }else{
            for(i = 0;i<4;i++){
                nowV[i] = Speed[i]->getSpeed();
            }
        }
        nh.spinOnce();
        gyro.updata();
        Yaw = gyro.yaw;
        if(loop.read_ms() > 20){//50msごとに通信する
            //データ送信
            now.data[0] = X;
            now.data[1] = Y;
            now.data[2] = T;
            now.data[3] = Yaw;
            now.data[4] = nowVx;
            now.data[5] = nowVy;
            now.data[6] = nowVt;
            place.publish(&now);
            loop.reset();
        }
        Yaw *= 0.01745329251994329576923690768489;//PI/180
        for(i = 0;i<Moter_NUM;++i){
            diff[i] = Place[i]->diff();//Place
            Pspeed[i] = Speed[i]->getSpeed();//Place
        }
        //オドメトリ計算
#if Moter_NUM == 3
        X += -2.0/3.0*diff[0]*cos(Yaw) + 2.0/3.0*diff[1]*cos(Yaw-PI_3) + 2.0/3.0*diff[2]*cos(Yaw+PI_3);
        Y += -2.0/3.0*diff[0]*sin(Yaw) + 2.0/3.0*diff[1]*sin(Yaw-PI_3) + 2.0/3.0*diff[2]*sin(Yaw+PI_3);
        T +=  diff[0]/Rotary_L/3 + diff[1]/Rotary_L/3 + diff[2]/Rotary_L/3;
        nowVx = -2.0/3.0*Pspeed[0]*cos(Yaw) + 2.0/3.0*Pspeed[1]*cos(Yaw-PI_3) + 2.0/3.0*Pspeed[2]*cos(Yaw+PI_3);
        nowVy = -2.0/3.0*Pspeed[0]*sin(Yaw) + 2.0/3.0*Pspeed[1]*sin(Yaw-PI_3) + 2.0/3.0*Pspeed[2]*sin(Yaw+PI_3);
        nowVt =  Pspeed[0]/Rotary_L/3 + Pspeed[1]/Rotary_L/3 + Pspeed[2]/Rotary_L/3;
#elif Moter_NUM == 4
        cos_yaw_2 = cos(Yaw)/2.0;
        sin_yaw_2 = sin(Yaw)/2.0;
        X += diff[0]*cos_yaw_2 - diff[1]*sin_yaw_2 - diff[2]*cos_yaw_2 + diff[3]*sin_yaw_2;
        Y += diff[0]*sin_yaw_2 + diff[1]*cos_yaw_2 - diff[2]*sin_yaw_2 - diff[3]*cos_yaw_2;
        T += diff[0]/Rotary_L/4 + diff[1]/Rotary_L/4 + diff[2]/Rotary_L/4 + diff[3]/Rotary_L/4;
        cos_yaw_2 = cos(Yaw)/2.0;
        sin_yaw_2 = sin(Yaw)/2.0;
        nowVx = Pspeed[0]*cos_yaw_2 - Pspeed[1]*sin_yaw_2 - Pspeed[2]*cos_yaw_2 + Pspeed[3]*sin_yaw_2;
        nowVy = Pspeed[0]*sin_yaw_2 + Pspeed[1]*cos_yaw_2 - Pspeed[2]*sin_yaw_2 - Pspeed[3]*cos_yaw_2;
        nowVt = Pspeed[0]/Rotary_L/4 + Pspeed[1]/Rotary_L/4 + Pspeed[2]/Rotary_L/4 + Pspeed[3]/Rotary_L/4;
#endif
        if(!button && !flag){
            flag = true;
            Reset();
        }else if(button && flag){
            flag = false;
        }
    }
}