File content as of revision 2:0be4cfbdf408:

#include "mbed.h"
#include "EC.h"
#include "SpeedController.h"
#include "math.h"
#include "R1370P.h"
#include"hcsr04.h"
#define RESOLUTION 500

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);
Serial pc(USBTX, USBRX); // tx, rx
R1370P gyro(PC_10,PC_11);   // tx, rx

/*HCSR04 echo[]= {
    HCSR04(PC_0,PC_12)//A
    ,HCSR04(PA_15,PB_7)//A
    ,HCSR04(PH_1,PB_0)// B
    ,HCSR04(PC_3,PB_10)//B
};*/

//自己位置取得
double theta;
double angle;

class Location
{
public:
    Location():x_(0),y_(0)
    {
        for(int i =0; i<2; i++) {
            old_count[i]=0;
        }
    }
    void calXY()
    {
        double ec_count[2]= {};
        double ax,ay,bx,by;
        double atheta,btheta;
        atheta = (45+theta)/180*3.14;
        btheta = (135+theta)/180*3.14;

        ec_count[0]=ecXY[0].getCount();
        ec_count[1]=ecXY[1].getCount();
        ax = (ec_count[0]-old_count[0])*cos(atheta);
        ay = (ec_count[0]-old_count[0])*sin(atheta);
        bx = (ec_count[1]-old_count[1])*cos(btheta);
        by = (ec_count[1]-old_count[1])*sin(btheta);
        x_=x_+ax + bx;
        y_=y_+ay + by;
        old_count[0]=ec_count[0];
        old_count[1]=ec_count[1];
    }
    double getX()
    {
        return x_;
    }
    double getY()
    {
        return y_;
    }

private:
    double x_;
    double y_;
    double old_count[2];
};



//目的地決定
int plot[][2]= {
    {0,0}
    ,{2000,16000}
    ,{8000,16000}
    ,{4000,0}
    ,{0,0}
    ,{0,4000}
    ,{4000,4000}
    ,{4000,0}
    ,{0,0}
};

double aimTheta[]= {//目標角度を指定
    0,0,0,0,0,0,0,0,0,0,0,0
};

double zMin[]= { //速度の最少を指定
    0,0,0,0,0,0,0,0,0,0,0,0
};

//出力を計算
int x,y;

class WheelOmega
{
public:
    WheelOmega(): max_(0),vx_(0),vy_(0),theta_(0)
    {
        for(int i=0; i<4; i++) {
            omega[i]=0;
        }
    }
    void setOmega(double max,double k)
    {
        max_=max;
        k_=k;

    }
    void setVxy(double vx,double vy,double aimtheta_)
    {
        vx_=vx;
        vy_=vy;
        theta_=aimtheta_ - theta;
        if(theta_>30) {//目標角度まで３０度以上空いていたら補正、係数調整のため３０は適当
            theta_=30;
        }
        if(theta_<-30) {
            theta_=-30;
        }
    }
    void calOmega()
    {
        double theta_rad=45/180*3.14;
        omega[0]=max_*vx_*cos(theta_rad)-max_*vy_*cos(theta_rad) + theta_*k_;
        omega[1]=-max_*vx_*cos(theta_rad)-max_*vy_*cos(theta_rad)+theta_*k_;
        omega[2]=-max_*vx_*cos(theta_rad)+max_*vy_*cos(theta_rad)+theta_*k_;
        omega[3]=max_*vx_*cos(theta_rad)+max_*vy_*cos(theta_rad)+theta_*k_;
    };
    double getOmega(int i)
    {
        return omega[i];
    }
private:
    double max_,vx_,vy_,theta_,k_;
    double omega[4];
};

WheelOmega omega;
//パラメタ処理
double pControl(double distance_,double zMin,double newtime)
{
    double z,zMax,olddistance,oldtime;
    double diftime_;
    diftime_ = newtime - oldtime;
    oldtime= newtime;
    z=0.004*distance_ - 0.1*(olddistance-distance_)/diftime_;
    zMax=2;
    if(z>zMax) {
        z=zMax;
    }
    if(z<zMin) {
        z=zMin;
    }
    if(newtime<1) {
        z=z*newtime;
    }
    olddistance = distance_;
    return z;
}

//超音波
/*class Sonic
{
public:
    Sonic()
    {
        for(int i=0; i<4; i++) {
            sonic_cm[i]=0;
        }
    }

    void cal_sonic()
    {
        for(int i=0; i<4; i++) {
            echo[i].start();
        }
        for(int i=0; i<4; i++) {
            sonic_cm[i] =echo[i].get_dist_cm();
        }
    }
    double get_sonic(int i)
    {
        return sonic_cm[i];
    }

private:
    double sonic_cm[4];
};

*/
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 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;
            printf("   1");
            break;
        case 2:
            canOmega[0]=a*1.4;
            canOmega[1]=a*1.4;
            canOmega[2]=-a*1.4;
            canOmega[3]=-a*1.4;
            printf("   2");
            break;
        case 3:
            canOmega[0]=a;
            canOmega[1]=0;
            canOmega[2]=-a;
            canOmega[3]=0;
            printf("   3");
            break;
        case 4:
            canOmega[0]=-a*1.4;
            canOmega[1]=a*1.4;
            canOmega[2]=a*1.4;
            canOmega[3]=-a*1.4;
            printf("   4");
            break;
        case 5:
            for(int i=0; i<4; i++) {
                canOmega[i]=0;
            }
            printf("   5");
            break;
        case 6:
            canOmega[0]=a*1.4;
            canOmega[1]=-a*1.4;
            canOmega[2]=-a*1.4;
            canOmega[3]=a*1.4;
            printf("   6");
            break;
        case 7:
            canOmega[0]=0;
            canOmega[1]=-a;
            canOmega[2]=0;
            canOmega[3]=a;
            printf("   7");
            break;
        case 8:
            canOmega[0]=-a*1.4;
            canOmega[1]=-a*1.4;
            canOmega[2]=a*1.4;
            canOmega[3]=a*1.4;
            printf("   8");
            break;
        case 9:
            canOmega[0]=-a;
            canOmega[1]=0;
            canOmega[2]=a;
            canOmega[3]=0;
            printf("   9");
            break;
        case 10:
            for(int i=0; i<4; i++) {
                canOmega[i]=a;
            }
            printf("   10");
            break;
        case 11:
            for(int i=0; i<4; i++) {
                canOmega[i]=-a;
            }
            printf("   11");
            break;
        case 12:
            wait(0.2);
            if(count==1) {
                a=0.1;
                count=0;
            } else {
                a=0.05;
                count=1;
            }
            printf("   12 Speed change");
            break;

    }
    printf(" %f\r\n",a);
    int i=0;
    while(i<4) {
        motor[i].turn(canOmega[i]);
        i++;
    }
}

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

//出力
void motorOut()
{
    for(int i=0; i<4; i++) {
        motor[i].Sc(omega.getOmega(i));
    }
}
CAN_ticker canx;
void ticker_CanRead()
{
    canx.canmsg_read();
    /*for(int i=0; i<4; i++) {
       motor[i].Sc(canOmega[i]);
    }*/
}

Location location;
void calLoc()
{
    theta=gyro.getAngle()-angle;    //角度受け取り
    location.calXY();
}
int main()
{
    can1.frequency(1000000);
    gyro.initialize();    //main関数の最初に一度だけ実行
    gyro.acc_offset();
    angle=gyro.getAngle();//角度初期化
    double z;
    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 first=0;
    while(first==0) {
        printf("waiting\r\n");
        if(button==0) {
            wait(1);
            ticker.attach(&motorOut,0.05);
            first++;
            break;
        }
    }

    int canX=20;//can変数
    int n=1,dx,dy,aimX,aimY;
    double vx_,vy_,vx,vy,newtime,distance;
    Timer time;
    time.start();
    locTicker.attach(&calLoc,0.05);
    canTicker.attach(&ticker_CanRead,0.1);//can読み込み
    while(1) {
        canX=canx.get_xCAN();//０で手\動化
        if(canX==13) {//手動化
            wait(0.2);
            printf("change1\r\n");
            ticker.detach();
            while(1) {
                canX=canx.get_xCAN();
                calOmega_CAN(canX);
                //自己位置取得
                x=location.getX();
                y=location.getY();
                printf("X=%06d ,Y=%06d",x,y);
                //目的地決定(syuusoku check)
                aimX = plot[n][0];
                aimY = plot[n][1];
                dx=aimX-x;
                dy=aimY-y;
                distance = sqrt((float)dx*dx+dy*dy);
                if(distance<1000) {
                    n++;
                    printf("reach%d\r\n",n);
                    time.reset();
                }
                if(canX==13) {//自動化
                    canX=20;
                    for(int i=0; i<4; i++) {
                        canOmega[i]=0;
                    }
                    ticker.attach(&motorOut,0.05);
                    printf("change2\r\n");
                    wait(0.1);
                    break;
                }
            }
        }
        //自己位置取得
        x=location.getX();
        y=location.getY();
        int i=0;
        if(i==10) {
            printf("X=%d,Y=%d,theta=%5.3f ,angle=%5.3f z=%5.3f  n=%d\r\n",x,y,theta,angle,z,n);
            for(int j=0; j<4; j++) {
                printf("%d : %d,",j,ec[j].getCount());
            }
            printf("\r\n");
            i=10;
        } else {
            i++;
        }
        //目的地決定(syuusoku check)
        aimX = plot[n][0];
        aimY = plot[n][1];
        //出力を計算(kitai xy);
        dx=aimX-x;
        dy=aimY-y;
        vx_=dx/sqrt((double)dx*dx+dy*dy);
        vy_=dy/sqrt((double)dx*dx+dy*dy);
        vx=vx_*cos(theta/180*3.14)+vy_*sin(theta/180*3.14);
        vy=-vx_*sin(theta/180*3.14)+vy_*cos(theta/180*3.14);
        //四輪の出力計算
        newtime=time.read();
        distance = sqrt((float)dx*dx+dy*dy);
        z=pControl(distance,zMin[n],newtime);
        omega.setOmega(z,0.05);
        omega.setVxy(vx,vy,aimTheta[n]);
        omega.calOmega();
        //ゴール判定
        if(distance<800) {
            n++;
            printf("reach%d\r\n",n);
            time.reset();
        }

        if(n>=8) {
            for(int j=0; j<4; j++) {
                motor[j].stop();
            }
            printf("fin\r\n");
            ticker.detach();
        }
    }
}