コウスケ スズキ / Mbed 2 deprecated koubousat1-1

lpc1768 aassss

Dependencies:   mbed Servo LPS25HB_I2C MPU6050 SDFileSystem

main.cpp@4:ec40fbfb90f6, 2022-02-28 (annotated)

Committer:
kosukesuzuki
Date:
Mon Feb 28 09:45:43 2022 +0000
Revision:
4:ec40fbfb90f6
Parent:
3:14b178724982 
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kosukesuzuki 3:14b178724982 1 //｢変更｣と書かれている場所の数値は、自分で決めて変える。（主にセンサー数値の補正）
mbed_official 0:bdbd3d6fc5d5 2 #include "mbed.h"
kosukesuzuki 2:194e00108f75 3 #include "MPU6050.h"
kosukesuzuki 2:194e00108f75 4 #include "LPS.h"
mbed_official 0:bdbd3d6fc5d5 5 #include "SDFileSystem.h"
kosukesuzuki 3:14b178724982 6 #include "Servo.h"
kosukesuzuki 3:14b178724982 7
kosukesuzuki 3:14b178724982 8 const double dt = 0.01; //微小時間(変更)
kosukesuzuki 3:14b178724982 9
kosukesuzuki 4:ec40fbfb90f6 10 const double Kp =11; //変更
kosukesuzuki 4:ec40fbfb90f6 11 const double Ki =7; //変更
kosukesuzuki 4:ec40fbfb90f6 12 const double Kd =5; //変更
kosukesuzuki 4:ec40fbfb90f6 13
kosukesuzuki 4:ec40fbfb90f6 14 const double k =0.25; //プロペラの定数(適当に置いたやつ)
kosukesuzuki 3:14b178724982 15
kosukesuzuki 4:ec40fbfb90f6 16 const double mc = 3000; //最大トルク(変更)
kosukesuzuki 3:14b178724982 17
kosukesuzuki 4:ec40fbfb90f6 18 const double T = 0; //目標角度°(変更)
kosukesuzuki 4:ec40fbfb90f6 19
kosukesuzuki 4:ec40fbfb90f6 20 DigitalOut led[]={LED1,LED2,LED3,LED4};
kosukesuzuki 2:194e00108f75 21
kosukesuzuki 2:194e00108f75 22 I2C i2c(p28,p27);
kosukesuzuki 2:194e00108f75 23 LPS ps(i2c);
kosukesuzuki 2:194e00108f75 24 MPU6050 mpu(p9,p10);
kosukesuzuki 2:194e00108f75 25 SDFileSystem sd(p5, p6, p7, p8, "sd");
kosukesuzuki 2:194e00108f75 26 Serial xbee(p13,p14);
kosukesuzuki 2:194e00108f75 27 Serial pc(USBTX,USBRX);
kosukesuzuki 2:194e00108f75 28
kosukesuzuki 4:ec40fbfb90f6 29 Servo myservo1(p21);
kosukesuzuki 4:ec40fbfb90f6 30 Servo myservo2(p22);
kosukesuzuki 4:ec40fbfb90f6 31 Servo myservo3(p23);
kosukesuzuki 4:ec40fbfb90f6 32 Servo myservo4(p24);
kosukesuzuki 3:14b178724982 33
kosukesuzuki 3:14b178724982 34 Timer ti;
kosukesuzuki 3:14b178724982 35
kosukesuzuki 2:194e00108f75 36 int gyro[3];
kosukesuzuki 2:194e00108f75 37 int accel[3];
kosukesuzuki 2:194e00108f75 38
kosukesuzuki 3:14b178724982 39 int count;
kosukesuzuki 2:194e00108f75 40 double GX,GY,GZ;
kosukesuzuki 2:194e00108f75 41 double Tgx,Tgy,Tgz;
kosukesuzuki 2:194e00108f75 42
kosukesuzuki 4:ec40fbfb90f6 43 int cmd;
kosukesuzuki 3:14b178724982 44
mbed_official 0:bdbd3d6fc5d5 45 int main() {
kosukesuzuki 4:ec40fbfb90f6 46 xbee.printf("motor start\r\n");
kosukesuzuki 4:ec40fbfb90f6 47 led[0]=1;
kosukesuzuki 3:14b178724982 48 //モーター準備開始(一時待つ事が必要)
kosukesuzuki 4:ec40fbfb90f6 49 myservo1 = myservo2 = myservo3 = myservo4 = 0.0;
kosukesuzuki 4:ec40fbfb90f6 50 wait(0.5);
kosukesuzuki 4:ec40fbfb90f6 51 myservo1 = myservo2 = myservo3 = myservo4 = 1.0;
kosukesuzuki 3:14b178724982 52 wait(8);
kosukesuzuki 4:ec40fbfb90f6 53 myservo1 = myservo2 = myservo3 = myservo4 = 0.0;
kosukesuzuki 3:14b178724982 54 wait(8);
kosukesuzuki 4:ec40fbfb90f6 55 xbee.printf("motor end\r\n");
kosukesuzuki 3:14b178724982 56 //モーター準備終了
kosukesuzuki 2:194e00108f75 57
kosukesuzuki 4:ec40fbfb90f6 58 led[1]=1;
kosukesuzuki 4:ec40fbfb90f6 59 //約5秒で開始
kosukesuzuki 4:ec40fbfb90f6 60 wait(5);
kosukesuzuki 2:194e00108f75 61
kosukesuzuki 4:ec40fbfb90f6 62 wait(1.0);
kosukesuzuki 4:ec40fbfb90f6 63 led[0]=1;
kosukesuzuki 4:ec40fbfb90f6 64 wait(1.0);
kosukesuzuki 4:ec40fbfb90f6 65 led[0]=0;
kosukesuzuki 4:ec40fbfb90f6 66
kosukesuzuki 4:ec40fbfb90f6 67 led[1]=0;
kosukesuzuki 3:14b178724982 68 xbee.printf("start\r\n");
kosukesuzuki 3:14b178724982 69 pc.printf("start\r\n");
kosukesuzuki 2:194e00108f75 70
kosukesuzuki 3:14b178724982 71 count = 0;
kosukesuzuki 3:14b178724982 72 GX = 0,GY = 0,GZ = 0;
kosukesuzuki 3:14b178724982 73 Tgx = 0,Tgy = 0,Tgz = 0;
kosukesuzuki 2:194e00108f75 74
kosukesuzuki 3:14b178724982 75 if (!ps.init()){
kosukesuzuki 3:14b178724982 76 pc.printf("Failed to autodetect pressure sensor!\r\n");
kosukesuzuki 3:14b178724982 77 while (1);
kosukesuzuki 3:14b178724982 78 }
kosukesuzuki 3:14b178724982 79
kosukesuzuki 3:14b178724982 80 ps.enableDefault();
kosukesuzuki 2:194e00108f75 81
kosukesuzuki 3:14b178724982 82 mkdir("/sd/test1", 0777);
kosukesuzuki 3:14b178724982 83 FILE *fp = fopen("/sd/test1/sdtest1.txt", "w");
mbed_official 0:bdbd3d6fc5d5 84
kosukesuzuki 3:14b178724982 85 fprintf(fp,"start\r\n");
kosukesuzuki 3:14b178724982 86
kosukesuzuki 3:14b178724982 87 ti.start();
kosukesuzuki 3:14b178724982 88 while(1){
kosukesuzuki 3:14b178724982 89 if(fp == NULL) {
kosukesuzuki 3:14b178724982 90 error("Could not open file for write\n");
kosukesuzuki 3:14b178724982 91 }
kosukesuzuki 2:194e00108f75 92
kosukesuzuki 3:14b178724982 93
kosukesuzuki 3:14b178724982 94 //角度算出
kosukesuzuki 3:14b178724982 95 mpu.readGyroData(gyro);
kosukesuzuki 3:14b178724982 96 int gx = gyro[0]+3656-3505-50-1850+2223-24; //変更
kosukesuzuki 4:ec40fbfb90f6 97 int gy = gyro[1]-30+710-140-300+375-50-420+255; //変更
kosukesuzuki 4:ec40fbfb90f6 98 int gz = gyro[2]+5+195-453+300-244-20-10+95+72; //変更
kosukesuzuki 3:14b178724982 99 //printf("%d %d %d\r\n",gx,gy,gz);
kosukesuzuki 2:194e00108f75 100
kosukesuzuki 3:14b178724982 101 double gX = gx*0.02562; //0.0128114995(測定レンジ±500) //変更
kosukesuzuki 3:14b178724982 102 double gY = gy*0.02562; //0.02562299(±250) //変更
kosukesuzuki 3:14b178724982 103 double gZ = gz*0.02562; //変更
kosukesuzuki 2:194e00108f75 104
kosukesuzuki 3:14b178724982 105 int gX1 = gX;
kosukesuzuki 3:14b178724982 106 int gY1 = gY;
kosukesuzuki 3:14b178724982 107 int gZ1 = gZ;
kosukesuzuki 2:194e00108f75 108
kosukesuzuki 3:14b178724982 109 double gX2 = gX1*dt;
kosukesuzuki 3:14b178724982 110 double gY2 = gY1*dt;
kosukesuzuki 3:14b178724982 111 double gZ2 = gZ1*dt;
kosukesuzuki 2:194e00108f75 112
kosukesuzuki 3:14b178724982 113 GX = GX + gX2;
kosukesuzuki 3:14b178724982 114 GY = GY + gY2;
kosukesuzuki 3:14b178724982 115 GZ = GZ + gZ2;
kosukesuzuki 2:194e00108f75 116
kosukesuzuki 3:14b178724982 117 Tgx = Tgx + abs(gX2);
kosukesuzuki 3:14b178724982 118 Tgy = Tgy + abs(gY2);
kosukesuzuki 3:14b178724982 119 Tgz = Tgz + abs(gZ2);
kosukesuzuki 2:194e00108f75 120
kosukesuzuki 3:14b178724982 121 //和の補正(精度はよくない)
kosukesuzuki 3:14b178724982 122 if(Tgx > 3){
kosukesuzuki 3:14b178724982 123 if(GX > 0){
kosukesuzuki 3:14b178724982 124 GX = GX - 0.3;//変更
kosukesuzuki 3:14b178724982 125 }else{
kosukesuzuki 3:14b178724982 126 GX = GX + 0.3;//変更
kosukesuzuki 3:14b178724982 127 }
kosukesuzuki 3:14b178724982 128 Tgx = 0;
kosukesuzuki 2:194e00108f75 129 }
kosukesuzuki 3:14b178724982 130 if(Tgy > 3){
kosukesuzuki 3:14b178724982 131 if(GY >0){
kosukesuzuki 3:14b178724982 132 GY = GY - 0.3;//変更
kosukesuzuki 3:14b178724982 133 }else{
kosukesuzuki 3:14b178724982 134 GY = GY + 0.3;//変更
kosukesuzuki 3:14b178724982 135 }
kosukesuzuki 3:14b178724982 136 Tgy = 0;
kosukesuzuki 3:14b178724982 137 }
kosukesuzuki 3:14b178724982 138 if(Tgz > 3){
kosukesuzuki 3:14b178724982 139 if(GZ >0){
kosukesuzuki 3:14b178724982 140 GZ = GZ - 0.3;//変更
kosukesuzuki 3:14b178724982 141 }else{
kosukesuzuki 3:14b178724982 142 GZ = GZ + 0.3;//変更
kosukesuzuki 3:14b178724982 143 }
kosukesuzuki 3:14b178724982 144 Tgz = 0;
kosukesuzuki 3:14b178724982 145 }
kosukesuzuki 2:194e00108f75 146
kosukesuzuki 3:14b178724982 147
kosukesuzuki 3:14b178724982 148 //加速度算出
kosukesuzuki 3:14b178724982 149 mpu.readAccelData(accel);
kosukesuzuki 3:14b178724982 150 int ax = accel[0]-123; //変更
kosukesuzuki 3:14b178724982 151 int ay = accel[1]+60; //変更
kosukesuzuki 3:14b178724982 152 int az = accel[2]+1110 ; //変更
kosukesuzuki 3:14b178724982 153 float AX = ax*0.000597964111328125; //変更
kosukesuzuki 3:14b178724982 154 float AY = ay*0.000597964111328125; //変更
kosukesuzuki 3:14b178724982 155 float AZ = az*0.000597964111328125; //変更
kosukesuzuki 4:ec40fbfb90f6 156 double ad = AX*AX+AY*AY+AZ*AZ-95.982071137936;
kosukesuzuki 3:14b178724982 157
kosukesuzuki 3:14b178724982 158
kosukesuzuki 3:14b178724982 159 //気圧・標高・温度算出
kosukesuzuki 3:14b178724982 160 float pres = ps.readPressureMillibars();
kosukesuzuki 3:14b178724982 161 float altit = ps.pressureToAltitudeMeters(pres);
kosukesuzuki 3:14b178724982 162 float tempe = ps.readTemperatureC();
kosukesuzuki 3:14b178724982 163
kosukesuzuki 2:194e00108f75 164
kosukesuzuki 3:14b178724982 165 //PID開始
kosukesuzuki 3:14b178724982 166 //必要なトルクを算出
kosukesuzuki 3:14b178724982 167 double c1,c2;
kosukesuzuki 3:14b178724982 168 double f1,f2;
kosukesuzuki 3:14b178724982 169 f1 = T - GX;
kosukesuzuki 3:14b178724982 170 f2 = T - GY;
kosukesuzuki 3:14b178724982 171
kosukesuzuki 4:ec40fbfb90f6 172 c1 = Kp*f1+Ki*f1*dt-Kd*gX1; //トルク
kosukesuzuki 4:ec40fbfb90f6 173 c2 = Kp*f2+Ki*f2*dt-Kd*gY1;
kosukesuzuki 3:14b178724982 174 //printf("%.2f %.2f\r\n",c1,c2);　//トルク
kosukesuzuki 3:14b178724982 175
kosukesuzuki 3:14b178724982 176 //回転トルク→duty比を算出
kosukesuzuki 3:14b178724982 177 double d1,d2;
kosukesuzuki 3:14b178724982 178 d1 = c1/mc;
kosukesuzuki 3:14b178724982 179 d2 = c2/mc;
kosukesuzuki 4:ec40fbfb90f6 180
kosukesuzuki 4:ec40fbfb90f6 181
kosukesuzuki 4:ec40fbfb90f6 182 //
kosukesuzuki 3:14b178724982 183 if(d1 > 0.9){ //変更
kosukesuzuki 3:14b178724982 184 d1 = 0.9; //変更
kosukesuzuki 3:14b178724982 185 }
kosukesuzuki 3:14b178724982 186 if(d2 > 0.9){ //変更
kosukesuzuki 3:14b178724982 187 d2 = 0.9; //変更
kosukesuzuki 3:14b178724982 188 }
kosukesuzuki 4:ec40fbfb90f6 189 printf("%.2f %.2f\r\n",d1,d2); //duty比
kosukesuzuki 4:ec40fbfb90f6 190
kosukesuzuki 3:14b178724982 191 //試行回数
kosukesuzuki 3:14b178724982 192 count = count + 1;
kosukesuzuki 4:ec40fbfb90f6 193
kosukesuzuki 3:14b178724982 194 //duty比からmotorを動かす。
kosukesuzuki 3:14b178724982 195 if(GX > 0){
kosukesuzuki 4:ec40fbfb90f6 196 myservo1 = abs(d1);
kosukesuzuki 4:ec40fbfb90f6 197 myservo2 = abs(d1)*k;
kosukesuzuki 4:ec40fbfb90f6 198 }else if(GX < 0){
kosukesuzuki 4:ec40fbfb90f6 199 myservo2 = abs(d1);
kosukesuzuki 4:ec40fbfb90f6 200 myservo1 = abs(d1)*k;
kosukesuzuki 4:ec40fbfb90f6 201 }else{
kosukesuzuki 4:ec40fbfb90f6 202 myservo1 = myservo2 = 0.025;
kosukesuzuki 4:ec40fbfb90f6 203 }
kosukesuzuki 3:14b178724982 204
kosukesuzuki 4:ec40fbfb90f6 205 if(GY > 0){
kosukesuzuki 4:ec40fbfb90f6 206 myservo3 = abs(d2);
kosukesuzuki 4:ec40fbfb90f6 207 myservo4 = abs(d2)*k;
kosukesuzuki 4:ec40fbfb90f6 208 }else if(GY < 0){
kosukesuzuki 4:ec40fbfb90f6 209 myservo4 = abs(d2);
kosukesuzuki 4:ec40fbfb90f6 210 myservo3 = abs(d2)*k;
kosukesuzuki 4:ec40fbfb90f6 211 }else{
kosukesuzuki 4:ec40fbfb90f6 212 myservo3 = myservo4 = 0.025;
kosukesuzuki 4:ec40fbfb90f6 213 }
kosukesuzuki 2:194e00108f75 214
kosukesuzuki 3:14b178724982 215 /*主なセンサー表示
kosukesuzuki 3:14b178724982 216 pc.printf("T %.2f\r\n",ti.read());
kosukesuzuki 3:14b178724982 217 pc.printf("c %d\r\n",count);
kosukesuzuki 3:14b178724982 218 pc.printf("ax%.2f ay%.2f az%.2f |a|%.2f\r\n",AX,AY,AZ,a);
kosukesuzuki 3:14b178724982 219 pc.printf("gx%.2f gy%.2f gz%.2f\r\n",GX,GY,GZ);
kosukesuzuki 3:14b178724982 220 pc.printf("p%.2f alt%.2f t%.2f\r\n",pres,altit,tempe);
kosukesuzuki 3:14b178724982 221 */
kosukesuzuki 3:14b178724982 222
kosukesuzuki 4:ec40fbfb90f6 223 //pc.printf("c%d, T%.2f, ax%.2f,ay%.2f,az%.2f,|a|%.2f, gx%.2f,gy%.2f,gz%.2f, p%.2f,al%.2f,t%.2f\r\n",count,ti.read(),AX,AY,AZ,a,GX,GY,GZ,pres,altit,tempe);
kosukesuzuki 4:ec40fbfb90f6 224 //xbee.printf("%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",AX,AY,AZ,a,GX,GY,GZ,pres,altit,tempe);
kosukesuzuki 4:ec40fbfb90f6 225 //fprintf(fp, "%d %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f\r\n",count,ti.read(),AX,AY,AZ,a,GX,GY,GZ,pres,altit,tempe,c1,c2,d1,d2);
kosukesuzuki 3:14b178724982 226 //fprintf(fp,"%.2f %.2f %.2f %.2f %.2f\r\n",ti.read(),c1,c2,d1,d2); //調整用
kosukesuzuki 3:14b178724982 227
kosukesuzuki 3:14b178724982 228 wait(dt);
kosukesuzuki 3:14b178724982 229
kosukesuzuki 4:ec40fbfb90f6 230 /* if(AZ > 9 && AZ < 11 && a > 8 && a < 11 && 5 < ti.read()){
kosukesuzuki 3:14b178724982 231 fprintf(fp,"end\r\n");
kosukesuzuki 3:14b178724982 232 fclose(fp); //注意：ここまで行わないと、SDカードに保存されない
kosukesuzuki 3:14b178724982 233 xbee.printf("end\r\n");
kosukesuzuki 3:14b178724982 234 pc.printf("end\r\n");
kosukesuzuki 3:14b178724982 235 ti.stop();
kosukesuzuki 4:ec40fbfb90f6 236 myservo1 = myservo2 = myservo3 = myservo4 = 0.0;
kosukesuzuki 4:ec40fbfb90f6 237 led[2]=1;
kosukesuzuki 4:ec40fbfb90f6 238 break;}
kosukesuzuki 4:ec40fbfb90f6 239 */
kosukesuzuki 3:14b178724982 240 }
kosukesuzuki 3:14b178724982 241
kosukesuzuki 4:ec40fbfb90f6 242 //led[1]=0;
kosukesuzuki 4:ec40fbfb90f6 243 //led[3]=1;
kosukesuzuki 4:ec40fbfb90f6 244
kosukesuzuki 4:ec40fbfb90f6 245 }