File content as of revision 0:b811f40948e2:

#include "mbed.h"
#include "SpeedController.h"
#include "EC.h"
#include "R1370P.h"
#include "enc_1multi.h"
#include "math.h"
#define BASIC_SPEED 24  //モーターはこの角速度で駆動させる
#define PI 3.141592


SpeedControl Motor_RF(NC,NC,NC,500,0.05,NC,NC);
SpeedControl Motor_LF(NC,NC,NC,500,0.05,NC,NC);
SpeedControl Motor_LB(NC,NC,NC,500,0.05,NC,NC);
SpeedControl Motor_RB(NC,NC,NC,500,0.05,NC,NC);//モーター定義

Ec EC1(NC,NC,NC,300,0.05);
Ec EC2(NC,NC,NC,300,0.05);//エンコーダ定義

Ticker motor_tick;  //角速度計算用ticker
Ticker ticker;//for enc

Serial pc(USBTX, USBRX); // tx, rx //PC USB
R1370P gyro(PC_6,PC_7);

void calOmega() //角速度計算関数
{
    Motor_RF.CalOmega();
    Motor_LF.CalOmega();
    Motor_LB.CalOmega();
    Motor_RB.CalOmega();
    EC1.CalOmega();
    EC2.CalOmega();
}
double new_dist1=0,new_dist2=0;
double old_dist1=0,old_dist2=0;
double d_dist1=0,d_dist2=0;//座標計算用関数
double now_x;//現在地X座標
double now_y;//現在地Y座標
double now_angle; //現在角度

double start_x=0;
double start_y=0;

double target_RF=0,target_LF=0,target_LB=0,target_RB=0;//目標速度
Timer t;
int i=0;//い　つ　も　の



void idou(double iRF,double iLF,double iLB,double iRB)//各モーターの動作明瞭化
{
    target_RF=BASIC_SPEED*iRF;
    target_LF=BASIC_SPEED*iLF;
    target_LB=BASIC_SPEED*iLB;
    target_RB=BASIC_SPEED*iRB;
}


int main()
{
    gyro.initialize();    //main関数の最初に一度だけ実行
    gyro.acc_offset();    //やってもやらなくてもいい
    printf("start\r\n");

    motor_tick.attach(&calOmega,0.05);
    Motor_RF.setDOconstant(30);
    Motor_LF.setDOconstant(30);//c
    Motor_LB.setDOconstant(30);
    Motor_RB.setDOconstant(30);//c

    Motor_RF.setPDparam(0,0);
    Motor_LF.setPDparam(0,0);//pd
    Motor_LB.setPDparam(0,0);
    Motor_RB.setPDparam(0,0);//pd


    EC1.setDiameter_mm(48);
    EC2.setDiameter_mm(48);//測定輪半径
    double  getDistance_mm();

    void reset();
    EC1.reset();

    now_x=start_x;
    now_y=start_y;
    while(1) {
        now_angle=gyro.getAngle();

        new_dist1=EC1.getDistance_mm();
        new_dist2=EC2.getDistance_mm();
        d_dist1=new_dist1-old_dist1;
        d_dist2=new_dist2-old_dist2;
        old_dist1=new_dist1;
        old_dist2=new_dist2;//微小時間当たりのエンコーダ読み込み


        double d_x=d_dist2*sin(now_angle*3.1415926535/180)-d_dist1*sin(now_angle*3.1415926535/180);
        double d_y=d_dist2*sin(now_angle*3.1415926535/180)+d_dist1*cos(now_angle*3.1415926535/180);//微小時間毎の座標変化


        now_x=now_x+d_x;
        now_y=now_y+d_y;//微小時間毎に座標に加算
    }
}