setup内にあり
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