hitomi shio / Mbed 2 deprecated Integration_SD

a

Dependencies:   mbed SDFileSystem BMP180

main.cpp@0:0624addc6841, 2021-11-19 (annotated)

Committer:
ShioHitomi
Date:
Fri Nov 19 15:43:11 2021 +0000
Revision:
0:0624addc6841
1119

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ShioHitomi 0:0624addc6841 1 #include "mbed.h"
ShioHitomi 0:0624addc6841 2 #include "math.h"
ShioHitomi 0:0624addc6841 3 #include "BMP180.h"
ShioHitomi 0:0624addc6841 4 #include "SDFileSystem.h"
ShioHitomi 0:0624addc6841 5
ShioHitomi 0:0624addc6841 6 #define p0 1013.25f //海面気圧
ShioHitomi 0:0624addc6841 7 #define PI 3.1415
ShioHitomi 0:0624addc6841 8 #define Alt_num 3.0 //高度を何回の平均値にするか
ShioHitomi 0:0624addc6841 9 #define Angle_num 3 //姿勢角を何回の平均値にするか
ShioHitomi 0:0624addc6841 10 #define Angle_cnt 10 //何回姿勢角取得するか
ShioHitomi 0:0624addc6841 11 #define Alt_bou 30 //この高度より低かったらFlight mode入ったとみなす
ShioHitomi 0:0624addc6841 12
ShioHitomi 0:0624addc6841 13 #define Z_0 0.962 //KXTC9-2050_rawを使ってキャリブレーションする
ShioHitomi 0:0624addc6841 14 #define Z_90 1.62
ShioHitomi 0:0624addc6841 15 #define Z_180 2.24
ShioHitomi 0:0624addc6841 16
ShioHitomi 0:0624addc6841 17 enum PHASE{CHECK, STANDBY, FLIGHT, EXPANSION, MISSION, WITHDRAW} Phase;
ShioHitomi 0:0624addc6841 18
ShioHitomi 0:0624addc6841 19 Serial pc(USBTX, USBRX, 115200);
ShioHitomi 0:0624addc6841 20 Serial twe(p28, p27, 115200);
ShioHitomi 0:0624addc6841 21 DigitalOut myled_1(LED1);
ShioHitomi 0:0624addc6841 22 DigitalOut myled_2(LED2);
ShioHitomi 0:0624addc6841 23 DigitalOut myled_3(LED3);
ShioHitomi 0:0624addc6841 24 DigitalOut myled_4(LED4);
ShioHitomi 0:0624addc6841 25 DigitalIn Flight_IN(p20);
ShioHitomi 0:0624addc6841 26 //SDFileSystem sd(p5, p6, p7, p8, "sd");
ShioHitomi 0:0624addc6841 27 BMP180 bmp(p9, p10); //(sda, scl)
ShioHitomi 0:0624addc6841 28 AnalogIn z(p19); //加速度センサ
ShioHitomi 0:0624addc6841 29 DigitalOut load_sck(p13); //ロードセル
ShioHitomi 0:0624addc6841 30 DigitalIn load_data(p14);
ShioHitomi 0:0624addc6841 31 DigitalOut fet(p18); //ニクロム用MOSFET
ShioHitomi 0:0624addc6841 32
ShioHitomi 0:0624addc6841 33 PwmOut servo(p24);
ShioHitomi 0:0624addc6841 34 PwmOut motor1_1(p25); //motor1：回転用
ShioHitomi 0:0624addc6841 35 PwmOut motor1_2(p26);
ShioHitomi 0:0624addc6841 36 DigitalOut motor2_1(p22); //motor2：上昇下降用
ShioHitomi 0:0624addc6841 37 DigitalOut motor2_2(p23);
ShioHitomi 0:0624addc6841 38 PwmOut motor2_pwm(p21);
ShioHitomi 0:0624addc6841 39
ShioHitomi 0:0624addc6841 40 Timer time_0;
ShioHitomi 0:0624addc6841 41 Timer time_flight;
ShioHitomi 0:0624addc6841 42
ShioHitomi 0:0624addc6841 43 int _getTime(); //time_0取得
ShioHitomi 0:0624addc6841 44 float _getAlt(); //高度取得
ShioHitomi 0:0624addc6841 45 float _getAngle(); //姿勢角取得
ShioHitomi 0:0624addc6841 46 int _averageLoad(uint8_t times); //ロードセルのキャリブレーションする時に使う
ShioHitomi 0:0624addc6841 47 int _getLoad();
ShioHitomi 0:0624addc6841 48 float _getGram(); //質量取得
ShioHitomi 0:0624addc6841 49 void _Rand_judge(); //着地判定
ShioHitomi 0:0624addc6841 50 //void _Angle_judge();
ShioHitomi 0:0624addc6841 51
ShioHitomi 0:0624addc6841 52 int getTime;
ShioHitomi 0:0624addc6841 53 float getAlt, getAngle, getGram;;
ShioHitomi 0:0624addc6841 54 float Load_offset = 0;
ShioHitomi 0:0624addc6841 55
ShioHitomi 0:0624addc6841 56 FILE *fp;
ShioHitomi 0:0624addc6841 57
ShioHitomi 0:0624addc6841 58 int main(){
ShioHitomi 0:0624addc6841 59
ShioHitomi 0:0624addc6841 60 /*タイマー*/
ShioHitomi 0:0624addc6841 61 time_0.start();
ShioHitomi 0:0624addc6841 62
ShioHitomi 0:0624addc6841 63 /*SD
ShioHitomi 0:0624addc6841 64 fp = fopen("/sd/test.txt", "a");
ShioHitomi 0:0624addc6841 65 fprintf(fp, "Start.\r\n");
ShioHitomi 0:0624addc6841 66 fclose(fp);
ShioHitomi 0:0624addc6841 67 */
ShioHitomi 0:0624addc6841 68
ShioHitomi 0:0624addc6841 69 /*BMP180*/
ShioHitomi 0:0624addc6841 70 bmp.Initialize(64,BMP180_OSS_ULTRA_LOW_POWER);
ShioHitomi 0:0624addc6841 71
ShioHitomi 0:0624addc6841 72 /*ロードセル*/
ShioHitomi 0:0624addc6841 73 load_sck = 1;
ShioHitomi 0:0624addc6841 74 wait_us(100);
ShioHitomi 0:0624addc6841 75 load_sck = 0;
ShioHitomi 0:0624addc6841 76 Load_offset = _averageLoad(10);
ShioHitomi 0:0624addc6841 77
ShioHitomi 0:0624addc6841 78
ShioHitomi 0:0624addc6841 79 /*サーボ*/
ShioHitomi 0:0624addc6841 80 servo.period(0.020);
ShioHitomi 0:0624addc6841 81
ShioHitomi 0:0624addc6841 82 Phase = CHECK;
ShioHitomi 0:0624addc6841 83
ShioHitomi 0:0624addc6841 84 switch(Phase){
ShioHitomi 0:0624addc6841 85 case CHECK:
ShioHitomi 0:0624addc6841 86
ShioHitomi 0:0624addc6841 87 pc.printf("Check mode.\r\n");
ShioHitomi 0:0624addc6841 88 twe.printf("Check mode.\r\n");
ShioHitomi 0:0624addc6841 89 twe.printf("Check mode.\r\n");
ShioHitomi 0:0624addc6841 90
ShioHitomi 0:0624addc6841 91 /*
ShioHitomi 0:0624addc6841 92 fp = fopen("/sd/test.txt", "a");
ShioHitomi 0:0624addc6841 93 fprintf(fp, "Check mode.\r\n");
ShioHitomi 0:0624addc6841 94 fclose(fp);
ShioHitomi 0:0624addc6841 95 */
ShioHitomi 0:0624addc6841 96
ShioHitomi 0:0624addc6841 97 /*センサ値取得*/
ShioHitomi 0:0624addc6841 98 for(int i=0; i<30; i++){
ShioHitomi 0:0624addc6841 99
ShioHitomi 0:0624addc6841 100 getTime = _getTime();
ShioHitomi 0:0624addc6841 101 getAlt = _getAlt();
ShioHitomi 0:0624addc6841 102 getAngle = _getAngle();
ShioHitomi 0:0624addc6841 103 getGram = _getGram();
ShioHitomi 0:0624addc6841 104
ShioHitomi 0:0624addc6841 105 pc.printf("Time: %d Alt: %f Angle: %f Gram: %f\r\n", getTime, getAlt, getAngle, getGram);
ShioHitomi 0:0624addc6841 106 twe.printf("Time: %d Alt: %f Angle: %f Gram: %f\r\n", getTime, getAlt, getAngle, getGram);
ShioHitomi 0:0624addc6841 107 wait(1.0);
ShioHitomi 0:0624addc6841 108
ShioHitomi 0:0624addc6841 109 }
ShioHitomi 0:0624addc6841 110 /*
ShioHitomi 0:0624addc6841 111 fp = fopen("/sd/test.txt", "a");
ShioHitomi 0:0624addc6841 112 fprintf(fp, "Time: %d\r\n Alt: %f\r\n Angle: %f\r\n Gram: %f\r\n", getTime, getAlt, getAngle, getGram);
ShioHitomi 0:0624addc6841 113 fclose(fp);
ShioHitomi 0:0624addc6841 114 */
ShioHitomi 0:0624addc6841 115 myled_1 = 1;
ShioHitomi 0:0624addc6841 116 Phase = STANDBY;
ShioHitomi 0:0624addc6841 117 //break;
ShioHitomi 0:0624addc6841 118
ShioHitomi 0:0624addc6841 119 case STANDBY:
ShioHitomi 0:0624addc6841 120
ShioHitomi 0:0624addc6841 121 //pc.printf("Standby mode.\r\n");
ShioHitomi 0:0624addc6841 122 twe.printf("Standby mode.\r\n");
ShioHitomi 0:0624addc6841 123 twe.printf("Standby mode.\r\n");
ShioHitomi 0:0624addc6841 124 /*
ShioHitomi 0:0624addc6841 125 fp = fopen("/sd/test.txt", "a");
ShioHitomi 0:0624addc6841 126 fprintf(fp, "Standby mode.\r\n");
ShioHitomi 0:0624addc6841 127 fclose(fp);
ShioHitomi 0:0624addc6841 128 */
ShioHitomi 0:0624addc6841 129 /*放出判定*/
ShioHitomi 0:0624addc6841 130 int fly_cnt = 0;
ShioHitomi 0:0624addc6841 131 while(1) {
ShioHitomi 0:0624addc6841 132 int FlightPin = Flight_IN;
ShioHitomi 0:0624addc6841 133 getAlt = _getAlt();
ShioHitomi 0:0624addc6841 134 //if((FlightPin == 0) && (getAlt < Alt_bou)){
ShioHitomi 0:0624addc6841 135 if(FlightPin == 0){
ShioHitomi 0:0624addc6841 136 time_flight.start();
ShioHitomi 0:0624addc6841 137 break;
ShioHitomi 0:0624addc6841 138 }else{
ShioHitomi 0:0624addc6841 139 fly_cnt++;
ShioHitomi 0:0624addc6841 140 getTime = _getTime();
ShioHitomi 0:0624addc6841 141 getAlt = _getAlt();
ShioHitomi 0:0624addc6841 142 getAngle = _getAngle();
ShioHitomi 0:0624addc6841 143 if(fly_cnt == 100){ //100回に1回データ保存
ShioHitomi 0:0624addc6841 144 fly_cnt = 0;
ShioHitomi 0:0624addc6841 145 /*
ShioHitomi 0:0624addc6841 146 fp = fopen("/sd/test.txt", "a");
ShioHitomi 0:0624addc6841 147 fprintf(fp, "Time: %d Alt: %f Angle: %f\r\n", getTime, getAlt, getAngle);
ShioHitomi 0:0624addc6841 148 fclose(fp);
ShioHitomi 0:0624addc6841 149 */
ShioHitomi 0:0624addc6841 150 }
ShioHitomi 0:0624addc6841 151 }
ShioHitomi 0:0624addc6841 152 }
ShioHitomi 0:0624addc6841 153
ShioHitomi 0:0624addc6841 154 myled_2 = 1;
ShioHitomi 0:0624addc6841 155 Phase = FLIGHT;
ShioHitomi 0:0624addc6841 156
ShioHitomi 0:0624addc6841 157 case FLIGHT:
ShioHitomi 0:0624addc6841 158
ShioHitomi 0:0624addc6841 159 //pc.printf("Flight mode.\r\n");
ShioHitomi 0:0624addc6841 160 twe.printf("Flight mode.\r\n");
ShioHitomi 0:0624addc6841 161 twe.printf("Flight mode.\r\n");
ShioHitomi 0:0624addc6841 162 /*
ShioHitomi 0:0624addc6841 163 fp = fopen("/sd/test.txt", "a");
ShioHitomi 0:0624addc6841 164 fprintf(fp, "Flight mode.\r\n");
ShioHitomi 0:0624addc6841 165 fclose(fp);
ShioHitomi 0:0624addc6841 166 */
ShioHitomi 0:0624addc6841 167 /*着地判定*/
ShioHitomi 0:0624addc6841 168 _Rand_judge();
ShioHitomi 0:0624addc6841 169
ShioHitomi 0:0624addc6841 170 //pc.printf("Rand judge success!!\r\n");
ShioHitomi 0:0624addc6841 171 twe.printf("Rand judge success!!\r\n");
ShioHitomi 0:0624addc6841 172 twe.printf("Rand judge success!!\r\n");
ShioHitomi 0:0624addc6841 173
ShioHitomi 0:0624addc6841 174 myled_3 = 1;
ShioHitomi 0:0624addc6841 175 Phase = EXPANSION;
ShioHitomi 0:0624addc6841 176
ShioHitomi 0:0624addc6841 177 case EXPANSION:
ShioHitomi 0:0624addc6841 178
ShioHitomi 0:0624addc6841 179 //pc.printf("Expansion mode.\r\n");
ShioHitomi 0:0624addc6841 180 twe.printf("Expansion mode.\r\n");
ShioHitomi 0:0624addc6841 181 twe.printf("Expansion mode.\r\n");
ShioHitomi 0:0624addc6841 182 /*
ShioHitomi 0:0624addc6841 183 fp = fopen("/sd/test.txt", "a");
ShioHitomi 0:0624addc6841 184 fprintf(fp, "Expansion mode.\r\n");
ShioHitomi 0:0624addc6841 185 fclose(fp);
ShioHitomi 0:0624addc6841 186 */
ShioHitomi 0:0624addc6841 187 wait(30);
ShioHitomi 0:0624addc6841 188
ShioHitomi 0:0624addc6841 189 /*ニクロム線動作*/
ShioHitomi 0:0624addc6841 190 fet = 1;
ShioHitomi 0:0624addc6841 191 wait(0.3);
ShioHitomi 0:0624addc6841 192 fet = 0;
ShioHitomi 0:0624addc6841 193 wait(30.0);
ShioHitomi 0:0624addc6841 194
ShioHitomi 0:0624addc6841 195
ShioHitomi 0:0624addc6841 196 for(int i=0; i<30; i++){
ShioHitomi 0:0624addc6841 197
ShioHitomi 0:0624addc6841 198 getAngle = _getAngle();
ShioHitomi 0:0624addc6841 199
ShioHitomi 0:0624addc6841 200 //pc.printf("Angle: %f\r\n", getAngle);
ShioHitomi 0:0624addc6841 201 twe.printf("Angle: %f\r\n", getAngle);
ShioHitomi 0:0624addc6841 202 /*
ShioHitomi 0:0624addc6841 203 fp = fopen("/sd/test.txt", "a"); //毎回角度保存してるけどどうしようかな
ShioHitomi 0:0624addc6841 204 fprintf(fp, "Angle: %f\r\n", getAngle);
ShioHitomi 0:0624addc6841 205 fclose(fp);
ShioHitomi 0:0624addc6841 206 */
ShioHitomi 0:0624addc6841 207 wait(1.0);
ShioHitomi 0:0624addc6841 208
ShioHitomi 0:0624addc6841 209 }
ShioHitomi 0:0624addc6841 210
ShioHitomi 0:0624addc6841 211 myled_4 = 1;
ShioHitomi 0:0624addc6841 212 Phase = MISSION;
ShioHitomi 0:0624addc6841 213
ShioHitomi 0:0624addc6841 214 case MISSION:
ShioHitomi 0:0624addc6841 215
ShioHitomi 0:0624addc6841 216 //pc.printf("Mission mode.\r\n");
ShioHitomi 0:0624addc6841 217 twe.printf("Mission mode.\r\n");
ShioHitomi 0:0624addc6841 218 twe.printf("Mission mode.\r\n");
ShioHitomi 0:0624addc6841 219 /*
ShioHitomi 0:0624addc6841 220 fp = fopen("/sd/test.txt", "a");
ShioHitomi 0:0624addc6841 221 fprintf(fp, "Mission mode.\r\n");
ShioHitomi 0:0624addc6841 222 fclose(fp);
ShioHitomi 0:0624addc6841 223 */
ShioHitomi 0:0624addc6841 224
ShioHitomi 0:0624addc6841 225 load_sck = 1;
ShioHitomi 0:0624addc6841 226 wait_us(100);
ShioHitomi 0:0624addc6841 227 load_sck = 0;
ShioHitomi 0:0624addc6841 228
ShioHitomi 0:0624addc6841 229 for(int i=0; i<10; i++){
ShioHitomi 0:0624addc6841 230 getGram += _getGram();
ShioHitomi 0:0624addc6841 231 wait(1.0);
ShioHitomi 0:0624addc6841 232 }
ShioHitomi 0:0624addc6841 233 float Gram_0 = 0.0;
ShioHitomi 0:0624addc6841 234 Gram_0 = getGram/10.0;
ShioHitomi 0:0624addc6841 235
ShioHitomi 0:0624addc6841 236 //pc.printf("Gram_0: %f\r\n", Gram_0);
ShioHitomi 0:0624addc6841 237 twe.printf("Gram_0: %f\r\n", Gram_0);
ShioHitomi 0:0624addc6841 238 /*
ShioHitomi 0:0624addc6841 239 fp = fopen("/sd/test.txt", "a");
ShioHitomi 0:0624addc6841 240 fprintf(fp, "Gram_0: %f\r\n", Gram_0);
ShioHitomi 0:0624addc6841 241 fclose(fp);
ShioHitomi 0:0624addc6841 242 */
ShioHitomi 0:0624addc6841 243 /*ちょっとあげる*/
ShioHitomi 0:0624addc6841 244 motor2_pwm = 1.0;
ShioHitomi 0:0624addc6841 245 motor2_1 = 1;
ShioHitomi 0:0624addc6841 246 motor2_2 = 0;
ShioHitomi 0:0624addc6841 247 wait(3.0);
ShioHitomi 0:0624addc6841 248
ShioHitomi 0:0624addc6841 249 /*サーボ*/
ShioHitomi 0:0624addc6841 250 servo.pulsewidth(0.0015);
ShioHitomi 0:0624addc6841 251 wait(1.0);
ShioHitomi 0:0624addc6841 252 servo.pulsewidth(0.0018);
ShioHitomi 0:0624addc6841 253 wait(1.0);
ShioHitomi 0:0624addc6841 254 servo.pulsewidth(0.0020);
ShioHitomi 0:0624addc6841 255 wait(1.0);
ShioHitomi 0:0624addc6841 256 servo.pulsewidth(0.0022);
ShioHitomi 0:0624addc6841 257 wait(1.0);
ShioHitomi 0:0624addc6841 258 servo.pulsewidth(0.0023);
ShioHitomi 0:0624addc6841 259 wait(1.0);
ShioHitomi 0:0624addc6841 260 //servo.pulsewidth(0.0026);
ShioHitomi 0:0624addc6841 261 //wait(1.0);
ShioHitomi 0:0624addc6841 262 /*
ShioHitomi 0:0624addc6841 263 servo.pulsewidth(0.0030);
ShioHitomi 0:0624addc6841 264 */
ShioHitomi 0:0624addc6841 265 /*ドリル動作　motor1:回転、motor2:下降上昇*/
ShioHitomi 0:0624addc6841 266 motor2_pwm = 1.0;
ShioHitomi 0:0624addc6841 267 motor2_1 = 0; //下降・回転
ShioHitomi 0:0624addc6841 268 motor2_2 = 1;
ShioHitomi 0:0624addc6841 269 motor1_1 = 1.0;
ShioHitomi 0:0624addc6841 270 motor1_2 = 0.0;
ShioHitomi 0:0624addc6841 271 wait(9.0);
ShioHitomi 0:0624addc6841 272 motor2_1 = 0; //止まる・回転
ShioHitomi 0:0624addc6841 273 motor2_2 = 0;
ShioHitomi 0:0624addc6841 274 motor1_1 = 1.0;
ShioHitomi 0:0624addc6841 275 motor1_2 = 0.0;
ShioHitomi 0:0624addc6841 276 wait(2.0);
ShioHitomi 0:0624addc6841 277 motor1_1 = 0.0; //止まる・回転停止
ShioHitomi 0:0624addc6841 278 motor1_2 = 0.0;
ShioHitomi 0:0624addc6841 279 wait(3.0);
ShioHitomi 0:0624addc6841 280 motor2_1 = 1; //上昇・回転停止
ShioHitomi 0:0624addc6841 281 motor2_2 = 0;
ShioHitomi 0:0624addc6841 282 motor1_1 = 0.0;
ShioHitomi 0:0624addc6841 283 motor1_2 = 0.0;
ShioHitomi 0:0624addc6841 284 wait(15.0);
ShioHitomi 0:0624addc6841 285 motor2_1 = 0; //止まる・回転停止
ShioHitomi 0:0624addc6841 286 motor2_2 = 0;
ShioHitomi 0:0624addc6841 287 motor1_1 = 0.0;
ShioHitomi 0:0624addc6841 288 motor1_2 = 0.0;
ShioHitomi 0:0624addc6841 289 wait(2.0);
ShioHitomi 0:0624addc6841 290
ShioHitomi 0:0624addc6841 291 /*サーボ動作*/
ShioHitomi 0:0624addc6841 292 servo.pulsewidth(0.0023);
ShioHitomi 0:0624addc6841 293 wait(1.0);
ShioHitomi 0:0624addc6841 294 servo.pulsewidth(0.0022);
ShioHitomi 0:0624addc6841 295 wait(1.0);
ShioHitomi 0:0624addc6841 296 servo.pulsewidth(0.0020);
ShioHitomi 0:0624addc6841 297 wait(1.0);
ShioHitomi 0:0624addc6841 298 servo.pulsewidth(0.0018);
ShioHitomi 0:0624addc6841 299 wait(1.0);
ShioHitomi 0:0624addc6841 300 servo.pulsewidth(0.0014);
ShioHitomi 0:0624addc6841 301 wait(1.0);
ShioHitomi 0:0624addc6841 302
ShioHitomi 0:0624addc6841 303 /*サンプルをドリルから取るための回転*/
ShioHitomi 0:0624addc6841 304 motor1_1 = 1.0;
ShioHitomi 0:0624addc6841 305 motor1_2 = 0.0;
ShioHitomi 0:0624addc6841 306 wait(10.0);
ShioHitomi 0:0624addc6841 307 motor1_1 = 0.0;
ShioHitomi 0:0624addc6841 308 motor1_2 = 0.0;
ShioHitomi 0:0624addc6841 309
ShioHitomi 0:0624addc6841 310
ShioHitomi 0:0624addc6841 311 /*質量値取得*/
ShioHitomi 0:0624addc6841 312
ShioHitomi 0:0624addc6841 313 for(int i=0; i<30; i++){
ShioHitomi 0:0624addc6841 314 getGram = _getGram();
ShioHitomi 0:0624addc6841 315 //pc.printf("Gram: %f \r\n", getGram - Gram_0);
ShioHitomi 0:0624addc6841 316 twe.printf("Gram: %f \r\n", getGram - Gram_0);
ShioHitomi 0:0624addc6841 317 /*
ShioHitomi 0:0624addc6841 318 fp = fopen("/sd/test.txt", "a");
ShioHitomi 0:0624addc6841 319 fprintf(fp, "Gram: %f\r\n", getGram - Gram_0);
ShioHitomi 0:0624addc6841 320 fclose(fp);
ShioHitomi 0:0624addc6841 321 */
ShioHitomi 0:0624addc6841 322 wait(1.0);
ShioHitomi 0:0624addc6841 323 }
ShioHitomi 0:0624addc6841 324
ShioHitomi 0:0624addc6841 325 Phase = WITHDRAW;
ShioHitomi 0:0624addc6841 326
ShioHitomi 0:0624addc6841 327 case WITHDRAW:
ShioHitomi 0:0624addc6841 328
ShioHitomi 0:0624addc6841 329 //pc.printf("Withdraw mode.\r\n");
ShioHitomi 0:0624addc6841 330 twe.printf("Withdraw mode.\r\n");
ShioHitomi 0:0624addc6841 331 twe.printf("Withdraw mode.\r\n");
ShioHitomi 0:0624addc6841 332 /*
ShioHitomi 0:0624addc6841 333 fp = fopen("/sd/test.txt", "a");
ShioHitomi 0:0624addc6841 334 fprintf(fp, "Withdraw mode.\r\n");
ShioHitomi 0:0624addc6841 335 fclose(fp);
ShioHitomi 0:0624addc6841 336 */
ShioHitomi 0:0624addc6841 337
ShioHitomi 0:0624addc6841 338 while(1){
ShioHitomi 0:0624addc6841 339
ShioHitomi 0:0624addc6841 340 myled_1 = !myled_1;
ShioHitomi 0:0624addc6841 341 myled_2 = !myled_2;
ShioHitomi 0:0624addc6841 342 myled_3 = !myled_3;
ShioHitomi 0:0624addc6841 343 myled_4 = !myled_4;
ShioHitomi 0:0624addc6841 344
ShioHitomi 0:0624addc6841 345 getTime = _getTime();
ShioHitomi 0:0624addc6841 346 //pc.printf("Time: %d\r\n", getTime);
ShioHitomi 0:0624addc6841 347 twe.printf("Time: %d\r\n", getTime);
ShioHitomi 0:0624addc6841 348
ShioHitomi 0:0624addc6841 349 wait(1.0);
ShioHitomi 0:0624addc6841 350
ShioHitomi 0:0624addc6841 351 }
ShioHitomi 0:0624addc6841 352 }
ShioHitomi 0:0624addc6841 353 }
ShioHitomi 0:0624addc6841 354
ShioHitomi 0:0624addc6841 355
ShioHitomi 0:0624addc6841 356 int _getTime(){ //Timer_0取得
ShioHitomi 0:0624addc6841 357 int time;
ShioHitomi 0:0624addc6841 358 time = time_0.read();
ShioHitomi 0:0624addc6841 359 return time;
ShioHitomi 0:0624addc6841 360 }
ShioHitomi 0:0624addc6841 361
ShioHitomi 0:0624addc6841 362
ShioHitomi 0:0624addc6841 363 float _getAlt(){ //高度取得
ShioHitomi 0:0624addc6841 364 float Tem, Pre, Alt, sum = 0;
ShioHitomi 0:0624addc6841 365
ShioHitomi 0:0624addc6841 366 for(int i=0; i<Alt_num; i++){
ShioHitomi 0:0624addc6841 367 bmp.ReadData(&Tem, &Pre);
ShioHitomi 0:0624addc6841 368 Alt = (pow((p0/Pre), (1.0f/5.257f))-1.0f)*(Tem + 273.15f)/0.0065f;
ShioHitomi 0:0624addc6841 369 sum += Alt;
ShioHitomi 0:0624addc6841 370 }
ShioHitomi 0:0624addc6841 371 return sum/Alt_num;
ShioHitomi 0:0624addc6841 372 }
ShioHitomi 0:0624addc6841 373
ShioHitomi 0:0624addc6841 374 float _getAngle(){
ShioHitomi 0:0624addc6841 375 float Z, rad, angle, Angle_sum = 0;
ShioHitomi 0:0624addc6841 376 for(int i=0; i<Angle_num; i++){
ShioHitomi 0:0624addc6841 377 Z = 3.3*z.read();
ShioHitomi 0:0624addc6841 378 if(Z <= Z_0){
ShioHitomi 0:0624addc6841 379 angle = 0;
ShioHitomi 0:0624addc6841 380 }else if(Z > Z_0 && Z < Z_90){
ShioHitomi 0:0624addc6841 381 rad = asin((Z - Z_0)/(Z_90 - Z_0));
ShioHitomi 0:0624addc6841 382 angle = 180*rad/PI;
ShioHitomi 0:0624addc6841 383 }else if(Z >= Z_90 && Z < Z_180){
ShioHitomi 0:0624addc6841 384 rad = asin((Z - Z_90)/(Z_180 - Z_90));
ShioHitomi 0:0624addc6841 385 angle = 90+180*rad/PI;
ShioHitomi 0:0624addc6841 386 }else{
ShioHitomi 0:0624addc6841 387 angle = 180;
ShioHitomi 0:0624addc6841 388 }
ShioHitomi 0:0624addc6841 389 /*
ShioHitomi 0:0624addc6841 390 if(Z <= Z_180){
ShioHitomi 0:0624addc6841 391 angle = 180;
ShioHitomi 0:0624addc6841 392 }else if(Z > Z_180 && Z < Z_90){
ShioHitomi 0:0624addc6841 393 rad = asin((Z - Z_180)/(Z_90 - Z_180));
ShioHitomi 0:0624addc6841 394 angle = 180 - 180*rad/PI;
ShioHitomi 0:0624addc6841 395 }else if(Z >= Z_90 && Z < Z_0){
ShioHitomi 0:0624addc6841 396 rad = asin((Z - Z_90)/(Z_0 - Z_90));
ShioHitomi 0:0624addc6841 397 angle = 90 - 180*rad/PI;
ShioHitomi 0:0624addc6841 398 }else{
ShioHitomi 0:0624addc6841 399 angle = 0; //全部-180した
ShioHitomi 0:0624addc6841 400 }
ShioHitomi 0:0624addc6841 401 */
ShioHitomi 0:0624addc6841 402 Angle_sum += angle;
ShioHitomi 0:0624addc6841 403 }
ShioHitomi 0:0624addc6841 404 return Angle_sum/Angle_num;
ShioHitomi 0:0624addc6841 405 }
ShioHitomi 0:0624addc6841 406
ShioHitomi 0:0624addc6841 407 void _Rand_judge(){
ShioHitomi 0:0624addc6841 408 float Alt, Alt_old, Alt_new;
ShioHitomi 0:0624addc6841 409 float Alt_sum = 0;
ShioHitomi 0:0624addc6841 410 float Pre, Tem;
ShioHitomi 0:0624addc6841 411 int rand_cnt = 0;
ShioHitomi 0:0624addc6841 412 int flight_time;
ShioHitomi 0:0624addc6841 413
ShioHitomi 0:0624addc6841 414 for(int i=0; i<Alt_num; i++){
ShioHitomi 0:0624addc6841 415 bmp.ReadData(&Tem, &Pre);
ShioHitomi 0:0624addc6841 416 Alt = (pow((p0/Pre), (1.0f/5.257f))-1.0f)*(Tem + 273.15f)/0.0065f;
ShioHitomi 0:0624addc6841 417 Alt_sum += Alt;
ShioHitomi 0:0624addc6841 418 }
ShioHitomi 0:0624addc6841 419 Alt_old = Alt_sum/Alt_num;
ShioHitomi 0:0624addc6841 420 Alt_sum = 0;
ShioHitomi 0:0624addc6841 421
ShioHitomi 0:0624addc6841 422 while(1) {
ShioHitomi 0:0624addc6841 423 for(int j=0; j<Alt_num; j++){
ShioHitomi 0:0624addc6841 424 bmp.ReadData(&Tem, &Pre);
ShioHitomi 0:0624addc6841 425 Alt = (pow((p0/Pre), (1.0f/5.257f))-1.0f)*(Tem + 273.15f)/0.0065f;
ShioHitomi 0:0624addc6841 426 Alt_sum += Alt;
ShioHitomi 0:0624addc6841 427 }
ShioHitomi 0:0624addc6841 428 Alt_new = Alt_sum/Alt_num;
ShioHitomi 0:0624addc6841 429 Alt_sum = 0;
ShioHitomi 0:0624addc6841 430 flight_time = time_flight.read();
ShioHitomi 0:0624addc6841 431 /*
ShioHitomi 0:0624addc6841 432 fp = fopen("/sd/test.txt", "a");
ShioHitomi 0:0624addc6841 433 fprintf(fp, "flight_time: %d , Alt_new: %f, rand_cnt: %d\r\n", flight_time, Alt_new, rand_cnt);
ShioHitomi 0:0624addc6841 434 fclose(fp);
ShioHitomi 0:0624addc6841 435 */
ShioHitomi 0:0624addc6841 436 if(fabsf(Alt_new-Alt_old)<0.8){ //高度差が何m以内の時着地判定クリアとするか
ShioHitomi 0:0624addc6841 437 rand_cnt++;
ShioHitomi 0:0624addc6841 438 Alt_old = Alt_new;
ShioHitomi 0:0624addc6841 439 wait(0.97); //1Hzになるように調整
ShioHitomi 0:0624addc6841 440 if(rand_cnt == 3){ //3回連続 または　60秒
ShioHitomi 0:0624addc6841 441 break;
ShioHitomi 0:0624addc6841 442 }
ShioHitomi 0:0624addc6841 443 }else if(flight_time >= 60){
ShioHitomi 0:0624addc6841 444 break;
ShioHitomi 0:0624addc6841 445
ShioHitomi 0:0624addc6841 446 }else{
ShioHitomi 0:0624addc6841 447 rand_cnt = 0;
ShioHitomi 0:0624addc6841 448 Alt_old = Alt_new;
ShioHitomi 0:0624addc6841 449 wait(0.97);
ShioHitomi 0:0624addc6841 450 }
ShioHitomi 0:0624addc6841 451 //pc.printf("flightTime:%d s, Alt:%f m, cnt:%d \r\n", flight_time, Alt_old, rand_cnt);
ShioHitomi 0:0624addc6841 452 twe.printf("flightTime:%d s, Alt:%f m, cnt:%d \r\n", flight_time, Alt_old, rand_cnt);
ShioHitomi 0:0624addc6841 453 }
ShioHitomi 0:0624addc6841 454 }
ShioHitomi 0:0624addc6841 455
ShioHitomi 0:0624addc6841 456 int _averageLoad(uint8_t times){
ShioHitomi 0:0624addc6841 457 int sum = 0;
ShioHitomi 0:0624addc6841 458 for (int i = 0; i < times; i++){
ShioHitomi 0:0624addc6841 459 sum += _getLoad();
ShioHitomi 0:0624addc6841 460 }
ShioHitomi 0:0624addc6841 461 return sum / times;
ShioHitomi 0:0624addc6841 462 }
ShioHitomi 0:0624addc6841 463
ShioHitomi 0:0624addc6841 464 int _getLoad(){
ShioHitomi 0:0624addc6841 465 int buffer;
ShioHitomi 0:0624addc6841 466 buffer = 0 ;
ShioHitomi 0:0624addc6841 467
ShioHitomi 0:0624addc6841 468 while (load_data.read()==1);
ShioHitomi 0:0624addc6841 469
ShioHitomi 0:0624addc6841 470 for (uint8_t i = 24; i--;){
ShioHitomi 0:0624addc6841 471 load_sck=1;
ShioHitomi 0:0624addc6841 472 buffer = buffer << 1 ;
ShioHitomi 0:0624addc6841 473 if (load_data.read()){
ShioHitomi 0:0624addc6841 474 buffer ++;
ShioHitomi 0:0624addc6841 475 }
ShioHitomi 0:0624addc6841 476 load_sck=0;
ShioHitomi 0:0624addc6841 477 }
ShioHitomi 0:0624addc6841 478
ShioHitomi 0:0624addc6841 479 for (int i = 0; i < 1; i++){
ShioHitomi 0:0624addc6841 480 load_sck=1;
ShioHitomi 0:0624addc6841 481 load_sck=0;
ShioHitomi 0:0624addc6841 482 }
ShioHitomi 0:0624addc6841 483
ShioHitomi 0:0624addc6841 484 buffer = buffer ^ 0x800000;
ShioHitomi 0:0624addc6841 485 return buffer;
ShioHitomi 0:0624addc6841 486 }
ShioHitomi 0:0624addc6841 487
ShioHitomi 0:0624addc6841 488 float _getGram(){
ShioHitomi 0:0624addc6841 489
ShioHitomi 0:0624addc6841 490 long val = (_averageLoad(2) - Load_offset);
ShioHitomi 0:0624addc6841 491
ShioHitomi 0:0624addc6841 492 if(val < 0) val = 0;
ShioHitomi 0:0624addc6841 493
ShioHitomi 0:0624addc6841 494 float scale = 0.0003*4.2987*128.0/100.0; //定格出力：0.0006V，　　定格容量：100g
ShioHitomi 0:0624addc6841 495 float volt;
ShioHitomi 0:0624addc6841 496 float gram;
ShioHitomi 0:0624addc6841 497 volt = val*(4.2987/16777216.0);
ShioHitomi 0:0624addc6841 498 gram = volt/scale;
ShioHitomi 0:0624addc6841 499
ShioHitomi 0:0624addc6841 500 return (float) gram;
ShioHitomi 0:0624addc6841 501 }