imano yuta
skipper_raspi_uart_test
Dependencies: mbed MPU6050_2 HMC5883L_4 SDFileSystem3
Diff: main.cpp
- Revision:
- 22:a19be3b505b7
- Parent:
- 21:ca8f35e24e66
- Child:
- 23:29b2722bd753
--- a/main.cpp Sat Feb 16 09:29:11 2019 +0000
+++ b/main.cpp Sun Feb 17 12:45:35 2019 +0000
@@ -123,9 +123,9 @@
setup();
//コンパスチェック用
/*while(1){
- SensingMPU();
+ //SensingMPU();
SensingHMC();
- pc.printf("%3.2f\t",nowAngle[2]);
+ //pc.printf("%3.2f\t",nowAngle[2]);
pc.printf("%3.2f\t",nowAngle_HMC); //HMC
pc.printf("\r\n");
wait_ms(30);
@@ -145,13 +145,20 @@
MoveCameraBoard();
pc.printf("Position\r\n");
+ pc.printf("g_landingcommand=%c\r\n",g_landingcommand);
+ NVIC_DisableIRQ(USART1_IRQn);
+ NVIC_DisableIRQ(USART2_IRQn);
if(g_landingcommand!='N') MatchPosition();
+ NVIC_EnableIRQ(USART1_IRQn);
+ NVIC_EnableIRQ(USART2_IRQn);
if(jevoisFlag==true) NVIC_DisableIRQ(USART1_IRQn);
else NVIC_DisableIRQ(USART2_IRQn);
pc.printf("Move Cansat\r\n");
MoveCansat(g_landingcommand);
+ if(jevoisFlag==true) NVIC_DisableIRQ(USART1_IRQn);
+ else NVIC_DisableIRQ(USART2_IRQn);
wait_ms(23);
pc.printf("finish\r\n");
@@ -161,122 +168,139 @@
}
-void MoveCansat(char g_landingcommand)
+void MoveCansat(char a)
{
- //NVIC_DisableIRQ(USART1_IRQn);
- //NVIC_DisableIRQ(USART2_IRQn);
- switch(g_landingcommand){
+ NVIC_DisableIRQ(USART1_IRQn);
+ NVIC_DisableIRQ(USART2_IRQn);
+ switch(a){
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(USART1_IRQn);
+ NVIC_EnableIRQ(USART2_IRQn);
break;
case 'Y': //MOVE_FORWARD
servoR.pulsewidth_us(Servo_high_FORWARD_R);
servoL.pulsewidth_us(Servo_high_FORWARD_L);
- //NVIC_EnableIRQ(USART1_IRQn);
- //NVIC_EnableIRQ(USART2_IRQn);
+ NVIC_EnableIRQ(USART1_IRQn);
+ NVIC_EnableIRQ(USART2_IRQn);
break;
case 'l': //MOVE_LEFT Low Speed
servoR.pulsewidth_us(Servo_slow_FORWARD_R);
- //NVIC_EnableIRQ(USART1_IRQn);
- //NVIC_EnableIRQ(USART2_IRQn);
+ servoL.pulsewidth_us(Servo_NEUTRAL_L);
+ NVIC_EnableIRQ(USART1_IRQn);
+ NVIC_EnableIRQ(USART2_IRQn);
case 'L': //MOVE_LEFT High Speed
servoR.pulsewidth_us(Servo_high_FORWARD_R);
- //NVIC_EnableIRQ(USART1_IRQn);
- //NVIC_EnableIRQ(USART2_IRQn);
+ servoL.pulsewidth_us(Servo_NEUTRAL_L);
+ NVIC_EnableIRQ(USART1_IRQn);
+ NVIC_EnableIRQ(USART2_IRQn);
case 'r': //MOVE_RIGHT Low Speed
+ servoR.pulsewidth_us(Servo_NEUTRAL_R);
servoL.pulsewidth_us(Servo_slow_FORWARD_L);
- //NVIC_EnableIRQ(USART1_IRQn);
- //NVIC_EnableIRQ(USART2_IRQn);
+ NVIC_EnableIRQ(USART1_IRQn);
+ NVIC_EnableIRQ(USART2_IRQn);
break;
case 'R': //MOVE_RIGHT High Speed
+ servoR.pulsewidth_us(Servo_NEUTRAL_R);
servoL.pulsewidth_us(Servo_high_FORWARD_L);
- //NVIC_EnableIRQ(USART1_IRQn);
- //NVIC_EnableIRQ(USART2_IRQn);
+ NVIC_EnableIRQ(USART1_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(USART1_IRQn);
+ NVIC_EnableIRQ(USART2_IRQn);
break;
case '1': //MOVE_FORWARD Speed Level 1
servoR.pulsewidth_us(Servo_high_FORWARD_R);
servoL.pulsewidth_us(Servo_high_FORWARD_L);
- //NVIC_EnableIRQ(USART1_IRQn);
wait(1);
do{
- }while((nowAngle[PITCH]<20&&nowAngle[PITCH]>-20)&&(nowAngle[ROLL]<20&&nowAngle[ROLL]>-20));
- //NVIC_EnableIRQ(USART2_IRQn);
+ SensingMPU();
+ wait_ms(30);
+ }while((nowAngle[PITCH]>20||nowAngle[PITCH]<-20)&&(nowAngle[ROLL]>20||nowAngle[ROLL]<-20));
+ NVIC_EnableIRQ(USART1_IRQn);
+ NVIC_EnableIRQ(USART2_IRQn);
break;
case '2': //MOVE_FORWARD Speed Level 2
servoR.pulsewidth_us(Servo_high_FORWARD_R);
servoL.pulsewidth_us(Servo_high_FORWARD_L);
- //NVIC_EnableIRQ(USART1_IRQn);
wait(2);
do{
- }while((nowAngle[PITCH]<20&&nowAngle[PITCH]>-20)&&(nowAngle[ROLL]<20&&nowAngle[ROLL]>-20));
- //NVIC_EnableIRQ(USART2_IRQn);
+ SensingMPU();
+ wait_ms(30);
+ }while((nowAngle[PITCH]>20||nowAngle[PITCH]<-20)&&(nowAngle[ROLL]>20||nowAngle[ROLL]<-20));
+ NVIC_EnableIRQ(USART1_IRQn);
+ NVIC_EnableIRQ(USART2_IRQn);
break;
case '3': //MOVE_FORWARD Speed Level 3
servoR.pulsewidth_us(Servo_high_FORWARD_R);
servoL.pulsewidth_us(Servo_high_FORWARD_L);
- //NVIC_EnableIRQ(USART1_IRQn);
wait(3);
do{
- }while((nowAngle[PITCH]<20&&nowAngle[PITCH]>-20)&&(nowAngle[ROLL]<20&&nowAngle[ROLL]>-20));
- //NVIC_EnableIRQ(USART2_IRQn);
+ SensingMPU();
+ wait_ms(30);
+ }while((nowAngle[PITCH]>20||nowAngle[PITCH]<-20)&&(nowAngle[ROLL]>20||nowAngle[ROLL]<-20));
+ NVIC_EnableIRQ(USART1_IRQn);
+ NVIC_EnableIRQ(USART2_IRQn);
break;
case '4': //MOVE_FORWARD Speed Level 4
servoR.pulsewidth_us(Servo_high_FORWARD_R);
servoL.pulsewidth_us(Servo_high_FORWARD_L);
- //NVIC_EnableIRQ(USART1_IRQn);
wait(4);
do{
- }while((nowAngle[PITCH]<20&&nowAngle[PITCH]>-20)&&(nowAngle[ROLL]<20&&nowAngle[ROLL]>-20));
- //NVIC_EnableIRQ(USART2_IRQn);
+ SensingMPU();
+ wait_ms(30);
+ }while((nowAngle[PITCH]>20||nowAngle[PITCH]<-20)||(nowAngle[ROLL]>20||nowAngle[ROLL]<-20));
+ NVIC_EnableIRQ(USART1_IRQn);
+ NVIC_EnableIRQ(USART2_IRQn);
break;
case '5': //MOVE_FORWARD Speed Level 5
servoR.pulsewidth_us(Servo_high_FORWARD_R);
servoL.pulsewidth_us(Servo_high_FORWARD_L);
- //NVIC_EnableIRQ(USART1_IRQn);
wait(5);
do{
- }while((nowAngle[PITCH]<20&&nowAngle[PITCH]>-20)&&(nowAngle[ROLL]<20&&nowAngle[ROLL]>-20));
- //NVIC_EnableIRQ(USART2_IRQn);
+ SensingMPU();
+ wait_ms(30);
+ }while((nowAngle[PITCH]>20||nowAngle[PITCH]<-20)&&(nowAngle[ROLL]>20||nowAngle[ROLL]<-20));
+ NVIC_EnableIRQ(USART1_IRQn);
+ NVIC_EnableIRQ(USART2_IRQn);
break;
case 'M': //MatchPosition
servoR.pulsewidth_us(Matchspeed);
- //NVIC_EnableIRQ(USART1_IRQn);
- //NVIC_EnableIRQ(USART2_IRQn);
+ NVIC_EnableIRQ(USART1_IRQn);
+ NVIC_EnableIRQ(USART2_IRQn);
break;
case 'P': //jevoisからraspberry piへの切り替え
jevoisFlag = false;
- //NVIC_EnableIRQ(USART2_IRQn);
+ NVIC_EnableIRQ(USART1_IRQn);
+ NVIC_EnableIRQ(USART2_IRQn);
break;
case 'S':
/*RasPiからの超音波判定(プログラムスタート部)*/
+ NVIC_EnableIRQ(USART1_IRQn);
+ NVIC_EnableIRQ(USART2_IRQn);
break;
default :
- //NVIC_EnableIRQ(USART1_IRQn);
- //NVIC_EnableIRQ(USART2_IRQn);
+ NVIC_EnableIRQ(USART1_IRQn);
+ NVIC_EnableIRQ(USART2_IRQn);
break;
}
@@ -285,56 +309,83 @@
}
void MoveCameraBoard(){
- pc.printf("start\r\n");
+ //pc.printf("start\r\n");
MoveCansat('N');
- pc.printf("ok\r\n");
+ //pc.printf("ok\r\n");
g_landingcommand='N';
servoTurnTable.pulsewidth_us(Turntable_speed);
wait_ms(Camera_board_wait);
servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
- pc.printf("zoom1\r\n");
+ //pc.printf("zoom1\r\n");
if(jevoisFlag == true) FocusAdjust();
else wait(1);
if(g_landingcommand!='N') return;
if(!CameraDegFlag){
for(int i=30; i<Camera_deg_B-Camera_deg_A;i=i+30){
servoCameradeg.pulsewidth_us(Camera_deg_A+i);
- pc.printf("%d\r\n",i);
+ //pc.printf("%d\r\n",i);
wait_ms(20);
}
CameraDegFlag=!CameraDegFlag;
}else{
for(int i=30; i<Camera_deg_B-Camera_deg_A;i=i+30){
servoCameradeg.pulsewidth_us(Camera_deg_B-i);
- pc.printf("%d\r\n",i);
+ //pc.printf("%d\r\n",i);
wait_ms(20);
}
CameraDegFlag = !CameraDegFlag;
}
-
- pc.printf("zoom2\r\n");
+ if(g_landingcommand!='N') return;
+ //pc.printf("zoom2\r\n");
if(jevoisFlag == true) FocusAdjust();
else wait(1);
- pc.printf("Move Board Finish\r\n");
+ //pc.printf("Move Board Finish\r\n");
return;
}
void MatchPosition(){
+ pc.printf("MatchPosition\r\n");
+ int SetLoop=0;
+
+ while(SetLoop<30){
SensingMPU();
+ wait_ms(20);
SensingHMC();
- DebugPrint();
+ wait_ms(20);
+ //DebugPrint();
+ SetLoop++;
+ //pc.printf("nowAngle_HMC=%f\tMPU=%f\r\n",nowAngle_HMC,nowAngle[YAW]);
+
+ }
float HighTargetYaw = nowAngle_HMC+5;
float LowTargetYaw = nowAngle_HMC-5;
+
if(HighTargetYaw >= 360.0) HighTargetYaw = HighTargetYaw - 360.0;
+
if(LowTargetYaw < 0) LowTargetYaw = LowTargetYaw + 360.0;
- while(nowAngle[YAW] <=HighTargetYaw && nowAngle[YAW] >= LowTargetYaw){
- MoveCansat('M');
- SensingMPU();
+ pc.printf("\r\nnow=%f\t,high=%f\t,low=%f\r\n",nowAngle[YAW],HighTargetYaw,LowTargetYaw);
+
+ MoveCansat('l');
+
+ if(HighTargetYaw-LowTargetYaw<0){
+ while(nowAngle[YAW] > HighTargetYaw && nowAngle[YAW] < LowTargetYaw){
+ //MoveCansat('r');
+ SensingMPU();
+ wait_ms(30);
+ pc.printf("nowAngle_MPU=%f\r\n",nowAngle[YAW]);
+ }
+ }else{
+ while(nowAngle[YAW] > HighTargetYaw || nowAngle[YAW] < LowTargetYaw){
+ //MoveCansat('r');
+ SensingMPU();
+ wait_ms(30);
+ pc.printf("nowAngle_MPU=%f\r\n",nowAngle[YAW]);
+ }
}
return;
}
@@ -353,6 +404,8 @@
}
void getSF_Serial_jevois(){
+
+ //pc.printf("jevois\r\n");
static char SFbuf[16]={'Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q'};
@@ -371,7 +424,7 @@
- //pc.printf("%c",SFbuf[bufcounter]);
+ pc.printf("%c",SFbuf[bufcounter]);
if(SFbuf[0]=='S' && bufcounter<5)bufcounter++;
@@ -486,7 +539,7 @@
SensingMPU();
SensingHMC();
for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]);
- pc.printf("%3.2f\t",nowAngle_HMC);
+ pc.printf("\t%3.2f\t",nowAngle_HMC);
pc.printf("\r\n");
}
@@ -748,7 +801,7 @@
*Turntable_speed = turntable_speed;
*Calib_x = calib_x;
*Calib_y = calib_y;
- *Camera_board_wait;
+ *Camera_board_wait = camera_board_wait;
SDerrorcount = -1;
}
pc.printf("SDsetup finished.\r\n");