suda akihiro
a
Dependencies: mbed MPU6050 FastPWM
Diff: Line_Avoid.cpp
- Revision:
- 0:67a3f3de0758
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Line_Avoid.cpp Mon Nov 15 15:35:31 2021 +0000
@@ -0,0 +1,253 @@
+#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);
+ }
\ No newline at end of file