lpc1768 aassssq

Dependencies:   mbed Servo LPS25HB_I2C MPU6050 SDFileSystem

Committer:
kosukesuzuki
Date:
Fri Mar 04 07:53:32 2022 +0000
Revision:
5:e4fe216f01ad
Parent:
4:ec40fbfb90f6
a

Who changed what in which revision?

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