File content as of revision 0:67a3f3de0758:

#include "mbed.h"
#include "FastPWM.h"
#include "MPU6050.h"
Serial pc(USBTX, USBRX);

void Line_();
void Line_Print();

AnalogIn Line_Flont(PA_7);
AnalogIn Line_Back(PA_5);
AnalogIn Line_Right(PA_4);
AnalogIn Line_Left(PA_6);

MPU6050 mpu(PB_7,PB_6);/////////MPU6050
double Degree;
double dt=0.00860;
float gx,gy,gz,ax,ay,az;
double GZ;
double preGZ;
Timer MPU6050timer;
void MPU6050();

float Kp = 3.06; //3.06 2.26   //PID設定
float Kd = 0.00; //0.58
float Ki = 0.00; //5.03

int Gyro_power = 0;
int GyroE ;
int GyroE_1;

float Line_Flont_Value;
float Line_Back_Value;
float Line_Right_Value;
float Line_Left_Value;

float Line_Flont_Judg = 0.4;
float Line_Back_Judg = 0.4;
float Line_Right_Judg = 0.65;
float Line_Left_Judg = 0.65;

void Motor_Speed (int Port,int Power);
void Motor_Stop ();
void Motor_Angle(int Dgree);
void Motor_Direction(int Dgree,int Baisepower);


double V[4];//行列
double kesu[4][3] = {0.71, 0.71, 0.00,
                     0.71, -0.71, 0.00,
                     -0.71, -0.71, 0.00,
                     -0.71, 0.71, 0.00,
                    };
double yoso[3] ;
float pi = 3.141592;
float deg, rad;
double Vx, Vy, L;

int Baisepower=0;

int Directionpower1;
int Directionpower2;
int Directionpower3;
int Directionpower4;

FastPWM MotorPWM1(PF_0); 
FastPWM MotorPWM2(PA_12);//PA_11 PWM出力おかしい
FastPWM MotorPWM3(PB_5); 
FastPWM MotorPWM4(PA_8); 

DigitalOut Buzzer(PA_1);

int main() {
    
    MotorPWM1.period_us(5);
    MotorPWM2.period_us(5);
    MotorPWM3.period_us(5);
    MotorPWM4.period_us(5);
    
    if(mpu.getID()==0x68){//72
        pc.printf("MPU6050 OK");
        wait(1);
    } else {
        pc.printf("MPU6050 error ID=0x%x\r\n",mpu.getID());
        while(1) {
        }
    }
    mpu.start();
    while(1) {
        while(1){
            MPU6050();
            Line_Flont_Value = Line_Flont.read();
            Line_Back_Value  = Line_Back.read();
            Line_Right_Value = Line_Right.read();
            Line_Left_Value  = Line_Left.read();
        
            Line_Print();
            if(Line_Right_Value > Line_Right_Judg){
                Buzzer = 1;
                while(1){
                    Motor_Direction(-90,50);
                    wait(0.2);
                    Line_Right_Value = Line_Right.read();
                    if(Line_Right_Value < Line_Right_Judg){
                       Buzzer = 0;
                       Motor_Stop();
                       wait(0.2);
                       break;  
                    }   
                }
            }else if(Line_Left_Value > Line_Left_Judg){
                Buzzer = 1;
                while(1){
                    Motor_Direction(90,50);
                    wait(0.2);
                    Line_Left_Value = Line_Left.read();
                    if(Line_Left_Value < Line_Left_Judg){
                       Buzzer = 0;
                       Motor_Stop();
                       wait(0.2);
                       break;  
                    }   
                }
            }else if(Line_Flont_Value > Line_Flont_Judg){
                Buzzer = 1;
                while(1){
                    Motor_Direction(180,50);
                    wait(0.5);
                    Line_Flont_Value = Line_Flont.read();
                    if(Line_Flont_Value < Line_Flont_Judg){
                       Buzzer = 0;
                       Motor_Stop();
                       wait(0.5);
                       break;  
                    }   
                }
            }else if(Line_Back_Value > Line_Back_Judg){
                Buzzer = 1;
                while(1){
                    Motor_Direction(0,50);
                    wait(0.2);
                    Line_Back_Value = Line_Back.read();
                    if(Line_Back_Value < Line_Back_Judg){
                       Buzzer = 0;
                       Motor_Stop();
                       wait(0.2);
                       break;  
                    }   
                }
            }
            
            Motor_Angle(0);
            //Motor_Stop();
            Baisepower=50;
        
            Directionpower1=V[0]*Baisepower;
            Directionpower2=V[1]*Baisepower;
            Directionpower3=V[2]*Baisepower;
            Directionpower4=V[3]*Baisepower;
        
            Motor_Speed(1,Directionpower1+Gyro_power);
            Motor_Speed(2,Directionpower2+Gyro_power);
            Motor_Speed(3,Directionpower3+Gyro_power);
            Motor_Speed(4,Directionpower4+Gyro_power);
        }
    }
}

void Line_Print(){
      printf("Float//");
      printf("%2f//",Line_Flont_Value);
      printf("Back_//");
      printf("%2f//",Line_Back_Value);
      printf("Right//");
      printf("%2f//",Line_Right_Value);
      printf("Left//");
      printf("%2f//\r\n",Line_Left_Value);  
}


void Motor_Speed (int Port,int Power){ //　モータ速度関数（モーター番号、速度｛-100~100｝）　
        if(Power>93){
            Power=93;
        }else if(Power<-93){
            Power=-93;
        }
        float motorPWM=(Power/2)*0.01+0.5; //PWM 0.0~0.5~1.0   回転～停止～回転
        if(Port==1){
            MotorPWM1.write(motorPWM);
        }else if(Port==2){
             MotorPWM2.write(motorPWM);
        }else if(Port==3){
             MotorPWM3.write(motorPWM);
        }else if(Port==4){
             MotorPWM4.write(motorPWM);
        }    
    }

void Motor_Stop(){
        Motor_Speed(1,0);
        Motor_Speed(2,0);
        Motor_Speed(3,0);
        Motor_Speed(4,0);
    }
    
void Motor_Angle(int Dgree){
  V[0] = 0.00;
  V[1] = 0.00;
  V[2] = 0.00;
  V[3] = 0.00;

  rad = Dgree * pi / 180;

  Vx = sin(rad);
  Vy = cos(rad);
  L = 0.00;

  yoso[0] = Vx;
  yoso[1] = Vy;
  yoso[2] = L;
  for (int i = 0; i <= 3; i++){
    for (int j = 0; j <= 2; j++){
      V[i] = V[i] + (yoso[j] * kesu[i][j]);
    }
  }        
}
void Motor_Direction(int Dgree,int Baisepower){
    Motor_Angle(Dgree);
    Directionpower1=V[0]*Baisepower;
    Directionpower2=V[1]*Baisepower;
    Directionpower3=V[2]*Baisepower;
    Directionpower4=V[3]*Baisepower;
        
    Motor_Speed(1,Directionpower1);
    Motor_Speed(2,Directionpower2);
    Motor_Speed(3,Directionpower3);
    Motor_Speed(4,Directionpower4);  
}

void MPU6050(){ 
        MPU6050timer.stop();
        dt=MPU6050timer.read();
        mpu.read(&gx,&gy,&gz,&ax,&ay,&az);
        MPU6050timer.reset();
        MPU6050timer.start();
        double gz_=gz;
        GZ=0.97*gz_+0.03*preGZ;
        if((GZ<=0.95)&&(GZ>=-0.95)){
            GZ=0;
        }
        Degree+=(preGZ+GZ)*dt/2;
        preGZ=GZ;    
        //pc.printf("%f\r\n",GZ);
    }