hitomi shio / Mbed 2 deprecated Integration

a

Dependencies:   mbed SDFileSystem BMP180

main.cpp@0:5ea8372bdf6e, 2021-10-12 (annotated)

Committer:
ShioHitomi
Date:
Tue Oct 12 07:44:50 2021 +0000
Revision:
0:5ea8372bdf6e
Child:
1:b74930a31769
a

Who changed what in which revision?

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