pyonta cansat
k
Dependencies: Hm MPL MPU60580 MU2Class SDFileSystem mbed
Diff: main.cpp
- Revision:
- 1:8a25883c423c
- Parent:
- 0:6ddf1386e71d
--- a/main.cpp Mon Sep 11 10:05:39 2017 +0000
+++ b/main.cpp Wed Sep 13 09:44:08 2017 +0000
@@ -55,6 +55,7 @@
unsigned char timer_set;
unsigned char phase;
unsigned char step;
+unsigned char pitch;
//気圧///////
MPL3115A2 APsensor(&i2c, &mp);
@@ -93,7 +94,7 @@
}
else{
rela_max = max_alt -alt;
- if(rela_max > 50)flag2 = flag2+1;
+ if(rela_max > 150)flag2 = flag2+1;
}
}
//着地判定
@@ -136,8 +137,8 @@
void getGPS(){
//目的地 ブラックロック修正
- goal_tokei = 118.9833;
- goal_hokui = 40.92493;
+ goal_tokei = 119.1218;
+ goal_hokui = 40.8797;
c = gps.getc();
if( c=='$' || i == 256){
mode = 0;
@@ -188,7 +189,7 @@
mp.printf("%s",gps_data);
Mu2.printf("@DT02No\r\n");
if(phase == Descending)getAltitude();
- //if(phase == Moving)test_get_direction();
+ if(phase == Moving)step = jump;
}
sprintf(gps_data, "");
}
@@ -307,6 +308,7 @@
double bfy;
double bfx;
+
//3軸からheading算出
void getheading3axis() {
//加速度算出
@@ -318,6 +320,8 @@
phi = atan2(aycal,azcal);
//phi += M_PI;
theta = atan2(-axcal,aycal*sin(phi)+azcal*cos(phi));
+ if(theta > 0)pitch = 1;
+ if(theta < 0)pitch = 0;
//theta += M_PI;
//地磁気算出
@@ -369,30 +373,31 @@
void motor_drive(){
controlmotor1.write(1);
controlmotor2.write(0);
- pin21.write(0.8);
+ pin21.write(1.0);
//if(pin1)mp.printf("Yes!\r\n");
}
//溶断プログラム
+//最終チェックポイント////////////////////////////////
void burning(){
wait(5);
myled1 = 1;
- para = 1;
- wait(1);
+ //para = 1;
+ wait(1);
myled1 = 0;
- para = 0;
+ //para = 0;
wait(5);
myled2 = 1;
- marker = 1;
+ //marker = 1;
wait(1);
myled2 = 0;
- marker = 0;
+ //marker = 0;
wait(5);
myled3 = 1;
- jumparm = 1;
+ //jumparm = 1;
wait(1);
myled3 = 0;
- jumparm = 0;
+ //jumparm = 0;
}
//跳躍モーター駆動まいまいかわいい
void jumping(){
@@ -462,7 +467,7 @@
APsensor.setOffsetTemperature(10);
APsensor.setOffsetPressure(-32);
mkdir("/sd/mydir", 0777);
- min_alt = 4000.0;
+ min_alt = 5000.0;
Flightpin = 1;
//高度初期値
for(int h = 0; h < 10; h++){
@@ -473,9 +478,13 @@
ini_alt = ini_alt/10;
mp.printf("ini_alt:%d\r\n",ini_alt);
int jump_count;
- phase = Preparing;
- wait(900);//開始15分待機
+
+ //最終チェックポイント/////////////////////////////////////////
+ //phase = Preparing;
+ //wait(1500);//開始25分待機
+ //phase = Descending;
//phase = Moving;
+ phase = Landing_Fusing;
/////////////////////////////単機能////////
//wait(5);
@@ -554,71 +563,7 @@
//}
/////////////////////////
-
-
- /*
- //////////////////////////安全試験用/////////
- //振動試験
- myled1 = 0;
- //wait(5);//test
- wait(900);//end to end
- //静荷重
- FILE *sl = fopen("/local/static2.txt", "a");
- wait(10);
- myled1 = 1;
- wait(5);
- timer.reset();
- timer.start();
- //while(val < 30000){ //test
- while(val < 600000){ //end to end
- val = timer.read_ms();
- adxl[0] = 0x32;
- i2c.write(addr,adxl,1);
- i2c.read(0xA7,adxl,6);
- axout = (((int16_t )adxl[1]) << 8) | adxl[0];
- ayout = (((int16_t )adxl[3]) << 8) | adxl[2];
- azout = (((int16_t )adxl[5]) << 8) | adxl[4];
- //pc.printf("time:%d,X:%d,Y:%d,Z:%d\r\n",val,axout,ayout,azout);
- fprintf(sl, "%d,%d,%d,%d\r\n",val,axout,ayout,azout);
- wait_ms(200);
- //wait必要、ＳＤがbusyの状態で送っちゃってるんじゃない？
- }
- fclose(sl);
- timer.reset();
- timer.stop();
- val = 0;
- myled1 = 0;
- //wait(5); //test
- wait(960); //end to end
-
- //開傘衝撃
- FILE *im = fopen("/local/impact2.txt", "a");
- wait(10);
- myled2 = 1;
- timer.reset();
- timer.start();
- //while(val < 10000){ //test
- while(val < 30000){ //end to end
- val = timer.read_ms();
- adxl[0] = 0x32;
- i2c.write(addr,adxl,1);
- i2c.read(0xA7,adxl,6);
- axout = (((int16_t )adxl[1]) << 8) | adxl[0];
- ayout = (((int16_t )adxl[3]) << 8) | adxl[2];
- azout = (((int16_t )adxl[5]) << 8) | adxl[4];
- fprintf(im, "%d,%d,%d,%d\r\n",val,axout,ayout,azout);
- wait_ms(3);
- //wait必要、ＳＤがbusyの状態で送っちゃってるんじゃない？
- }
- fclose(im);
- timer.reset();
- timer.stop();
- val = 0;
- myled2 = 0;
- //wait(30); //test
- wait(600); //end to end
- ///////////////////////////
- */
+
//着地検知用timer
timer.reset();
timer.start();
@@ -631,7 +576,7 @@
wait(0.5);
val = timer.read();
if(flag1 > 20) phase = Rising;
- if( val > 600) phase = Rising; //計25分
+ if( val > 600) phase = Rising; //計35分
break;
case Rising:
myled1 = 0;
@@ -639,15 +584,19 @@
getAltitude();
wait(0.5);
val = timer.read();
- if(flag2 > 10) phase = Descending;
- if(val > 900) phase = Descending;
+ if(flag2 > 40) phase = Descending;
+ if(val > 1800) phase = Descending; //計55分
break;
case Descending:
val = timer.read();
myled2 = 0;
myled3 = 1;
- getGPS();
-
+ if(flag3 < 50){
+ getGPS();
+ }
+ else{
+ getAltitude();
+ }
if ( val >1500){
timer.reset();
val = 0;
@@ -655,13 +604,17 @@
}
if (timer_set == 1){
val_total = 1500 + val;
- }
- if ( flag3 >= 300){
- if( val_total > 1800){
+ }
+ //最終チェックポイント//////////////////////////////////////////
+ if ( flag3 >= 200){
+ if( val > 900){
phase = Landing_Fusing;
}
}
- if( val_total > 2400) phase = Landing_Fusing;//電源onから55分
+ if( val_total >2400 ){
+ wait(600);
+ phase = Landing_Fusing;//電源onから75分
+ }
break;
case Landing_Fusing:
timer.reset();
@@ -669,16 +622,29 @@
val = 0;
myled3 = 0;
myled4 = 1;
- wait(30);
+ //wait(30);
burning();
wait(5);
Mu2.printf("@DT04FIRE\r\n");
for ( int gp; gp <= 10; gp++){
getGPS();
}
- wait(5);
+
+ wait(2);
jumpmotor = 1;
- wait(15);
+ wait(10);
+ jumpmotor = 0;
+ wait(2);
+ pin21.write(1.0);
+ controlmotor1.write(0);
+ controlmotor2.write(1);
+ wait(4);
+ pin21.write(0.0);
+ controlmotor1.write(0);
+ controlmotor2.write(0);
+ wait(3);
+ jumpmotor = 1;
+ wait(10);
jumpmotor = 0;
phase = Moving;
break;
@@ -692,7 +658,6 @@
case get_goaldirection:
myled1 = 1;
getGPS();
- //test_get_direction();
step = attitude_determination;
wait(1);
break;
@@ -707,6 +672,9 @@
getheading3axis();
wait(0.5);
}
+ timer.reset();
+ timer.stop();
+ val = 0;
getheading3axis();
step = calculate_direction;
wait(1);
@@ -739,6 +707,7 @@
else {
step = direction_control;
}
+ if(pitch == 1) step = direction_control;
wait(1);
break;
@@ -751,10 +720,15 @@
timer.stop();
timer.start();
val = 0;
- while(val < 2){
- val = timer.read();
- motor_drive();
- }
+ pin21.write(1.0);
+ if(pitch == 1){
+ controlmotor1.write(0);
+ controlmotor2.write(1);
+ }else{
+ controlmotor1.write(1);
+ controlmotor2.write(0);
+ }
+ wait(3);
controlmotor1.write(0);
controlmotor2.write(0);
pin21.write(0.0);
@@ -762,7 +736,7 @@
timer.stop();
val = 0;
//step = jump;
- if(jump_count == 3){
+ if(jump_count == 2){
step = jump;
}
else{
@@ -776,7 +750,7 @@
myled1 = 1;
myled2 = 1;
wait(2);
- FILE *fp = fopen("/sd/mydir/height1.txt", "a");
+ FILE *fp = fopen("/sd/mydir/height.txt", "a");
wait(1);
timer.reset();
timer.start();
@@ -785,23 +759,28 @@
Mu2.printf("@DT04JUMP\r\n");
while(val < 4000){
val = timer.read_ms();
- adxl[0] = 0x32;
- i2c.write(addr,adxl,1);
- i2c.read(0xA7,adxl,6);
- axout = (((int16_t )adxl[1]) << 8) | adxl[0];
- ayout = (((int16_t )adxl[3]) << 8) | adxl[2];
- azout = (((int16_t )adxl[5]) << 8) | adxl[4];
- fprintf(fp, "%d,%d,%d,%d\r\n",val,axout,ayout,azout);
- wait_ms(2);
+ if(fp == NULL) {
+ jumpmotor = 1;
+ }
+ else{
+ adxl[0] = 0x32;
+ i2c.write(addr,adxl,1);
+ i2c.read(0xA7,adxl,6);
+ axout = (((int16_t )adxl[1]) << 8) | adxl[0];
+ ayout = (((int16_t )adxl[3]) << 8) | adxl[2];
+ azout = (((int16_t )adxl[5]) << 8) | adxl[4];
+ fprintf(fp, "%d,%d,%d,%d\r\n",val,axout,ayout,azout);
+ wait_ms(2);
+ }
}
jumpmotor = 0;
wait(1);
- fclose(fp);
+ if(fp != NULL)fclose(fp);
myled3 = 1;
+ val = 0;
jump_count = 0;
wait(3);
step = get_goaldirection;
- //step = direction_control;
break;
}
}