航空研究会
/
sahaquiel_brushless_tset
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed MPU6050_2 HMC5883L_4 SDFileSystem3
Diff: main.cpp
- Revision:
- 35:0f89eff001a7
- Parent:
- 34:0c2b67b31b12
- Child:
- 36:e13fca1666a4
--- a/main.cpp Fri Mar 01 11:03:39 2019 +0000
+++ b/main.cpp Fri Mar 01 19:24:30 2019 +0000
@@ -74,7 +74,7 @@
bool setupFlag = false;
bool CameraDegFlag = false;
-bool jevoisFlag = true;
+bool jevoisFlag = false;
bool FocusFlag = false;
int g_CameraDegCounter = 0; //カメラの回転数をカウント
@@ -99,8 +99,8 @@
PwmOut servoTurnTable(PB_0); //TIM3_CH3 カメラ台回転Servo
PwmOut servoCameradeg(PB_1); //TIM3_CH4 カメラ角度調節Servo
PwmOut servoCameraPinto(PB_6); //TIM4_CH1 カメラピント合わせ
-PwmOut servoCameramount(PA_6); //skipperウラ カメラマウント起動
-PwmOut servoGetUP(PC_8); //skipperウラ 起き上がり動作
+PwmOut servoCameramount(PC_8); //skipperウラ カメラマウント起動
+//PwmOut servoGetUP(PC_8); //skipperウラ 起き上がり動作
/*通信用のpinは
PA_3(UART2_Rx) skipperウラ
@@ -145,16 +145,16 @@
pc2.attach(getSF_Serial_pi, Serial::RxIrq);
t2.start();
- /*NVIC_DisableIRQ(USART2_IRQn);
+ NVIC_DisableIRQ(USART2_IRQn);
while(g_landingcommand != 'B'){
wait_ms(30);
}
-
+ NVIC_EnableIRQ(USART2_IRQn);
jevoisFlag = true;
- NVIC_EnableIRQ(USART2_IRQn);
+ g_landingcommand='N';
wait(5);
pc.printf("start\r\n");
- */
+
servoCameraPinto.pulsewidth_us(Focus_NEUTRAL-200);
wait(1);
@@ -165,13 +165,15 @@
else NVIC_DisableIRQ(USART2_IRQn);
pc.printf("Move Camera Board\r\n");
MoveCameraBoard();
+ if(jevoisFlag==true) NVIC_DisableIRQ(USART6_IRQn);
+ else NVIC_DisableIRQ(USART2_IRQn);
pc.printf("Position\r\n");
pc.printf("g_landingcommand=%c\r\n",g_landingcommand);
NVIC_DisableIRQ(USART6_IRQn);
NVIC_DisableIRQ(USART2_IRQn);
- if(g_landingcommand!='N') MatchPosition();
+ if(g_landingcommand!='N'&&g_landingcommand!='P') MatchPosition();
NVIC_EnableIRQ(USART6_IRQn);
NVIC_EnableIRQ(USART2_IRQn);
@@ -204,6 +206,8 @@
g_fp = fopen( "/sd/Datalog01.txt" ,"a" );
fprintf(g_fp, "Time = %d min %d sec : Goal is not found.\r\n",(int)t2.read()/60,(int)t2.read()%60);
fclose(g_fp);
+ NVIC_EnableIRQ(USART6_IRQn);
+ NVIC_EnableIRQ(USART2_IRQn);
break;
case 'Y': //MOVE_FORWARD
@@ -214,26 +218,28 @@
g_fp = fopen( "/sd/Datalog01.txt" ,"a" );
fprintf(g_fp, "Time = %d min %d sec : Cansat move forward .\r\n",(int)t2.read()/60,(int)t2.read()%60);
fclose(g_fp);
+ NVIC_EnableIRQ(USART6_IRQn);
+ NVIC_EnableIRQ(USART2_IRQn);
break;
case 'l': //MOVE_LEFT Low Speed
servoR.pulsewidth_us(Servo_slow_FORWARD_R);
- servoL.pulsewidth_us(Servo_NEUTRAL_L);
- NVIC_EnableIRQ(USART6_IRQn);
- NVIC_EnableIRQ(USART2_IRQn);
+ servoL.pulsewidth_us(Servo_NEUTRAL_L);
g_fp = fopen( "/sd/Datalog01.txt","a" );
fprintf(g_fp, "Time = %d min %d sec : Cansat move left low speed.\r\n",(int)t2.read()/60,(int)t2.read()%60);
fclose(g_fp);
+ NVIC_EnableIRQ(USART6_IRQn);
+ NVIC_EnableIRQ(USART2_IRQn);
break;
case 'L': //MOVE_LEFT High Speed
servoR.pulsewidth_us(Servo_high_FORWARD_R);
- servoL.pulsewidth_us(Servo_NEUTRAL_L);
- NVIC_EnableIRQ(USART6_IRQn);
- NVIC_EnableIRQ(USART2_IRQn);
+ servoL.pulsewidth_us(Servo_NEUTRAL_L);
g_fp = fopen( "/sd/Datalog01.txt","a" );
fprintf(g_fp, "Time = %d min %d sec : Cansat move left high speed.\r\n",(int)t2.read()/60,(int)t2.read()%60);
fclose(g_fp);
+ NVIC_EnableIRQ(USART6_IRQn);
+ NVIC_EnableIRQ(USART2_IRQn);
break;
case 'r': //MOVE_RIGHT Low Speed
@@ -244,6 +250,8 @@
g_fp = fopen( "/sd/Datalog01.txt","a" );
fprintf(g_fp, "Time = %d min %d sec : Cansat move right low speed.\r\n",(int)t2.read()/60,(int)t2.read()%60);
fclose(g_fp);
+ NVIC_EnableIRQ(USART6_IRQn);
+ NVIC_EnableIRQ(USART2_IRQn);
break;
case 'R': //MOVE_RIGHT High Speed
@@ -254,16 +262,18 @@
g_fp = fopen( "/sd/Datalog01.txt","a" );
fprintf(g_fp, "Time = %d min %d sec : Cansat move left high speed.\r\n",(int)t2.read()/60,(int)t2.read()%60);
fclose(g_fp);
+ NVIC_EnableIRQ(USART6_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(USART6_IRQn);
- NVIC_EnableIRQ(USART2_IRQn);
g_fp = fopen( "/sd/Datalog01.txt","a" );
fprintf(g_fp, "Time = %d min %d sec : Cansat move goal forward mode.\r\n",(int)t2.read()/60,(int)t2.read()%60);
fclose(g_fp);
+ NVIC_EnableIRQ(USART6_IRQn);
+ NVIC_EnableIRQ(USART2_IRQn);
break;
case '1': //MOVE_FORWARD Speed Level 1
@@ -273,12 +283,12 @@
do{
SensingMPU();
wait_ms(30);
- }while((nowAngle[PITCH]>20||nowAngle[PITCH]<-20)&&(nowAngle[ROLL]>20||nowAngle[ROLL]<-20));
- NVIC_EnableIRQ(USART6_IRQn);
- NVIC_EnableIRQ(USART2_IRQn);
+ }while((nowAngle[PITCH]>20||nowAngle[PITCH]<-20)&&(nowAngle[ROLL]>20||nowAngle[ROLL]<-20));
g_fp = fopen( "/sd/Datalog01.txt","a" );
fprintf(g_fp, "Time = %d min %d sec : Cansat move forward 1sec!.\r\n",(int)t2.read()/60,(int)t2.read()%60);
fclose(g_fp);
+ NVIC_EnableIRQ(USART6_IRQn);
+ NVIC_EnableIRQ(USART2_IRQn);
break;
case '2': //MOVE_FORWARD Speed Level 2
@@ -289,11 +299,11 @@
SensingMPU();
wait_ms(30);
}while((nowAngle[PITCH]>20||nowAngle[PITCH]<-20)&&(nowAngle[ROLL]>20||nowAngle[ROLL]<-20));
- NVIC_EnableIRQ(USART6_IRQn);
- NVIC_EnableIRQ(USART2_IRQn);
g_fp = fopen( "/sd/Datalog01.txt","a" );
fprintf(g_fp, "Time = %d min %d sec : Cansat move forward 2sec!.\r\n",(int)t2.read()/60,(int)t2.read()%60);
fclose(g_fp);
+ NVIC_EnableIRQ(USART6_IRQn);
+ NVIC_EnableIRQ(USART2_IRQn);
break;
case '3': //MOVE_FORWARD Speed Level 3
@@ -303,12 +313,12 @@
do{
SensingMPU();
wait_ms(30);
- }while((nowAngle[PITCH]>20||nowAngle[PITCH]<-20)&&(nowAngle[ROLL]>20||nowAngle[ROLL]<-20));
- NVIC_EnableIRQ(USART6_IRQn);
- NVIC_EnableIRQ(USART2_IRQn);
+ }while((nowAngle[PITCH]>20||nowAngle[PITCH]<-20)&&(nowAngle[ROLL]>20||nowAngle[ROLL]<-20));
g_fp = fopen( "/sd/Datalog01.txt","a" );
fprintf(g_fp, "Time = %d min %d sec : Cansat move forward 3sec!.\r\n",(int)t2.read()/60,(int)t2.read()%60);
fclose(g_fp);
+ NVIC_EnableIRQ(USART6_IRQn);
+ NVIC_EnableIRQ(USART2_IRQn);
break;
case '4': //MOVE_FORWARD Speed Level 4
@@ -318,12 +328,12 @@
do{
SensingMPU();
wait_ms(30);
- }while((nowAngle[PITCH]>20||nowAngle[PITCH]<-20)||(nowAngle[ROLL]>20||nowAngle[ROLL]<-20));
- NVIC_EnableIRQ(USART6_IRQn);
- NVIC_EnableIRQ(USART2_IRQn);
+ }while((nowAngle[PITCH]>20||nowAngle[PITCH]<-20)||(nowAngle[ROLL]>20||nowAngle[ROLL]<-20));
g_fp = fopen( "/sd/Datalog01.txt","a" );
fprintf(g_fp, "Time = %d min %d sec : Cansat move forward 4sec!.\r\n",(int)t2.read()/60,(int)t2.read()%60);
fclose(g_fp);
+ NVIC_EnableIRQ(USART6_IRQn);
+ NVIC_EnableIRQ(USART2_IRQn);
break;
case '5': //MOVE_FORWARD Speed Level 5
@@ -334,11 +344,11 @@
SensingMPU();
wait_ms(30);
}while((nowAngle[PITCH]>20||nowAngle[PITCH]<-20)&&(nowAngle[ROLL]>20||nowAngle[ROLL]<-20));
- NVIC_EnableIRQ(USART6_IRQn);
- NVIC_EnableIRQ(USART2_IRQn);
g_fp = fopen( "/sd/Datalog01.txt","a" );
fprintf(g_fp, "Time = %d min %d sec : Cansat move forward 5sec!.\r\n",(int)t2.read()/60,(int)t2.read()%60);
fclose(g_fp);
+ NVIC_EnableIRQ(USART6_IRQn);
+ NVIC_EnableIRQ(USART2_IRQn);
break;
case 'M': //MatchPosition
@@ -373,8 +383,12 @@
}
void MoveCameraBoard(){
+ if(jevoisFlag==true) NVIC_DisableIRQ(USART6_IRQn);
+ else NVIC_DisableIRQ(USART2_IRQn);
//pc.printf("start\r\n");
MoveCansat('N');
+ if(jevoisFlag==true) NVIC_DisableIRQ(USART6_IRQn);
+ else NVIC_DisableIRQ(USART2_IRQn);
//pc.printf("ok\r\n");
g_landingcommand='N';
servoCameradeg.pulsewidth_us(Camera_deg_B);
@@ -394,6 +408,10 @@
//pc.printf("zoom2\r\n");
if(jevoisFlag == true) FocusAdjust();
else wait(1);
+
+ if(jevoisFlag==true) NVIC_DisableIRQ(USART6_IRQn);
+ else NVIC_DisableIRQ(USART2_IRQn);
+
if(g_landingcommand!='N') return;
}
@@ -407,6 +425,9 @@
//pc.printf("zoom1\r\n");
if(jevoisFlag == true) FocusAdjust();
else wait(1);
+
+ if(jevoisFlag==true) NVIC_DisableIRQ(USART6_IRQn);
+ else NVIC_DisableIRQ(USART2_IRQn);
if(g_landingcommand!='N') return;
@@ -420,6 +441,10 @@
//pc.printf("zoom2\r\n");
if(jevoisFlag == true) FocusAdjust();
else wait(1);
+
+ if(jevoisFlag==true) NVIC_DisableIRQ(USART6_IRQn);
+ else NVIC_DisableIRQ(USART2_IRQn);
+
if(g_landingcommand!='N') return;
}
@@ -483,14 +508,14 @@
while(nowAngle[YAW] > HighTargetYaw && nowAngle[YAW] < LowTargetYaw){
//MoveCansat('r');
SensingMPU();
- wait_ms(30);
+ wait_ms(10);
pc.printf("nowAngle_MPU=%f\r\n",nowAngle[YAW]);
}
}else{
while(nowAngle[YAW] > HighTargetYaw || nowAngle[YAW] < LowTargetYaw){
//MoveCansat('r');
SensingMPU();
- wait_ms(30);
+ wait_ms(10);
pc.printf("nowAngle_MPU=%f\r\n",nowAngle[YAW]);
}
}
@@ -502,6 +527,9 @@
}
void FocusAdjust(){
+ if(jevoisFlag==true) NVIC_DisableIRQ(USART6_IRQn);
+ else NVIC_DisableIRQ(USART2_IRQn);
+
if(FocusFlag == false){
servoCameraPinto.pulsewidth_us(Focus_NEUTRAL + Pint_speed);
servoR.pulsewidth_us(Servo_NEUTRAL_R); //servo initialize