File content as of revision 1:2189d6238ac7:

#include "mbed.h"
#include "SpeedController.h"//最終的にはSpeedController使わないけど
#include "EC.h"//エンコーダ
#include "R1370P.h"//ジャイロ
//#include "enc_1multi.h"
#include "math.h"
#define BASIC_SPEED 24  //モーターはこの角速度で駆動させる//SpeedController
#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);//モーター定義//SpeedController

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();//SpeedController
    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;




void output(double iRF,double iLF,double iLB,double iRB)//後にSPI用に変える
{
    target_RF=BASIC_SPEED*iRF;
    target_LF=BASIC_SPEED*iLF;
    target_LB=BASIC_SPEED*iLB;
    target_RB=BASIC_SPEED*iRB;
}
void base(double bRF,double bLF,double bLB,double bRB,double Max)//いろんな加算をしても最大OR最小が１になるような補正(?)//絶対値が一番でかいやつで除算//double Max(0~1)
{
    
    if     (fabs(bRF)>=fabs(bLF)&&fabs(bRF)>=fabs(bLB)&&fabs(bRF)>=fabs(bRB))output(Max              ,Max*bLF/fabs(bRF),Max*bLB/fabs(bRF),Max*bRB/fabs(bRF));
    else if(fabs(bLF)>=fabs(bRF)&&fabs(bLF)>=fabs(bLB)&&fabs(bLF)>=fabs(bRB))output(Max*bRF/fabs(bLF),Max              ,Max*bLB/fabs(bLF),Max*bRB/fabs(bLF));
    else if(fabs(bLB)>=fabs(bRF)&&fabs(bLB)>=fabs(bLF)&&fabs(bLB)>=fabs(bRB))output(Max*bRF/fabs(bLB),Max*bLF/fabs(bLB),Max              ,Max*bRB/fabs(bLB));
    else                                                                     output(Max*bRF/fabs(bRB),Max*bLF/fabs(bRB),Max*bLB/fabs(bRB),Max              );
}



//ここからそれぞれのプログラム//////////////////////////////////////////////////////
//now_x(現在のx座標),now_y(現在のy座標),now_angle(機体角度(ラジアンではない)(0~360や-180~180とは限らない))(反時計回りが正)
//ジャイロの出力は角度だが三角関数はラジアンとして計算する
//通常の移動＋座標のずれ補正＋機体の角度補正(＋必要に応じさらに別補正)
//ジャイロの仕様上、角度補正をするときに計算式内で角度はそのままよりsinをとったほうがいいかもね

void gostraight(int type,double goal_x,double goal_y,double speed,double front)//移動パターン(1,2,3,4)、目標X、目標Y、最高速度(0~1)、正面角度
{
    double y_hosei=(now_y-goal_y)*0.001;//Y座標(mm単位)にP処理
    double x_hosei=(now_x-goal_x)*0.001;//X座標(mm単位)にP処理
    double incl_hosei=sin(now_angle-front)*(PI/180)*0.1;//機体角度(sin(数度→ラジアンに変換))にP処理

    switch(type) {
        case 1://Y座標一定の正方向横移動
            while(now_x<goal_x){
            base(-1-y_hosei-incl_hosei,-1+y_hosei-incl_hosei,1+y_hosei-incl_hosei,1-y_hosei-incl_hosei,speed);
            }
            break;
            
        case 2://Y座標一定の負方向横移動
            while(now_x>goal_x){
            base(1-y_hosei-incl_hosei,1+y_hosei-incl_hosei,-1+y_hosei-incl_hosei,-1-y_hosei-incl_hosei,speed);
            }
            break;
            
        case 3://Y座標一定の正方向横移動
            while(now_y<goal_y){
            base(1+x_hosei-incl_hosei,-1+x_hosei-incl_hosei,-1-x_hosei-incl_hosei,1-x_hosei-incl_hosei,speed);
            }
            break;
            
        case 4://X座標一定の負方向横移動
            while(now_y>goal_y){
            base(-1+x_hosei-incl_hosei,1+x_hosei-incl_hosei,1-x_hosei-incl_hosei,-1-x_hosei-incl_hosei,speed);
            }
            break;
    }
}

//ここまで///////////////////////////////////////////////////////////////////////

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

    motor_tick.attach(&calOmega,0.05);

    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*PI/180)-d_dist1*sin(now_angle*PI/180);
        double d_y=d_dist2*sin(now_angle*PI/180)+d_dist1*cos(now_angle*PI/180);//微小時間毎の座標変化


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