CORE
2017年3月，伊豆大島共同打上実験 CORE＿缶ロケチーム電装
Dependencies: MPU6050 MS5607 mbed SDFileSystem
Revision 16:0a239047eb29, committed 2017-03-13
- Comitter:
- mikawataru
- Date:
- Mon Mar 13 12:41:09 2017 +0000
- Parent:
- 15:01d3969b89d0
- Child:
- 17:5f7808444d96
- Commit message:
- fixed servo motion
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Fri Mar 10 04:28:18 2017 +0000
+++ b/main.cpp Mon Mar 13 12:41:09 2017 +0000
@@ -46,14 +46,14 @@
#define ACC_LAUNCH 0//FIXME:本番は4g
#define TOP_DROP_AMOUNT 1.5
-#define ALT_UAV_DROP 150
+#define ALT_UAV_DROP 1.0//FIXME:本番は150m
#define TIME_REACH_TOP 15
#define RATE_LOG 10
#define RATE_OPEN 10
/*サーボ動作*/
-#define LOCK 0.0005
-#define UNLOCK 0.0015
+#define LOCK 0
+#define UNLOCK 1
/*モード定義*/
#define STANDBY 0
#define TEST 1
@@ -102,7 +102,7 @@
int8_t input_cnt = 0;
/*関数*/
void _Open();
-void _Servo(int8_t door_num, float motion);
+void _Servo(int8_t door_num, int8_t motion);
void _Log();
void _Input();
float _Measure_Alt(float press, float temp);
@@ -117,7 +117,8 @@
Door_1_2.period_ms(20);
Door_2_1.period_ms(20);
Door_2_2.period_ms(20);
- _Servo(0,LOCK);
+ // _Servo(1,UNLOCK);//todo:当日は状態記憶に仕様変更予定？
+ // _Servo(2,UNLOCK);//
fp = fopen("/sd/log.txt", "w");
fprintf(fp, "pressure,temperature,ax,ay,az,gx,gy,gz\r\n");
device.attach(&_Input,Serial::RxIrq);
@@ -127,14 +128,14 @@
/*開放用関数 RATE_OPEN[Hz]で判定を行う*/
void _Open(){
myled = 1 << (Phase-1);
-// device.printf("%f,%f,%f\r\n",alt_buff[col_open],alt_launch,alt_max);
+ device.printf("%f,%f,%f\r\n",alt_buff[col_open],alt_launch,alt_max);
switch (Phase) {
case SETUP://セットアップモード（発射判定不可）
break;
case LAUNCH://点火モード（発射判定可）
float acc_buff = (float)mpu.getAcceleroRawZ()/(ACC*0.981);
alt_buff[col_open] = _Measure_Alt(ms5607.getPressure()/100,ms5607.getTemperature());
- if(acc_buff>ACC_LAUNCH){
+ if(acc_buff>(float)ACC_LAUNCH){
if(cnt_judge++==3){
Phase = RISE;
alt_launch = _Median(alt_buff, RATE_OPEN);
@@ -149,9 +150,9 @@
case RISE://上昇中（パラシュート開放判定）
alt_buff[col_open] = _Measure_Alt(ms5607.getPressure()/100,ms5607.getTemperature());
if(alt_buff[col_open]>alt_max) alt_max = alt_buff[col_open];
- if(alt_buff[col_open]<alt_max-TOP_DROP_AMOUNT){
+ if(alt_buff[col_open]<alt_max-(float)TOP_DROP_AMOUNT){
if(cnt_judge++==3){
- _Servo(1,UNLOCK);
+ _Servo(2,UNLOCK);
Phase = DROP;
cnt_judge = 0;
}
@@ -159,16 +160,19 @@
}else{
if(timer.read()>t_top+1.0) cnt_judge = 0;
}
+ /*FIXME:当日は有効化
if(timer.read()-t_launch>TIME_REACH_TOP){
- _Servo(1,UNLOCK);
+ _Servo(2,UNLOCK);
Phase = DROP;
cnt_judge = 0;
}
+ */
break;
case DROP://降下中（缶サット開放判定)
- if(alt_buff[col_open] < alt_launch+ALT_UAV_DROP){
+ if(alt_buff[col_open] < alt_launch+(float)ALT_UAV_DROP){
if(cnt_judge++==3){
- _Servo(2,UNLOCK);
+ Phase++;
+ _Servo(1,UNLOCK);
cnt_judge = 0;
}
t_drop = timer.read();
@@ -186,7 +190,7 @@
temperature[row][col] = ms5607.getTemperature();
mpu.getAccelero(&acc[row][col][0]);
mpu.getGyro(&gyro[row][col][0]);
- fprintf(fp, "%f,%f,%f,%f,%f,%f,%f,%f,%f\r\n",
+ fprintf(fp,"%f,%f,%f,%f,%f,%f,%f,%f,%f\r\n",
t[row][col],pressure[row][col],temperature[row][col],acc[row][col][0]/10.0,
acc[row][col][1]/10.0,acc[row][col][2]/10.0,gyro[row][col][0],gyro[row][col][1],gyro[row][col][2]
);
@@ -197,22 +201,32 @@
col = 0;
}
}
-/*サーボ動作用関数 _Servo(int8_t 扉番号,float pwm波長)*/
-// TODO:サーボ回転量の調整
-void _Servo(int8_t door_num, float motion){
- if(door_num==0){//全扉
- Door_1_1.pulsewidth(motion);
- Door_1_2.pulsewidth(motion);
- Door_2_1.pulsewidth(motion);
- Door_2_2.pulsewidth(motion);
- }else if(door_num==1){//扉１
- Door_1_1.pulsewidth(motion);
- wait(0.1);
- Door_1_2.pulsewidth(motion);
+/*サーボ動作用関数 _Servo(int8_t 扉番号,int8_t 動作)*/
+void _Servo(int8_t door_num, int8_t motion){
+ if(door_num==1){//扉１
+ if(motion==UNLOCK){
+ Door_1_1.pulsewidth(0.0015);
+ wait(0.2);
+ Door_1_2.pulsewidth(0.0015);
+ }else if(motion==LOCK){
+ Door_1_1.pulsewidth(0.0024);
+ wait(0.2);
+ Door_1_2.pulsewidth(0.0005);
+ }else{
+ device.printf("error%f\r\n",motion);
+ }
}else if(door_num==2){//扉２
- Door_2_1.pulsewidth(motion);
- wait(0.1);
- Door_2_2.pulsewidth(motion);
+ if(motion==UNLOCK){
+ Door_2_1.pulsewidth(0.0015);
+ wait(0.2);
+ Door_2_2.pulsewidth(0.0015);
+ }else if(motion==LOCK){
+ Door_2_1.pulsewidth(0.0024);
+ wait(0.2);
+ Door_2_2.pulsewidth(0.0005);
+ }else{
+ device.printf("error%f\r\n",motion);
+ }
}else{
device.printf("servo error:%d\r\n",door_num);
}
@@ -221,7 +235,6 @@
/*入力用*/
// TODO:テストモード実装
// FIXME:android端末だと入力崩れが起こる
-// FIXME:サーボ入力がたぶん逆
void _Input(){
input_buff[input_cnt] = device.getc();
device.printf("\r\n");
@@ -287,11 +300,11 @@
}
}else if(input_cnt==2){
if(input_buff[2]=='U'){
- if(input_buff[1]=='P') _Servo(1,UNLOCK);
- if(input_buff[1]=='C') _Servo(2,UNLOCK);
+ if(input_buff[1]=='P') _Servo(2,UNLOCK);
+ if(input_buff[1]=='C') _Servo(1,UNLOCK);
}else if(input_buff[2]=='L'){
- if(input_buff[1]=='P') _Servo(1,LOCK);
- if(input_buff[1]=='C') _Servo(2,LOCK);
+ if(input_buff[1]=='P') _Servo(2,LOCK);
+ if(input_buff[1]=='C') _Servo(1,LOCK);
}else{
device.printf("This command is not found >> %c\r\n",input_buff[2]);
device.printf("U >> UNLOCK\r\n");