CORE
201903_14ISEで実際に使用した開放用プログラム． 使用マイコンがNUCLES-F303K8なので注意
Dependencies: mbed Madgwick MPU6050 Kalman BMP180
Diff: main.cpp
- Revision:
- 2:a4895f7c3058
- Parent:
- 1:b9ea35d93329
--- a/main.cpp Mon Oct 10 11:21:32 2016 +0000
+++ b/main.cpp Thu Nov 02 17:13:22 2017 +0000
@@ -14,34 +14,262 @@
*/
#include "mbed.h"
+#include "MPU6050.h"
#include "math.h"
#include "BMP180.h"
-#define p0 1013.25f//海面気圧
+
+#define LUNCH_G 1.0
+#define TIMER1 500
+#define TIMER2 30
+#define ALT_LEAF 300
+#define UNLOCK 0
+#define LOCK 1
+#define RATE_OPEN 1
+
+enum PHASE{LUNCH=1,RISE=3,DROP=7} Phase;
+
+DigitalIn pr(dp10);
+DigitalInOut le(dp11);
+MPU6050 mpu(dp5, dp27);
+BMP180 bmp(dp5, dp27);
+PwmOut servo_para(dp1);
+PwmOut servo_leaf(dp18);
+DigitalOut myled(dp14);
+DigitalOut myled2(dp28);
+
+Timer timer1;
+Timer timer2;
+Ticker tic_open;
-DigitalOut myled(LED1);
-Serial pc(USBTX,USBRX);
-BMP180 bmp(PB_7, PB_6);
-Timer timer;
+//高度取得
+float Alt_buff[10];
+float Alt_gnd,Alt_drop;
+float pressure,temperature;
+float p0 = 1013.25;
+//カウント変数
+int Cnt_buff;
+int i;
+int Cnt_para;
-float getAlt(float press, float temp);
+//加速度取得
+float a_abs;
+float a[3];
+float Acc_buff[10];
+float Acc_lnc;
+//その他
+int t1, t2;
+bool tf_para = true;
+bool tf_leaf = true;
+
+void _para(int motion);
+void _leaf(int motion);
+void kaihou(void);
+/*雑関数*/
+float median(float data[], int num);
+float get_Alt(float press, float temp);
+int _input(char c);
int main() {
- float pressure,temperature,altitude;
- float time;
- bmp.Initialize(64,BMP180_OSS_ULTRA_LOW_POWER);
- pc.printf("time, temperature ,pressure, altitude\r\n");
- timer.start();
+
+ le.input();
+
+ wait(1); //良い子は待つ
+ if(pr == 1){ //途中で電源落ちたら
+ Phase = RISE;
+ Cnt_buff = 0;
+ timer1.start();
+ goto yabai;
+ }
+
+ myled2 = 1;
+
+
+ while(1){
+
+
+ while(pr== 0 && le==0);
+ wait(0.2); //LPC1768の信号待ち
+ if(pr==1 && le==0){
+ if(tf_para == true){
+ _para(UNLOCK);
+ myled = 1;
+// myled2 = 0;
+ tf_para = false;
+ }
+ else{
+ _para(LOCK);
+ myled = 0;
+// myled2 = 0;
+ tf_para = true;
+ }
+ wait(1);
+ }
+ if(pr==0 && le==1){
+ if(tf_leaf == true){
+ _leaf(UNLOCK);
+ myled2 = 0;
+ tf_leaf = false;
+ }
+ else{
+ _leaf(LOCK);
+ myled2 = 0;
+ tf_leaf = true;
+ }
+ wait(1);
+ }
+
+
+ if(pr==1 && le==1) break;
+
+ }
+
+ le.output();
+ le = 0;
+ wait(2.5); //LPC1768の初期化処理待ち
+
+ Phase = LUNCH;
+
+yabai:
+
+ mpu.setAcceleroRange(0);
+ bmp.Initialize(60,BMP180_OSS_ULTRA_LOW_POWER);
+
+
+ Cnt_buff = 0;
+ for(i=0; i<10; i++){
+ bmp.ReadData(&temperature,&pressure);
+ Alt_buff[Cnt_buff]=get_Alt(temperature, pressure);
+ Cnt_buff++;
+ }
+ Alt_gnd = median(Alt_buff, 10);
+
+ Cnt_buff = 0;
+
+
+ tic_open.attach(&kaihou,1.0/RATE_OPEN);
+
+ while(1);
- while(1) {
- bmp.ReadData(&temperature,&pressure);
- altitude = getAlt(pressure,temperature);
- time = timer.read();
- pc.printf("%f, %f, %f, %f \r\n",time, temperature, pressure, altitude);
- myled =! myled;
- wait(1);
+
+}
+
+
+void kaihou(void){
+
+ switch(Phase){
+
+ case LUNCH:
+
+ mpu.getAccelero(a);
+ Acc_buff[Cnt_buff] = sqrt(pow(a[0]/9.81,2)+pow(a[1]/9.81,2)+pow(a[2]/9.81,2));
+ Cnt_buff++;
+
+ //myled2=!myled2;
+
+ if(Cnt_buff == 10){
+ myled2 = 0;
+ Acc_lnc = median(Acc_buff, 10);
+ Cnt_buff = 0;
+ //myled2 = 0;
+ }
+ if(Acc_lnc>LUNCH_G){
+ Phase = RISE;
+ //myled2 = 0;
+ timer1.start();
+ le = 1; //LPC1768に発射をお知らせする
+ }
+
+ break;
+
+
+ case RISE:
+ if(Cnt_buff == 0){
+ bmp.ReadData(&temperature,&pressure);
+ Alt_buff[Cnt_buff]=get_Alt(temperature, pressure); //まず高度取る
+ }
+
+ bmp.ReadData(&temperature,&pressure);
+ Alt_buff[Cnt_buff+1] = get_Alt(temperature, pressure);
+ if(Alt_buff[Cnt_buff]>Alt_buff[Cnt_buff+1]) Cnt_para++;
+ Cnt_buff++;
+
+ if(Cnt_buff == 9){
+ //myled2 = 1;
+ t1 = timer1.read();
+
+ if(Cnt_para>=5 || t1>TIMER1){
+ _para(UNLOCK);
+ myled2 =0;
+ le = 0; //LPC1768にパラ放出をお知らせする
+ timer1.stop();
+ timer2.start();
+ Phase = DROP;
+ }
+ Cnt_buff = 0;
+
+ }
+ break;
+
+ case DROP:
+
+ bmp.ReadData(&temperature,&pressure);
+ Alt_buff[Cnt_buff]=get_Alt(temperature, pressure);
+ Cnt_buff ++;
+ if(Cnt_buff == 9){
+ Alt_drop = median(Alt_buff, 10)-Alt_gnd;
+ t2 = timer2.read();
+ //twe.printf("%d\r\n",t2);
+ myled2 = !myled2;
+
+ if(Alt_drop<ALT_LEAF && t2>TIMER2){
+ _leaf(UNLOCK);
+ le = 1 ; //LPC1768にリーフィングをお知らせする
+ myled2 = 1;
+
+ }
+ Cnt_buff = 0;
+ }
+
+ break ;
}
}
-float getAlt(float press, float temp){
+void _para(int motion){
+ if(motion==UNLOCK){
+ servo_para.pulsewidth(0.0005); // pulse servo out sita
+ }else if(motion==LOCK){
+ servo_para.pulsewidth(0.0025); // pulse servo outu sita
+ }
+}
+void _leaf(int motion){
+ if(motion==UNLOCK){
+ servo_leaf.pulsewidth(0.0006); // pulse servo out
+ }else if(motion==LOCK){
+ servo_leaf.pulsewidth(0.0024); // pulse servo out
+ }
+}
+
+float median(float data[], int num){//todo:処理時間計測
+ float *data_cpy, ans;
+ data_cpy = new float[num];
+ memcpy(data_cpy,data,sizeof(float)*num);
+
+ for(int i=0; i<num; i++){
+ for(int j=0; j<num-i-1; j++){
+ if(data_cpy[j]>data_cpy[j+1]){
+ float buff = data_cpy[j+1];
+ data_cpy[j+1] = data_cpy[j];
+ data_cpy[j] = buff;
+ }
+ }
+ }
+
+ if(num%2!=0) ans = data_cpy[num/2];
+ else ans = (data_cpy[num/2-1]+data_cpy[num/2])/2.0;
+ delete[] data_cpy;
+ return ans;
+}
+
+float get_Alt(float temp, float press){
return (pow((p0/press), (1.0f/5.257f))-1.0f)*(temp+273.15f)/0.0065f;
}
\ No newline at end of file