コウスケ スズキ / Mbed 2 deprecated koubousattttttttttttttttttttttddooaass

lpc1768 aa

Dependencies:   mbed Servo LPS25HB_I2C MPU6050 SDFileSystem

main.cpp@3:14b178724982, 2022-02-25 (annotated)

Committer:
kosukesuzuki
Date:
Fri Feb 25 06:06:57 2022 +0000
Revision:
3:14b178724982
Parent:
2:194e00108f75 
hh

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