File content as of revision 6:08b8f6b05c22:

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

//PwmOut motor_1F(PA_5);//1Forward Right motor Forward
//PwmOut motor_1B(PC_7);//Forward Right motor Back
//PwmOut motor_2F(PC_9);//2Forward Left motor Forward
//PwmOut motor_2B(PA_1);//Forward Left motor Back
//PwmOut motor_3F(PA_10);//3Back Right motor Forward
//PwmOut motor_3B(PB_4);//Back Right motor Back
//PwmOut motor_4F(PA_9);//4Back Left motor Forward
//PwmOut motor_4B(PA_7);//Back Left motor Back

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)
               };  //1逓倍用class

Ec2multi ecXY[]= {Ec2multi(PC_6,PB_8,RESOLUTION),
                  Ec2multi(PB_9,PC_8,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])
                      };

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

DigitalIn button(USER_BUTTON);
Serial pc(USBTX, USBRX); // tx, rx
R1370P gyro(PC_10,PC_11);   // tx, rx

Ticker ticker;

void calOmega()
{
    for(int i=0; i<4; i++) {
        ec[i].calOmega();
    }
}

//自己位置取得
double theta=0;
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;
        ec_count[0]=ecXY[0].getCount();
        ec_count[1]=ecXY[1].getCount();
        ax = (ec_count[0]-old_count[0])*cos(45+theta);
        ay = (ec_count[0]-old_count[0])*sin(45+theta);
        bx = (ec_count[1]-old_count[1])*cos(45-theta);
        by = (ec_count[1]-old_count[1])*sin(45-theta);
        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[5][2]= {
    {0,0}
    ,{0,10000}
    ,{5000,10000}
    ,{5000,0}
    ,{0,0}
};

//出力を計算
double omega[4];
int x,y;
//出力
double vx,vy;
void setOmega(int max)
{
    omega[0]=max*vx/sqrt(2.0)-max*vy/sqrt(2.0);
    omega[1]=-max*vx/sqrt(2.0)-max*vy/sqrt(2.0);
    omega[2]=-max*vx/sqrt(2.0)+max*vy/sqrt(2.0);
    omega[3]=max*vx/sqrt(2.0)+max*vy/sqrt(2.0);
}
void motorOut()
{
    for(int i=0; i<4; i++) {
        motor[i].Sc((int)omega[i]);
    }
}
void echo_()
{
    double a=0,b=0,c=0,d=0;
    for(int i=0; i<4; i++) {
        echo[i].start();
    }
    wait(1);//minimum wait is 30msec
    a=echo[0].get_dist_cm();
    b=echo[1].get_dist_cm();
    c=echo[2].get_dist_cm();
    d=echo[3].get_dist_cm();
    printf("a:%f b:%f c:%f d:%f\r\n",a,b,c,d);
    if(a<5||b<5||c<5||d<5) {
        for(int j=0; j<4; j++) {
            motor[j].stop();
        }
    }
}

int main()
{
    gyro.initialize();    //main関数の最初に一度だけ実行
    gyro.acc_offset();
    ticker.attach(motorOut,0.05);
    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.5);
    motor[1].setDutyLimit(0.5);
    motor[2].setDutyLimit(0.5);
    motor[3].setDutyLimit(0.5);

    motor[0].setPDparam( 0.004839, 0.0026290 );
    motor[1].setPDparam( 0.004702, 0.025806 );
    motor[2].setPDparam( 0.004397, 0.025159 );
    motor[3].setPDparam( 0.004801, 0.026645 );

    int n=0,dx,dy,aimX,aimY;
    double a=0,b=0,c=0,d=0;
    Location location;
    while(1) {
        //自己位置取得
        theta=gyro.getAngle();    //角度の値を受け取る
        location.calXY();

        x=location.getX();
        y=location.getY();
        printf("%3.3f  %3.3f  %3.3f  %3.3f  location %d,%d \r\n",omega[0],omega[1],omega[2],omega[3],x,y);

        //目的地決定(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);

        //四輪の出力計算
        setOmega(10);

        if(dx<100 &&dx>-100) {
            if(dy<100 && dy>-100) {
                n++;
                printf("reached Location");
                for(int j=0; j<4; j++) {
                    motor[j].stop();
                }
                wait(2);
            }

        }
        if(n>4) {
            for(int j=0; j<4; j++) {
                motor[j].stop();
            }
        }
    }

}