suda akihiro / Mbed 2 deprecated Line_Avoid

a

Dependencies:   mbed MPU6050 FastPWM

Line_Avoid.cpp@0:67a3f3de0758, 2021-11-15 (annotated)

Committer:
tokoro
Date:
Mon Nov 15 15:35:31 2021 +0000
Revision:
0:67a3f3de0758
d

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tokoro 0:67a3f3de0758 1 #include "mbed.h"
tokoro 0:67a3f3de0758 2 #include "FastPWM.h"
tokoro 0:67a3f3de0758 3 #include "MPU6050.h"
tokoro 0:67a3f3de0758 4 Serial pc(USBTX, USBRX);
tokoro 0:67a3f3de0758 5
tokoro 0:67a3f3de0758 6 void Line_();
tokoro 0:67a3f3de0758 7 void Line_Print();
tokoro 0:67a3f3de0758 8
tokoro 0:67a3f3de0758 9 AnalogIn Line_Flont(PA_7);
tokoro 0:67a3f3de0758 10 AnalogIn Line_Back(PA_5);
tokoro 0:67a3f3de0758 11 AnalogIn Line_Right(PA_4);
tokoro 0:67a3f3de0758 12 AnalogIn Line_Left(PA_6);
tokoro 0:67a3f3de0758 13
tokoro 0:67a3f3de0758 14 MPU6050 mpu(PB_7,PB_6);/////////MPU6050
tokoro 0:67a3f3de0758 15 double Degree;
tokoro 0:67a3f3de0758 16 double dt=0.00860;
tokoro 0:67a3f3de0758 17 float gx,gy,gz,ax,ay,az;
tokoro 0:67a3f3de0758 18 double GZ;
tokoro 0:67a3f3de0758 19 double preGZ;
tokoro 0:67a3f3de0758 20 Timer MPU6050timer;
tokoro 0:67a3f3de0758 21 void MPU6050();
tokoro 0:67a3f3de0758 22
tokoro 0:67a3f3de0758 23 float Kp = 3.06; //3.06 2.26 //PID設定
tokoro 0:67a3f3de0758 24 float Kd = 0.00; //0.58
tokoro 0:67a3f3de0758 25 float Ki = 0.00; //5.03
tokoro 0:67a3f3de0758 26
tokoro 0:67a3f3de0758 27 int Gyro_power = 0;
tokoro 0:67a3f3de0758 28 int GyroE ;
tokoro 0:67a3f3de0758 29 int GyroE_1;
tokoro 0:67a3f3de0758 30
tokoro 0:67a3f3de0758 31 float Line_Flont_Value;
tokoro 0:67a3f3de0758 32 float Line_Back_Value;
tokoro 0:67a3f3de0758 33 float Line_Right_Value;
tokoro 0:67a3f3de0758 34 float Line_Left_Value;
tokoro 0:67a3f3de0758 35
tokoro 0:67a3f3de0758 36 float Line_Flont_Judg = 0.4;
tokoro 0:67a3f3de0758 37 float Line_Back_Judg = 0.4;
tokoro 0:67a3f3de0758 38 float Line_Right_Judg = 0.65;
tokoro 0:67a3f3de0758 39 float Line_Left_Judg = 0.65;
tokoro 0:67a3f3de0758 40
tokoro 0:67a3f3de0758 41 void Motor_Speed (int Port,int Power);
tokoro 0:67a3f3de0758 42 void Motor_Stop ();
tokoro 0:67a3f3de0758 43 void Motor_Angle(int Dgree);
tokoro 0:67a3f3de0758 44 void Motor_Direction(int Dgree,int Baisepower);
tokoro 0:67a3f3de0758 45
tokoro 0:67a3f3de0758 46
tokoro 0:67a3f3de0758 47 double V[4];//行列
tokoro 0:67a3f3de0758 48 double kesu[4][3] = {0.71, 0.71, 0.00,
tokoro 0:67a3f3de0758 49 0.71, -0.71, 0.00,
tokoro 0:67a3f3de0758 50 -0.71, -0.71, 0.00,
tokoro 0:67a3f3de0758 51 -0.71, 0.71, 0.00,
tokoro 0:67a3f3de0758 52 };
tokoro 0:67a3f3de0758 53 double yoso[3] ;
tokoro 0:67a3f3de0758 54 float pi = 3.141592;
tokoro 0:67a3f3de0758 55 float deg, rad;
tokoro 0:67a3f3de0758 56 double Vx, Vy, L;
tokoro 0:67a3f3de0758 57
tokoro 0:67a3f3de0758 58 int Baisepower=0;
tokoro 0:67a3f3de0758 59
tokoro 0:67a3f3de0758 60 int Directionpower1;
tokoro 0:67a3f3de0758 61 int Directionpower2;
tokoro 0:67a3f3de0758 62 int Directionpower3;
tokoro 0:67a3f3de0758 63 int Directionpower4;
tokoro 0:67a3f3de0758 64
tokoro 0:67a3f3de0758 65 FastPWM MotorPWM1(PF_0);
tokoro 0:67a3f3de0758 66 FastPWM MotorPWM2(PA_12);//PA_11 PWM出力おかしい
tokoro 0:67a3f3de0758 67 FastPWM MotorPWM3(PB_5);
tokoro 0:67a3f3de0758 68 FastPWM MotorPWM4(PA_8);
tokoro 0:67a3f3de0758 69
tokoro 0:67a3f3de0758 70 DigitalOut Buzzer(PA_1);
tokoro 0:67a3f3de0758 71
tokoro 0:67a3f3de0758 72 int main() {
tokoro 0:67a3f3de0758 73
tokoro 0:67a3f3de0758 74 MotorPWM1.period_us(5);
tokoro 0:67a3f3de0758 75 MotorPWM2.period_us(5);
tokoro 0:67a3f3de0758 76 MotorPWM3.period_us(5);
tokoro 0:67a3f3de0758 77 MotorPWM4.period_us(5);
tokoro 0:67a3f3de0758 78
tokoro 0:67a3f3de0758 79 if(mpu.getID()==0x68){//72
tokoro 0:67a3f3de0758 80 pc.printf("MPU6050 OK");
tokoro 0:67a3f3de0758 81 wait(1);
tokoro 0:67a3f3de0758 82 } else {
tokoro 0:67a3f3de0758 83 pc.printf("MPU6050 error ID=0x%x\r\n",mpu.getID());
tokoro 0:67a3f3de0758 84 while(1) {
tokoro 0:67a3f3de0758 85 }
tokoro 0:67a3f3de0758 86 }
tokoro 0:67a3f3de0758 87 mpu.start();
tokoro 0:67a3f3de0758 88 while(1) {
tokoro 0:67a3f3de0758 89 while(1){
tokoro 0:67a3f3de0758 90 MPU6050();
tokoro 0:67a3f3de0758 91 Line_Flont_Value = Line_Flont.read();
tokoro 0:67a3f3de0758 92 Line_Back_Value = Line_Back.read();
tokoro 0:67a3f3de0758 93 Line_Right_Value = Line_Right.read();
tokoro 0:67a3f3de0758 94 Line_Left_Value = Line_Left.read();
tokoro 0:67a3f3de0758 95
tokoro 0:67a3f3de0758 96 Line_Print();
tokoro 0:67a3f3de0758 97 if(Line_Right_Value > Line_Right_Judg){
tokoro 0:67a3f3de0758 98 Buzzer = 1;
tokoro 0:67a3f3de0758 99 while(1){
tokoro 0:67a3f3de0758 100 Motor_Direction(-90,50);
tokoro 0:67a3f3de0758 101 wait(0.2);
tokoro 0:67a3f3de0758 102 Line_Right_Value = Line_Right.read();
tokoro 0:67a3f3de0758 103 if(Line_Right_Value < Line_Right_Judg){
tokoro 0:67a3f3de0758 104 Buzzer = 0;
tokoro 0:67a3f3de0758 105 Motor_Stop();
tokoro 0:67a3f3de0758 106 wait(0.2);
tokoro 0:67a3f3de0758 107 break;
tokoro 0:67a3f3de0758 108 }
tokoro 0:67a3f3de0758 109 }
tokoro 0:67a3f3de0758 110 }else if(Line_Left_Value > Line_Left_Judg){
tokoro 0:67a3f3de0758 111 Buzzer = 1;
tokoro 0:67a3f3de0758 112 while(1){
tokoro 0:67a3f3de0758 113 Motor_Direction(90,50);
tokoro 0:67a3f3de0758 114 wait(0.2);
tokoro 0:67a3f3de0758 115 Line_Left_Value = Line_Left.read();
tokoro 0:67a3f3de0758 116 if(Line_Left_Value < Line_Left_Judg){
tokoro 0:67a3f3de0758 117 Buzzer = 0;
tokoro 0:67a3f3de0758 118 Motor_Stop();
tokoro 0:67a3f3de0758 119 wait(0.2);
tokoro 0:67a3f3de0758 120 break;
tokoro 0:67a3f3de0758 121 }
tokoro 0:67a3f3de0758 122 }
tokoro 0:67a3f3de0758 123 }else if(Line_Flont_Value > Line_Flont_Judg){
tokoro 0:67a3f3de0758 124 Buzzer = 1;
tokoro 0:67a3f3de0758 125 while(1){
tokoro 0:67a3f3de0758 126 Motor_Direction(180,50);
tokoro 0:67a3f3de0758 127 wait(0.5);
tokoro 0:67a3f3de0758 128 Line_Flont_Value = Line_Flont.read();
tokoro 0:67a3f3de0758 129 if(Line_Flont_Value < Line_Flont_Judg){
tokoro 0:67a3f3de0758 130 Buzzer = 0;
tokoro 0:67a3f3de0758 131 Motor_Stop();
tokoro 0:67a3f3de0758 132 wait(0.5);
tokoro 0:67a3f3de0758 133 break;
tokoro 0:67a3f3de0758 134 }
tokoro 0:67a3f3de0758 135 }
tokoro 0:67a3f3de0758 136 }else if(Line_Back_Value > Line_Back_Judg){
tokoro 0:67a3f3de0758 137 Buzzer = 1;
tokoro 0:67a3f3de0758 138 while(1){
tokoro 0:67a3f3de0758 139 Motor_Direction(0,50);
tokoro 0:67a3f3de0758 140 wait(0.2);
tokoro 0:67a3f3de0758 141 Line_Back_Value = Line_Back.read();
tokoro 0:67a3f3de0758 142 if(Line_Back_Value < Line_Back_Judg){
tokoro 0:67a3f3de0758 143 Buzzer = 0;
tokoro 0:67a3f3de0758 144 Motor_Stop();
tokoro 0:67a3f3de0758 145 wait(0.2);
tokoro 0:67a3f3de0758 146 break;
tokoro 0:67a3f3de0758 147 }
tokoro 0:67a3f3de0758 148 }
tokoro 0:67a3f3de0758 149 }
tokoro 0:67a3f3de0758 150
tokoro 0:67a3f3de0758 151 Motor_Angle(0);
tokoro 0:67a3f3de0758 152 //Motor_Stop();
tokoro 0:67a3f3de0758 153 Baisepower=50;
tokoro 0:67a3f3de0758 154
tokoro 0:67a3f3de0758 155 Directionpower1=V[0]*Baisepower;
tokoro 0:67a3f3de0758 156 Directionpower2=V[1]*Baisepower;
tokoro 0:67a3f3de0758 157 Directionpower3=V[2]*Baisepower;
tokoro 0:67a3f3de0758 158 Directionpower4=V[3]*Baisepower;
tokoro 0:67a3f3de0758 159
tokoro 0:67a3f3de0758 160 Motor_Speed(1,Directionpower1+Gyro_power);
tokoro 0:67a3f3de0758 161 Motor_Speed(2,Directionpower2+Gyro_power);
tokoro 0:67a3f3de0758 162 Motor_Speed(3,Directionpower3+Gyro_power);
tokoro 0:67a3f3de0758 163 Motor_Speed(4,Directionpower4+Gyro_power);
tokoro 0:67a3f3de0758 164 }
tokoro 0:67a3f3de0758 165 }
tokoro 0:67a3f3de0758 166 }
tokoro 0:67a3f3de0758 167
tokoro 0:67a3f3de0758 168 void Line_Print(){
tokoro 0:67a3f3de0758 169 printf("Float//");
tokoro 0:67a3f3de0758 170 printf("%2f//",Line_Flont_Value);
tokoro 0:67a3f3de0758 171 printf("Back_//");
tokoro 0:67a3f3de0758 172 printf("%2f//",Line_Back_Value);
tokoro 0:67a3f3de0758 173 printf("Right//");
tokoro 0:67a3f3de0758 174 printf("%2f//",Line_Right_Value);
tokoro 0:67a3f3de0758 175 printf("Left//");
tokoro 0:67a3f3de0758 176 printf("%2f//\r\n",Line_Left_Value);
tokoro 0:67a3f3de0758 177 }
tokoro 0:67a3f3de0758 178
tokoro 0:67a3f3de0758 179
tokoro 0:67a3f3de0758 180 void Motor_Speed (int Port,int Power){ //　モータ速度関数（モーター番号、速度｛-100~100｝）　
tokoro 0:67a3f3de0758 181 if(Power>93){
tokoro 0:67a3f3de0758 182 Power=93;
tokoro 0:67a3f3de0758 183 }else if(Power<-93){
tokoro 0:67a3f3de0758 184 Power=-93;
tokoro 0:67a3f3de0758 185 }
tokoro 0:67a3f3de0758 186 float motorPWM=(Power/2)*0.01+0.5; //PWM 0.0~0.5~1.0 回転～停止～回転
tokoro 0:67a3f3de0758 187 if(Port==1){
tokoro 0:67a3f3de0758 188 MotorPWM1.write(motorPWM);
tokoro 0:67a3f3de0758 189 }else if(Port==2){
tokoro 0:67a3f3de0758 190 MotorPWM2.write(motorPWM);
tokoro 0:67a3f3de0758 191 }else if(Port==3){
tokoro 0:67a3f3de0758 192 MotorPWM3.write(motorPWM);
tokoro 0:67a3f3de0758 193 }else if(Port==4){
tokoro 0:67a3f3de0758 194 MotorPWM4.write(motorPWM);
tokoro 0:67a3f3de0758 195 }
tokoro 0:67a3f3de0758 196 }
tokoro 0:67a3f3de0758 197
tokoro 0:67a3f3de0758 198 void Motor_Stop(){
tokoro 0:67a3f3de0758 199 Motor_Speed(1,0);
tokoro 0:67a3f3de0758 200 Motor_Speed(2,0);
tokoro 0:67a3f3de0758 201 Motor_Speed(3,0);
tokoro 0:67a3f3de0758 202 Motor_Speed(4,0);
tokoro 0:67a3f3de0758 203 }
tokoro 0:67a3f3de0758 204
tokoro 0:67a3f3de0758 205 void Motor_Angle(int Dgree){
tokoro 0:67a3f3de0758 206 V[0] = 0.00;
tokoro 0:67a3f3de0758 207 V[1] = 0.00;
tokoro 0:67a3f3de0758 208 V[2] = 0.00;
tokoro 0:67a3f3de0758 209 V[3] = 0.00;
tokoro 0:67a3f3de0758 210
tokoro 0:67a3f3de0758 211 rad = Dgree * pi / 180;
tokoro 0:67a3f3de0758 212
tokoro 0:67a3f3de0758 213 Vx = sin(rad);
tokoro 0:67a3f3de0758 214 Vy = cos(rad);
tokoro 0:67a3f3de0758 215 L = 0.00;
tokoro 0:67a3f3de0758 216
tokoro 0:67a3f3de0758 217 yoso[0] = Vx;
tokoro 0:67a3f3de0758 218 yoso[1] = Vy;
tokoro 0:67a3f3de0758 219 yoso[2] = L;
tokoro 0:67a3f3de0758 220 for (int i = 0; i <= 3; i++){
tokoro 0:67a3f3de0758 221 for (int j = 0; j <= 2; j++){
tokoro 0:67a3f3de0758 222 V[i] = V[i] + (yoso[j] * kesu[i][j]);
tokoro 0:67a3f3de0758 223 }
tokoro 0:67a3f3de0758 224 }
tokoro 0:67a3f3de0758 225 }
tokoro 0:67a3f3de0758 226 void Motor_Direction(int Dgree,int Baisepower){
tokoro 0:67a3f3de0758 227 Motor_Angle(Dgree);
tokoro 0:67a3f3de0758 228 Directionpower1=V[0]*Baisepower;
tokoro 0:67a3f3de0758 229 Directionpower2=V[1]*Baisepower;
tokoro 0:67a3f3de0758 230 Directionpower3=V[2]*Baisepower;
tokoro 0:67a3f3de0758 231 Directionpower4=V[3]*Baisepower;
tokoro 0:67a3f3de0758 232
tokoro 0:67a3f3de0758 233 Motor_Speed(1,Directionpower1);
tokoro 0:67a3f3de0758 234 Motor_Speed(2,Directionpower2);
tokoro 0:67a3f3de0758 235 Motor_Speed(3,Directionpower3);
tokoro 0:67a3f3de0758 236 Motor_Speed(4,Directionpower4);
tokoro 0:67a3f3de0758 237 }
tokoro 0:67a3f3de0758 238
tokoro 0:67a3f3de0758 239 void MPU6050(){
tokoro 0:67a3f3de0758 240 MPU6050timer.stop();
tokoro 0:67a3f3de0758 241 dt=MPU6050timer.read();
tokoro 0:67a3f3de0758 242 mpu.read(&gx,&gy,&gz,&ax,&ay,&az);
tokoro 0:67a3f3de0758 243 MPU6050timer.reset();
tokoro 0:67a3f3de0758 244 MPU6050timer.start();
tokoro 0:67a3f3de0758 245 double gz_=gz;
tokoro 0:67a3f3de0758 246 GZ=0.97*gz_+0.03*preGZ;
tokoro 0:67a3f3de0758 247 if((GZ<=0.95)&&(GZ>=-0.95)){
tokoro 0:67a3f3de0758 248 GZ=0;
tokoro 0:67a3f3de0758 249 }
tokoro 0:67a3f3de0758 250 Degree+=(preGZ+GZ)*dt/2;
tokoro 0:67a3f3de0758 251 preGZ=GZ;
tokoro 0:67a3f3de0758 252 //pc.printf("%f\r\n",GZ);
tokoro 0:67a3f3de0758 253 }