航空研究会
SDの設定が反映されるように修正
Dependencies: mbed MPU6050_2 HMC5883L_2 SDFileSystem3
Diff: main.cpp
- Revision:
- 7:8989a4b84695
- Parent:
- 6:166746820555
- Child:
- 8:d11a59d2a2f1
diff -r 166746820555 -r 8989a4b84695 main.cpp
--- a/main.cpp Fri Feb 08 10:39:17 2019 +0000
+++ b/main.cpp Sat Feb 09 06:23:48 2019 +0000
@@ -15,6 +15,9 @@
#define servo_slow_FORWARD_L 1560
#define servo_slow_back_R 1360
#define servo_slow_back_L 1360
+#define TurnTable_NEUTRAL 1500 //カメラ台座のサーボ
+#define MatchSpeed 1500 + 100 //カメラと方向を合わせるときの車輪の速度
+#define minFocus 1200 //焦点合わせ用サーボの最小値
#define MOVE_NEUTRAL 0
#define MOVE_FORWARD 1
@@ -31,19 +34,25 @@
void getSF_Serial_jevois();
void getSF_Serial_pi();
-
//MPU_check用
void SensingMPU();
+
+void MoveCansat(char g_landingcommand);
void setup();
void Init_sensors();
void DisplayClock();
void DebugPrint();
void SensingHMC();
+void MoveCameraBoard();
+void MatchPosition();
+void FocusAdjust();
static float nowAngle[3] = {0,0,0},nowAngle_HMC=0;
float FirstROLL = 0.0, FirstPITCH = 0.0 ,FirstYAW = 0.0,g_FirstYAW_HMC;
-bool setupFlag=false;
+bool setupFlag = false;
+bool CameraDegFlag = false;
+bool jevoisFlag = true;
enum Angle{ROLL, PITCH, YAW}; //yaw:北を0とした絶対角度
@@ -77,8 +86,6 @@
char g_landingcommand='N';
-void MoveCansat(char g_landingcommand);
-
//MPU_check用
MPU6050DMP6 mpu6050(PC_0,&pc);
@@ -92,18 +99,14 @@
pc.attach(getSF_Serial_jevois, Serial::RxIrq);
pc2.attach(getSF_Serial_pi, Serial::RxIrq);
- NVIC_SetPriority(USART1_IRQn,0);//割り込み優先度
- NVIC_SetPriority(USART2_IRQn,1);
-
while(1) {
- //pc.printf("Hello World!!");
- MoveCansat(g_landingcommand);
-
- SensingMPU();
- SensingHMC();
- DebugPrint();
-
+ if(jevoisFlag==true) NVIC_DisableIRQ(USART1_IRQn);
+ else NVIC_DisableIRQ(USART2_IRQn);
+ if(g_landingcommand!='N') MatchPosition();
+ MoveCansat(g_landingcommand);
wait_ms(23);
+ if(jevoisFlag==true) NVIC_EnableIRQ(USART1_IRQn);
+ else NVIC_EnableIRQ(USART2_IRQn);
}
}
@@ -111,58 +114,123 @@
void MoveCansat(char g_landingcommand)
{
//NVIC_DisableIRQ(USART1_IRQn);
- NVIC_DisableIRQ(USART2_IRQn);
+ //NVIC_DisableIRQ(USART2_IRQn);
switch(g_landingcommand){
case 'N': //MOVE_NEUTRAL
servoR.pulsewidth_us(servo_NEUTRAL_R);
servoL.pulsewidth_us(servo_NEUTRAL_L);
//NVIC_EnableIRQ(USART1_IRQn);
- NVIC_EnableIRQ(USART2_IRQn);
+ //NVIC_EnableIRQ(USART2_IRQn);
break;
case 'Y': //MOVE_FORWARD
servoR.pulsewidth_us(servo_FORWARD_R);
servoL.pulsewidth_us(servo_FORWARD_L);
//NVIC_EnableIRQ(USART1_IRQn);
- NVIC_EnableIRQ(USART2_IRQn);
+ //NVIC_EnableIRQ(USART2_IRQn);
break;
case 'L': //MOVE_LEFT
servoR.pulsewidth_us(servo_slow_FORWARD_R);
servoL.pulsewidth_us(servo_slow_back_L);
//NVIC_EnableIRQ(USART1_IRQn);
- NVIC_EnableIRQ(USART2_IRQn);
+ //NVIC_EnableIRQ(USART2_IRQn);
case 'R': //MOVE_RIGHT
servoR.pulsewidth_us(servo_slow_back_R);
servoL.pulsewidth_us(servo_slow_FORWARD_L);
//NVIC_EnableIRQ(USART1_IRQn);
- NVIC_EnableIRQ(USART2_IRQn);
+ //NVIC_EnableIRQ(USART2_IRQn);
break;
case 'B': //MOVE_BACK
servoR.pulsewidth_us(servo_back_R);
servoL.pulsewidth_us(servo_back_L);
//NVIC_EnableIRQ(USART1_IRQn);
- NVIC_EnableIRQ(USART2_IRQn);
+ //NVIC_EnableIRQ(USART2_IRQn);
break;
case 'G': //GOAL_FORWARD
servoR.pulsewidth_us(servo_slow_FORWARD_R);
servoL.pulsewidth_us(servo_slow_FORWARD_L);
//NVIC_EnableIRQ(USART1_IRQn);
- NVIC_EnableIRQ(USART2_IRQn);
+ //NVIC_EnableIRQ(USART2_IRQn);
+ break;
+
+ case 'J': //MOVE_FORWARD Tim
+ servoR.pulsewidth_us(servo_FORWARD_R);
+ servoL.pulsewidth_us(servo_FORWARD_L);
+ //NVIC_EnableIRQ(USART1_IRQn);
+ wait(5);
+ //NVIC_EnableIRQ(USART2_IRQn);
+ break;
+
+ case 'M': //MatchPosition
+ servoR.pulsewidth_us(MatchSpeed);
+ //NVIC_EnableIRQ(USART1_IRQn);
+ //NVIC_EnableIRQ(USART2_IRQn);
+ break;
+
+ case 'T': //jevoisからraspberry piへの切り替え
+ jevoisFlag = false;
+ //NVIC_EnableIRQ(USART2_IRQn);
break;
default :
//NVIC_EnableIRQ(USART1_IRQn);
- NVIC_EnableIRQ(USART2_IRQn);
+ //NVIC_EnableIRQ(USART2_IRQn);
break;
}
+
return;
}
+void MoveCameraBoard(){
+ MoveCansat('N');
+ servoTurnTable.pulsewidth_us(2000);
+ wait_ms(300);
+ servoTurnTable.pulsewidth_us(TurnTable_NEUTRAL);
+ if(jevoisFlag == true) FocusAdjust();
+ else wait(1);
+
+ if(g_landingcommand!='N') return;
+ if(!CameraDegFlag){
+ servoCameradeg.pulsewidth_us(1800);
+ CameraDegFlag=!CameraDegFlag;
+ }else{
+ servoCameradeg.pulsewidth_us(1500);
+ CameraDegFlag = !CameraDegFlag;
+ }
+
+ if(jevoisFlag == true) FocusAdjust();
+ else wait(1);
+
+ return;
+}
+
+void MatchPosition(){
+ SensingMPU();
+ SensingHMC();
+ DebugPrint();
+
+ while(nowAngle_HMC <= nowAngle[YAW]+5 && nowAngle_HMC >= nowAngle[YAW]-5){
+ MoveCansat('M');
+ }
+ return;
+}
+
+void FocusAdjust(){
+ servoCameraPinto.pulsewidth_us(minFocus);
+
+ for(int i=0; i<400; i++){
+ servoCameraPinto.pulsewidth_us(minFocus+i);
+ wait_ms(30);
+ if(g_landingcommand!='N') return;
+ }
+ return;
+}
+
void getSF_Serial_jevois(){
@@ -213,7 +281,7 @@
void getSF_Serial_pi(){
- NVIC_DisableIRQ(USART2_IRQn);
+ //NVIC_DisableIRQ(USART2_IRQn);
static char SFbuf[16]={'Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q'};
@@ -256,7 +324,7 @@
}
}
- NVIC_EnableIRQ(USART2_IRQn);
+ //NVIC_EnableIRQ(USART2_IRQn);
}
@@ -266,11 +334,15 @@
Init_sensors();
//switch2.rise(ResetTrim);
- NVIC_SetPriority(USART1_IRQn,0);
- NVIC_SetPriority(EXTI0_IRQn,1);
- NVIC_SetPriority(TIM5_IRQn,2);
- NVIC_SetPriority(EXTI9_5_IRQn,3);
- NVIC_SetPriority(USART2_IRQn,4);
+ //NVIC_SetPriority(USART1_IRQn,0);
+ //NVIC_SetPriority(EXTI0_IRQn,1);
+ //NVIC_SetPriority(TIM5_IRQn,2);
+ //NVIC_SetPriority(EXTI9_5_IRQn,3);
+ //NVIC_SetPriority(USART2_IRQn,4);
+
+ NVIC_SetPriority(USART1_IRQn,0);//割り込み優先度
+ NVIC_SetPriority(USART2_IRQn,1);
+
DisplayClock();
t.start();
@@ -308,7 +380,7 @@
//static int16_t deltaT = 0, t_start = 0;
//t_start = t.read_us();
- float rpy[3] = {0}, oldrpy[3] = {0};
+ float rpy[3] = {0};
static uint16_t count_changeRPY = 0;
static bool flg_checkoutlier = false;
NVIC_DisableIRQ(USART1_IRQn);
@@ -372,7 +444,7 @@
//static int16_t deltaT = 0, t_start = 0;
//t_start = t.read_us();
- float rpy=0, oldrpy=0;
+ float rpy=0;
static uint16_t count_changeRPY = 0;
static bool flg_checkoutlier = false;
NVIC_DisableIRQ(USART1_IRQn);
@@ -416,7 +488,7 @@
void DebugPrint(){
//for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]); //skipper地磁気センサ_デバック用
- pc.printf("%3.2f\t",nowAngle[2]);
- pc.printf("%3.2f\t",nowAngle_HMC);
- pc.printf("\r\n");
+ //pc.printf("%3.2f\t",nowAngle[2]);
+ //pc.printf("%3.2f\t",nowAngle_HMC); //HMC
+ //pc.printf("\r\n");
}
\ No newline at end of file