hitomi shio / Mbed 2 deprecated Integration

a

Dependencies:   mbed SDFileSystem BMP180

main.cpp@1:b74930a31769, 2021-10-21 (annotated)

Committer:
ShioHitomi
Date:
Thu Oct 21 00:22:55 2021 +0000
Revision:
1:b74930a31769
Parent:
0:5ea8372bdf6e
Child:
2:4c82067004a2
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(30.0);
ShioHitomi 0:5ea8372bdf6e 191
ShioHitomi 0:5ea8372bdf6e 192
ShioHitomi 0:5ea8372bdf6e 193 for(int i=0; i<30; i++){
ShioHitomi 0:5ea8372bdf6e 194
ShioHitomi 0:5ea8372bdf6e 195 getAngle = _getAngle();
ShioHitomi 0:5ea8372bdf6e 196
ShioHitomi 0:5ea8372bdf6e 197 //pc.printf("Angle: %f\r\n", getAngle);
ShioHitomi 0:5ea8372bdf6e 198 twe.printf("Angle: %f\r\n", getAngle);
ShioHitomi 0:5ea8372bdf6e 199 /*
ShioHitomi 0:5ea8372bdf6e 200 fp = fopen("/sd/test.txt", "a"); //毎回角度保存してるけどどうしようかな
ShioHitomi 0:5ea8372bdf6e 201 fprintf(fp, "Angle: %f\r\n", getAngle);
ShioHitomi 0:5ea8372bdf6e 202 fclose(fp);
ShioHitomi 0:5ea8372bdf6e 203 */
ShioHitomi 0:5ea8372bdf6e 204 wait(1.0);
ShioHitomi 0:5ea8372bdf6e 205
ShioHitomi 0:5ea8372bdf6e 206 }
ShioHitomi 0:5ea8372bdf6e 207
ShioHitomi 0:5ea8372bdf6e 208 myled_4 = 1;
ShioHitomi 0:5ea8372bdf6e 209 Phase = MISSION;
ShioHitomi 0:5ea8372bdf6e 210
ShioHitomi 0:5ea8372bdf6e 211 case MISSION:
ShioHitomi 0:5ea8372bdf6e 212
ShioHitomi 0:5ea8372bdf6e 213 //pc.printf("Mission mode.\r\n");
ShioHitomi 0:5ea8372bdf6e 214 twe.printf("Mission mode.\r\n");
ShioHitomi 0:5ea8372bdf6e 215 twe.printf("Mission mode.\r\n");
ShioHitomi 0:5ea8372bdf6e 216 /*
ShioHitomi 0:5ea8372bdf6e 217 fp = fopen("/sd/test.txt", "a");
ShioHitomi 0:5ea8372bdf6e 218 fprintf(fp, "Mission mode.\r\n");
ShioHitomi 0:5ea8372bdf6e 219 fclose(fp);
ShioHitomi 0:5ea8372bdf6e 220 */
ShioHitomi 0:5ea8372bdf6e 221 /*
ShioHitomi 0:5ea8372bdf6e 222 load_sck = 1;
ShioHitomi 0:5ea8372bdf6e 223 wait_us(100);
ShioHitomi 0:5ea8372bdf6e 224 load_sck = 0;
ShioHitomi 0:5ea8372bdf6e 225 Load_offset = _averageLoad(10);
ShioHitomi 0:5ea8372bdf6e 226
ShioHitomi 0:5ea8372bdf6e 227 for(int i=0; i<10; i++){
ShioHitomi 0:5ea8372bdf6e 228 getGram += _getGram();
ShioHitomi 0:5ea8372bdf6e 229 wait(1.0);
ShioHitomi 0:5ea8372bdf6e 230 }
ShioHitomi 0:5ea8372bdf6e 231
ShioHitomi 0:5ea8372bdf6e 232 Load_offset = getGram/10.0;
ShioHitomi 0:5ea8372bdf6e 233
ShioHitomi 0:5ea8372bdf6e 234
ShioHitomi 0:5ea8372bdf6e 235 getGram = 0.0;
ShioHitomi 0:5ea8372bdf6e 236
ShioHitomi 0:5ea8372bdf6e 237 //pc.printf("Load offset: %f\r\n", Load_offset);
ShioHitomi 0:5ea8372bdf6e 238 twe.printf("Load offset: %f\r\n", Load_offset);
ShioHitomi 0:5ea8372bdf6e 239
ShioHitomi 0:5ea8372bdf6e 240 fp = fopen("/sd/test.txt", "a");
ShioHitomi 0:5ea8372bdf6e 241 fprintf(fp, "Load offset: %f\r\n", Load_offset);
ShioHitomi 0:5ea8372bdf6e 242 fclose(fp);
ShioHitomi 0:5ea8372bdf6e 243 */
ShioHitomi 0:5ea8372bdf6e 244
ShioHitomi 1:b74930a31769 245 /*ちょっとあげる*/
ShioHitomi 1:b74930a31769 246 motor2_pwm = 1.0;
ShioHitomi 1:b74930a31769 247 motor2_1 = 1; //上昇・回転停止
ShioHitomi 1:b74930a31769 248 motor2_2 = 0;
ShioHitomi 1:b74930a31769 249 wait(2.0);
ShioHitomi 1:b74930a31769 250
ShioHitomi 0:5ea8372bdf6e 251 /*サーボ*/
ShioHitomi 0:5ea8372bdf6e 252 servo.pulsewidth(0.0015); //90度
ShioHitomi 0:5ea8372bdf6e 253 wait(1.0);
ShioHitomi 0:5ea8372bdf6e 254 servo.pulsewidth(0.0018); //90度
ShioHitomi 0:5ea8372bdf6e 255 wait(1.0);
ShioHitomi 0:5ea8372bdf6e 256 servo.pulsewidth(0.0020); //90度
ShioHitomi 0:5ea8372bdf6e 257 wait(1.0);
ShioHitomi 0:5ea8372bdf6e 258 servo.pulsewidth(0.0022);
ShioHitomi 0:5ea8372bdf6e 259 wait(1.0);
ShioHitomi 0:5ea8372bdf6e 260 servo.pulsewidth(0.0024);
ShioHitomi 0:5ea8372bdf6e 261 wait(1.0);
ShioHitomi 1:b74930a31769 262 servo.pulsewidth(0.0026);
ShioHitomi 1:b74930a31769 263 wait(1.0);
ShioHitomi 0:5ea8372bdf6e 264 /*
ShioHitomi 0:5ea8372bdf6e 265 servo.pulsewidth(0.0030);
ShioHitomi 0:5ea8372bdf6e 266 */
ShioHitomi 0:5ea8372bdf6e 267 /*ドリル動作　motor1:回転、motor2:下降上昇*/
ShioHitomi 0:5ea8372bdf6e 268 motor2_pwm = 1.0;
ShioHitomi 1:b74930a31769 269 motor2_1 = 0; //下降・回転
ShioHitomi 1:b74930a31769 270 motor2_2 = 1;
ShioHitomi 0:5ea8372bdf6e 271 motor1_1 = 1.0;
ShioHitomi 0:5ea8372bdf6e 272 motor1_2 = 0.0;
ShioHitomi 0:5ea8372bdf6e 273 wait(9.0);
ShioHitomi 0:5ea8372bdf6e 274 motor2_1 = 0; //止まる・回転
ShioHitomi 0:5ea8372bdf6e 275 motor2_2 = 0;
ShioHitomi 0:5ea8372bdf6e 276 motor1_1 = 1.0;
ShioHitomi 0:5ea8372bdf6e 277 motor1_2 = 0.0;
ShioHitomi 0:5ea8372bdf6e 278 wait(2.0);
ShioHitomi 0:5ea8372bdf6e 279 motor1_1 = 0.0; //止まる・回転停止
ShioHitomi 0:5ea8372bdf6e 280 motor1_2 = 0.0;
ShioHitomi 0:5ea8372bdf6e 281 wait(3.0);
ShioHitomi 1:b74930a31769 282 motor2_1 = 1; //上昇・回転停止
ShioHitomi 1:b74930a31769 283 motor2_2 = 0;
ShioHitomi 0:5ea8372bdf6e 284 motor1_1 = 0.0;
ShioHitomi 0:5ea8372bdf6e 285 motor1_2 = 0.0;
ShioHitomi 0:5ea8372bdf6e 286 wait(13.0);
ShioHitomi 0:5ea8372bdf6e 287 motor2_1 = 0; //止まる・回転停止
ShioHitomi 0:5ea8372bdf6e 288 motor2_2 = 0;
ShioHitomi 0:5ea8372bdf6e 289 motor1_1 = 0.0;
ShioHitomi 0:5ea8372bdf6e 290 motor1_2 = 0.0;
ShioHitomi 0:5ea8372bdf6e 291 wait(2.0);
ShioHitomi 1:b74930a31769 292
ShioHitomi 0:5ea8372bdf6e 293 /*サーボ動作*/
ShioHitomi 1:b74930a31769 294 servo.pulsewidth(0.0024); //0度
ShioHitomi 1:b74930a31769 295 wait(1.0);
ShioHitomi 0:5ea8372bdf6e 296 servo.pulsewidth(0.0022); //0度
ShioHitomi 0:5ea8372bdf6e 297 wait(1.0);
ShioHitomi 0:5ea8372bdf6e 298 servo.pulsewidth(0.0020); //90度
ShioHitomi 0:5ea8372bdf6e 299 wait(1.0);
ShioHitomi 0:5ea8372bdf6e 300 servo.pulsewidth(0.0018); //90度
ShioHitomi 0:5ea8372bdf6e 301 wait(1.0);
ShioHitomi 0:5ea8372bdf6e 302 servo.pulsewidth(0.0014);
ShioHitomi 0:5ea8372bdf6e 303 wait(1.0);
ShioHitomi 0:5ea8372bdf6e 304
ShioHitomi 0:5ea8372bdf6e 305 motor1_1 = 1.0;
ShioHitomi 0:5ea8372bdf6e 306 motor1_2 = 0.0;
ShioHitomi 0:5ea8372bdf6e 307 wait(10.0);
ShioHitomi 0:5ea8372bdf6e 308 motor1_1 = 0.0;
ShioHitomi 0:5ea8372bdf6e 309 motor1_2 = 0.0;
ShioHitomi 0:5ea8372bdf6e 310
ShioHitomi 0:5ea8372bdf6e 311
ShioHitomi 0:5ea8372bdf6e 312 /*質量値取得
ShioHitomi 0:5ea8372bdf6e 313 load_sck = 1;
ShioHitomi 0:5ea8372bdf6e 314 wait_us(100);
ShioHitomi 0:5ea8372bdf6e 315 load_sck = 0;
ShioHitomi 0:5ea8372bdf6e 316 Load_offset = _averageLoad(10);
ShioHitomi 0:5ea8372bdf6e 317 for(int i=0; i<10; i++){
ShioHitomi 0:5ea8372bdf6e 318 getGram = _getGram();
ShioHitomi 0:5ea8372bdf6e 319 //pc.printf("Gram: %f \r\n", getGram);
ShioHitomi 0:5ea8372bdf6e 320 twe.printf("Gram: %f \r\n", getGram);
ShioHitomi 0:5ea8372bdf6e 321 fp = fopen("/sd/test.txt", "a"); //最後の一回だけ保存してるけどどうしようかな
ShioHitomi 0:5ea8372bdf6e 322 fprintf(fp, "Gram: %f\r\n", getGram);
ShioHitomi 0:5ea8372bdf6e 323 fclose(fp);
ShioHitomi 0:5ea8372bdf6e 324 wait(1.0);
ShioHitomi 0:5ea8372bdf6e 325 }
ShioHitomi 0:5ea8372bdf6e 326 /*
ShioHitomi 0:5ea8372bdf6e 327 fp = fopen("/sd/test.txt", "a"); //最後の一回だけ保存してるけどどうしようかな
ShioHitomi 0:5ea8372bdf6e 328 fprintf(fp, "Gram: %f\r\n", getGram - Load_offset);
ShioHitomi 0:5ea8372bdf6e 329 fclose(fp);
ShioHitomi 0:5ea8372bdf6e 330 */
ShioHitomi 0:5ea8372bdf6e 331 Phase = WITHDRAW;
ShioHitomi 0:5ea8372bdf6e 332
ShioHitomi 0:5ea8372bdf6e 333 case WITHDRAW:
ShioHitomi 0:5ea8372bdf6e 334
ShioHitomi 0:5ea8372bdf6e 335 //pc.printf("Withdraw mode.\r\n");
ShioHitomi 0:5ea8372bdf6e 336 twe.printf("Withdraw mode.\r\n");
ShioHitomi 0:5ea8372bdf6e 337 twe.printf("Withdraw mode.\r\n");
ShioHitomi 0:5ea8372bdf6e 338 /*
ShioHitomi 0:5ea8372bdf6e 339 fp = fopen("/sd/test.txt", "a");
ShioHitomi 0:5ea8372bdf6e 340 fprintf(fp, "Withdraw mode.\r\n");
ShioHitomi 0:5ea8372bdf6e 341 fclose(fp);
ShioHitomi 0:5ea8372bdf6e 342 */
ShioHitomi 0:5ea8372bdf6e 343
ShioHitomi 0:5ea8372bdf6e 344 while(1){
ShioHitomi 0:5ea8372bdf6e 345
ShioHitomi 0:5ea8372bdf6e 346 myled_1 = !myled_1;
ShioHitomi 0:5ea8372bdf6e 347 myled_2 = !myled_2;
ShioHitomi 0:5ea8372bdf6e 348 myled_3 = !myled_3;
ShioHitomi 0:5ea8372bdf6e 349 myled_4 = !myled_4;
ShioHitomi 0:5ea8372bdf6e 350
ShioHitomi 0:5ea8372bdf6e 351 getTime = _getTime();
ShioHitomi 0:5ea8372bdf6e 352 //pc.printf("Time: %d\r\n", getTime);
ShioHitomi 0:5ea8372bdf6e 353 twe.printf("Time: %d\r\n", getTime);
ShioHitomi 0:5ea8372bdf6e 354
ShioHitomi 0:5ea8372bdf6e 355 wait(1.0);
ShioHitomi 0:5ea8372bdf6e 356
ShioHitomi 0:5ea8372bdf6e 357 }
ShioHitomi 0:5ea8372bdf6e 358 }
ShioHitomi 0:5ea8372bdf6e 359 }
ShioHitomi 0:5ea8372bdf6e 360
ShioHitomi 0:5ea8372bdf6e 361
ShioHitomi 0:5ea8372bdf6e 362 int _getTime(){ //Timer_0取得
ShioHitomi 0:5ea8372bdf6e 363 int time;
ShioHitomi 0:5ea8372bdf6e 364 time = time_0.read();
ShioHitomi 0:5ea8372bdf6e 365
ShioHitomi 0:5ea8372bdf6e 366 /*
ShioHitomi 0:5ea8372bdf6e 367 fp = fopen("/sd/test.txt", "a");
ShioHitomi 0:5ea8372bdf6e 368 fprintf(fp, "Time: %d\r\n",time);
ShioHitomi 0:5ea8372bdf6e 369 fclose(fp);
ShioHitomi 0:5ea8372bdf6e 370 */
ShioHitomi 0:5ea8372bdf6e 371
ShioHitomi 0:5ea8372bdf6e 372 //pc.printf("%d\r\n", time);
ShioHitomi 0:5ea8372bdf6e 373 return time;
ShioHitomi 0:5ea8372bdf6e 374 }
ShioHitomi 0:5ea8372bdf6e 375
ShioHitomi 0:5ea8372bdf6e 376
ShioHitomi 0:5ea8372bdf6e 377 float _getAlt(){ //高度取得
ShioHitomi 0:5ea8372bdf6e 378 float Tem, Pre, Alt, sum = 0;
ShioHitomi 0:5ea8372bdf6e 379
ShioHitomi 0:5ea8372bdf6e 380 for(int i=0; i<Alt_num; i++){
ShioHitomi 0:5ea8372bdf6e 381 bmp.ReadData(&Tem, &Pre);
ShioHitomi 0:5ea8372bdf6e 382 Alt = (pow((p0/Pre), (1.0f/5.257f))-1.0f)*(Tem + 273.15f)/0.0065f;
ShioHitomi 0:5ea8372bdf6e 383 sum += Alt;
ShioHitomi 0:5ea8372bdf6e 384 }
ShioHitomi 0:5ea8372bdf6e 385 /*
ShioHitomi 0:5ea8372bdf6e 386 fp = fopen("/sd/test.txt", "a");
ShioHitomi 0:5ea8372bdf6e 387 fprintf(fp, "Alt: %f\r\n",sum/Alt_num);
ShioHitomi 0:5ea8372bdf6e 388 fclose(fp);
ShioHitomi 0:5ea8372bdf6e 389 */
ShioHitomi 0:5ea8372bdf6e 390 //pc.printf("%f, %f, %f\r\n",Tem, Pre, Alt);
ShioHitomi 0:5ea8372bdf6e 391 //pc.printf("Alt: %f\r\n", sum/Alt_num);
ShioHitomi 0:5ea8372bdf6e 392 return sum/Alt_num;
ShioHitomi 0:5ea8372bdf6e 393 }
ShioHitomi 0:5ea8372bdf6e 394
ShioHitomi 0:5ea8372bdf6e 395 float _getAngle(){
ShioHitomi 0:5ea8372bdf6e 396 float Z, rad, angle, Angle_sum = 0;
ShioHitomi 0:5ea8372bdf6e 397 for(int i=0; i<Angle_num; i++){
ShioHitomi 0:5ea8372bdf6e 398 Z = 3.3*z.read();
ShioHitomi 0:5ea8372bdf6e 399 if(Z <= Z_0){
ShioHitomi 0:5ea8372bdf6e 400 angle = 0;
ShioHitomi 0:5ea8372bdf6e 401 }else if(Z > Z_0 && Z < Z_90){
ShioHitomi 0:5ea8372bdf6e 402 rad = asin((Z - Z_0)/(Z_90 - Z_0));
ShioHitomi 0:5ea8372bdf6e 403 angle = 180*rad/PI;
ShioHitomi 0:5ea8372bdf6e 404 }else if(Z >= Z_90 && Z < Z_180){
ShioHitomi 0:5ea8372bdf6e 405 rad = asin((Z - Z_90)/(Z_180 - Z_90));
ShioHitomi 0:5ea8372bdf6e 406 angle = 90+180*rad/PI;
ShioHitomi 0:5ea8372bdf6e 407 }else{
ShioHitomi 0:5ea8372bdf6e 408 angle = 180;
ShioHitomi 0:5ea8372bdf6e 409 }
ShioHitomi 0:5ea8372bdf6e 410 /*
ShioHitomi 0:5ea8372bdf6e 411 if(Z <= Z_180){
ShioHitomi 0:5ea8372bdf6e 412 angle = 180;
ShioHitomi 0:5ea8372bdf6e 413 }else if(Z > Z_180 && Z < Z_90){
ShioHitomi 0:5ea8372bdf6e 414 rad = asin((Z - Z_180)/(Z_90 - Z_180));
ShioHitomi 0:5ea8372bdf6e 415 angle = 180 - 180*rad/PI;
ShioHitomi 0:5ea8372bdf6e 416 }else if(Z >= Z_90 && Z < Z_0){
ShioHitomi 0:5ea8372bdf6e 417 rad = asin((Z - Z_90)/(Z_0 - Z_90));
ShioHitomi 0:5ea8372bdf6e 418 angle = 90 - 180*rad/PI;
ShioHitomi 0:5ea8372bdf6e 419 }else{
ShioHitomi 0:5ea8372bdf6e 420 angle = 0; //全部-180した
ShioHitomi 0:5ea8372bdf6e 421 }
ShioHitomi 0:5ea8372bdf6e 422 */
ShioHitomi 0:5ea8372bdf6e 423 Angle_sum += angle;
ShioHitomi 0:5ea8372bdf6e 424 }
ShioHitomi 0:5ea8372bdf6e 425 //pc.printf("Z:%f ,%f °\r\n", Z, Angle_sum/Angle_num);
ShioHitomi 0:5ea8372bdf6e 426 return Angle_sum/Angle_num;
ShioHitomi 0:5ea8372bdf6e 427 }
ShioHitomi 0:5ea8372bdf6e 428
ShioHitomi 0:5ea8372bdf6e 429 void _Rand_judge(){
ShioHitomi 0:5ea8372bdf6e 430 float Alt, Alt_old, Alt_new;
ShioHitomi 0:5ea8372bdf6e 431 float Alt_sum = 0;
ShioHitomi 0:5ea8372bdf6e 432 float Pre, Tem;
ShioHitomi 0:5ea8372bdf6e 433 int rand_cnt = 0;
ShioHitomi 0:5ea8372bdf6e 434 int flight_time;
ShioHitomi 0:5ea8372bdf6e 435
ShioHitomi 0:5ea8372bdf6e 436 for(int i=0; i<Alt_num; i++){
ShioHitomi 0:5ea8372bdf6e 437 bmp.ReadData(&Tem, &Pre);
ShioHitomi 0:5ea8372bdf6e 438 Alt = (pow((p0/Pre), (1.0f/5.257f))-1.0f)*(Tem + 273.15f)/0.0065f;
ShioHitomi 0:5ea8372bdf6e 439 Alt_sum += Alt;
ShioHitomi 0:5ea8372bdf6e 440 }
ShioHitomi 0:5ea8372bdf6e 441 Alt_old = Alt_sum/Alt_num;
ShioHitomi 0:5ea8372bdf6e 442 Alt_sum = 0;
ShioHitomi 0:5ea8372bdf6e 443
ShioHitomi 0:5ea8372bdf6e 444 while(1) {
ShioHitomi 0:5ea8372bdf6e 445 for(int j=0; j<Alt_num; j++){
ShioHitomi 0:5ea8372bdf6e 446 bmp.ReadData(&Tem, &Pre);
ShioHitomi 0:5ea8372bdf6e 447 Alt = (pow((p0/Pre), (1.0f/5.257f))-1.0f)*(Tem + 273.15f)/0.0065f;
ShioHitomi 0:5ea8372bdf6e 448 Alt_sum += Alt;
ShioHitomi 0:5ea8372bdf6e 449 }
ShioHitomi 0:5ea8372bdf6e 450 Alt_new = Alt_sum/Alt_num;
ShioHitomi 0:5ea8372bdf6e 451 Alt_sum = 0;
ShioHitomi 0:5ea8372bdf6e 452 flight_time = time_flight.read();
ShioHitomi 0:5ea8372bdf6e 453 /* //毎回データ保存してるけどどうしよ
ShioHitomi 0:5ea8372bdf6e 454 fp = fopen("/sd/test.txt", "a");
ShioHitomi 0:5ea8372bdf6e 455 fprintf(fp, "flight_time: %d , Alt_new: %f, rand_cnt: %d\r\n", flight_time, Alt_new, rand_cnt);
ShioHitomi 0:5ea8372bdf6e 456 fclose(fp);
ShioHitomi 0:5ea8372bdf6e 457 */
ShioHitomi 0:5ea8372bdf6e 458 if(fabsf(Alt_new-Alt_old)<0.8){ //何m以内の時着地判定クリアとするか
ShioHitomi 0:5ea8372bdf6e 459 rand_cnt++;
ShioHitomi 0:5ea8372bdf6e 460 Alt_old = Alt_new;
ShioHitomi 0:5ea8372bdf6e 461 wait(0.97); //1Hzになるように調整
ShioHitomi 0:5ea8372bdf6e 462 if(rand_cnt == 3){ //3回連続 または　60秒
ShioHitomi 0:5ea8372bdf6e 463 break;
ShioHitomi 0:5ea8372bdf6e 464 }
ShioHitomi 0:5ea8372bdf6e 465 }else if(flight_time >= 60){
ShioHitomi 0:5ea8372bdf6e 466 break;
ShioHitomi 0:5ea8372bdf6e 467
ShioHitomi 0:5ea8372bdf6e 468 }else{
ShioHitomi 0:5ea8372bdf6e 469 rand_cnt = 0;
ShioHitomi 0:5ea8372bdf6e 470 Alt_old = Alt_new;
ShioHitomi 0:5ea8372bdf6e 471 wait(0.97);
ShioHitomi 0:5ea8372bdf6e 472 }
ShioHitomi 0:5ea8372bdf6e 473 //pc.printf("flightTime:%d s, Alt:%f m, cnt:%d \r\n", flight_time, Alt_old, rand_cnt);
ShioHitomi 0:5ea8372bdf6e 474 twe.printf("flightTime:%d s, Alt:%f m, cnt:%d \r\n", flight_time, Alt_old, rand_cnt);
ShioHitomi 0:5ea8372bdf6e 475 }
ShioHitomi 0:5ea8372bdf6e 476 }
ShioHitomi 0:5ea8372bdf6e 477
ShioHitomi 0:5ea8372bdf6e 478 /*
ShioHitomi 0:5ea8372bdf6e 479 void _Angle_judge(){
ShioHitomi 0:5ea8372bdf6e 480 int i;
ShioHitomi 0:5ea8372bdf6e 481 float Angle;
ShioHitomi 0:5ea8372bdf6e 482 int success_cnt = 0;
ShioHitomi 0:5ea8372bdf6e 483 Angle = _getAngle();
ShioHitomi 0:5ea8372bdf6e 484 Angle = _getAngle(); //なぜか最初の二回は正しい値が出ない
ShioHitomi 0:5ea8372bdf6e 485
ShioHitomi 0:5ea8372bdf6e 486 for(i=0; i<Angle_cnt; i++){
ShioHitomi 0:5ea8372bdf6e 487 Angle = _getAngle();
ShioHitomi 0:5ea8372bdf6e 488 pc.printf("Angle: %f°, cnt: %d\r\n", Angle, success_cnt);
ShioHitomi 0:5ea8372bdf6e 489 //twe.printf("Angle: %f°, cnt: %d\r\n", Angle, success_cnt);
ShioHitomi 0:5ea8372bdf6e 490
ShioHitomi 0:5ea8372bdf6e 491 /*
ShioHitomi 0:5ea8372bdf6e 492 fp = fopen("/sd/test.txt", "a");
ShioHitomi 0:5ea8372bdf6e 493 fprintf(fp, "Angle: %f°, cnt: %d\r\n", Angle, success_cnt);
ShioHitomi 0:5ea8372bdf6e 494 fclose(fp);
ShioHitomi 0:5ea8372bdf6e 495
ShioHitomi 0:5ea8372bdf6e 496
ShioHitomi 0:5ea8372bdf6e 497 wait(1.0);
ShioHitomi 0:5ea8372bdf6e 498 if(Angle < 90){
ShioHitomi 0:5ea8372bdf6e 499 success_cnt++;
ShioHitomi 0:5ea8372bdf6e 500 if(success_cnt == 3){ //3回連続成功すれば
ShioHitomi 0:5ea8372bdf6e 501
ShioHitomi 0:5ea8372bdf6e 502 pc.printf("Angle judge success.");
ShioHitomi 0:5ea8372bdf6e 503 //twe.printf("Angle judge success.");
ShioHitomi 0:5ea8372bdf6e 504 /*
ShioHitomi 0:5ea8372bdf6e 505 fp = fopen("/sd/test.txt", "a");
ShioHitomi 0:5ea8372bdf6e 506 fprintf(fp, "Angle judge success.\r\n");
ShioHitomi 0:5ea8372bdf6e 507 fclose(fp);
ShioHitomi 0:5ea8372bdf6e 508
ShioHitomi 0:5ea8372bdf6e 509 break;
ShioHitomi 0:5ea8372bdf6e 510 }
ShioHitomi 0:5ea8372bdf6e 511
ShioHitomi 0:5ea8372bdf6e 512 }else{
ShioHitomi 0:5ea8372bdf6e 513 success_cnt = 0;
ShioHitomi 0:5ea8372bdf6e 514 if((i+1) == Angle_cnt){
ShioHitomi 0:5ea8372bdf6e 515 pc.printf("Angle judge faild.");
ShioHitomi 0:5ea8372bdf6e 516 //twe.printf("Angle judge faild.");
ShioHitomi 0:5ea8372bdf6e 517 /*
ShioHitomi 0:5ea8372bdf6e 518 fp = fopen("/sd/test.txt", "a");
ShioHitomi 0:5ea8372bdf6e 519 fprintf(fp, "Angle judge failed.\r\n");
ShioHitomi 0:5ea8372bdf6e 520 fclose(fp);
ShioHitomi 0:5ea8372bdf6e 521
ShioHitomi 0:5ea8372bdf6e 522 break;
ShioHitomi 0:5ea8372bdf6e 523 }
ShioHitomi 0:5ea8372bdf6e 524 }
ShioHitomi 0:5ea8372bdf6e 525 }
ShioHitomi 0:5ea8372bdf6e 526 }
ShioHitomi 0:5ea8372bdf6e 527 */
ShioHitomi 0:5ea8372bdf6e 528
ShioHitomi 0:5ea8372bdf6e 529 int _averageLoad(uint8_t times){
ShioHitomi 0:5ea8372bdf6e 530 int sum = 0;
ShioHitomi 0:5ea8372bdf6e 531 for (int i = 0; i < times; i++){
ShioHitomi 0:5ea8372bdf6e 532 sum += _getLoad();
ShioHitomi 0:5ea8372bdf6e 533 }
ShioHitomi 0:5ea8372bdf6e 534 return sum / times;
ShioHitomi 0:5ea8372bdf6e 535 }
ShioHitomi 0:5ea8372bdf6e 536
ShioHitomi 0:5ea8372bdf6e 537 int _getLoad(){
ShioHitomi 0:5ea8372bdf6e 538 int buffer;
ShioHitomi 0:5ea8372bdf6e 539 buffer = 0 ;
ShioHitomi 0:5ea8372bdf6e 540
ShioHitomi 0:5ea8372bdf6e 541 while (load_data.read()==1);
ShioHitomi 0:5ea8372bdf6e 542
ShioHitomi 0:5ea8372bdf6e 543 for (uint8_t i = 24; i--;){
ShioHitomi 0:5ea8372bdf6e 544 load_sck=1;
ShioHitomi 0:5ea8372bdf6e 545 buffer = buffer << 1 ;
ShioHitomi 0:5ea8372bdf6e 546 if (load_data.read()){
ShioHitomi 0:5ea8372bdf6e 547 buffer ++;
ShioHitomi 0:5ea8372bdf6e 548 }
ShioHitomi 0:5ea8372bdf6e 549 load_sck=0;
ShioHitomi 0:5ea8372bdf6e 550 }
ShioHitomi 0:5ea8372bdf6e 551
ShioHitomi 0:5ea8372bdf6e 552 for (int i = 0; i < 1; i++){
ShioHitomi 0:5ea8372bdf6e 553 load_sck=1;
ShioHitomi 0:5ea8372bdf6e 554 load_sck=0;
ShioHitomi 0:5ea8372bdf6e 555 }
ShioHitomi 0:5ea8372bdf6e 556
ShioHitomi 0:5ea8372bdf6e 557 buffer = buffer ^ 0x800000;
ShioHitomi 0:5ea8372bdf6e 558 return buffer;
ShioHitomi 0:5ea8372bdf6e 559 }
ShioHitomi 0:5ea8372bdf6e 560
ShioHitomi 0:5ea8372bdf6e 561 float _getGram(){
ShioHitomi 0:5ea8372bdf6e 562
ShioHitomi 0:5ea8372bdf6e 563 long val = (_averageLoad(2) - Load_offset);
ShioHitomi 0:5ea8372bdf6e 564
ShioHitomi 0:5ea8372bdf6e 565 if(val < 0) val = 0;
ShioHitomi 0:5ea8372bdf6e 566
ShioHitomi 0:5ea8372bdf6e 567 float scale = 0.0003*4.2987*128.0/100.0; //定格出力：0.0006V，　　定格容量：100g
ShioHitomi 0:5ea8372bdf6e 568 float volt;
ShioHitomi 0:5ea8372bdf6e 569 float gram;
ShioHitomi 0:5ea8372bdf6e 570 volt = val*(4.2987/16777216.0);
ShioHitomi 0:5ea8372bdf6e 571 gram = volt/scale;
ShioHitomi 0:5ea8372bdf6e 572
ShioHitomi 0:5ea8372bdf6e 573 return (float) gram;
ShioHitomi 0:5ea8372bdf6e 574 }