lpc1768 aassssq
Dependencies: mbed Servo LPS25HB_I2C MPU6050 SDFileSystem
main.cpp@5:e4fe216f01ad, 2022-03-04 (annotated)
- Committer:
- kosukesuzuki
- Date:
- Fri Mar 04 07:53:32 2022 +0000
- Revision:
- 5:e4fe216f01ad
- Parent:
- 4:ec40fbfb90f6
a
Who changed what in which revision?
User | Revision | Line number | New 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 | } |