Diff: main.cpp

Revision:

37:8e273677500d

Parent:

36:e13fca1666a4

Child:

38:7c323abc62fb

--- a/main.cpp	Sat Mar 02 00:16:23 2019 +0000
+++ b/main.cpp	Sun Mar 03 06:54:50 2019 +0000
@@ -69,7 +69,7 @@
             int *Camera_board_wait
             );
 
-static float nowAngle[3] = {0,0,0},nowAngle_HMC=0;
+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;
@@ -112,7 +112,7 @@
 
 DigitalOut led1(PA_0);  //tanakaOK kimuraは動作不良
 DigitalOut led2(PA_1);  //tanakaOK kimuraOK
-DigitalOut led3(PB_5);  //使ってないよ
+DigitalIn BoardCheck(PB_5);  //使ってないよ
 DigitalOut led4(PB_4);  //使ってないよ
 
 
@@ -145,7 +145,7 @@
     pc2.attach(getSF_Serial_pi, Serial::RxIrq);
     t2.start();
     
-    /*NVIC_DisableIRQ(USART2_IRQn);
+    NVIC_DisableIRQ(USART2_IRQn);
     while(g_landingcommand != 'B'){
         wait_ms(30);
         }
@@ -154,7 +154,7 @@
     g_landingcommand='N';
     wait(5); 
     pc.printf("start\r\n");  
-    */
+    
     
     servoCameraPinto.pulsewidth_us(Focus_NEUTRAL-200);
     wait(1);
@@ -275,6 +275,20 @@
             NVIC_EnableIRQ(USART6_IRQn);
             NVIC_EnableIRQ(USART2_IRQn); 
             break;
+            
+        case '0': //MOVE_FORWARD Speed Level 1
+            servoR.pulsewidth_us(Servo_high_FORWARD_R);
+            servoL.pulsewidth_us(Servo_high_FORWARD_L);
+            wait(5);  
+            g_fp = fopen( "/sd/Datalog01.txt","a" );
+            fprintf(g_fp, "Time = %d min %d sec : Finish!.\r\n",(int)t2.read()/60,(int)t2.read()%60);        
+            fclose(g_fp);
+            servoR.pulsewidth_us(Servo_NEUTRAL_R);
+            servoL.pulsewidth_us(Servo_NEUTRAL_L);
+            exit(0);
+            NVIC_EnableIRQ(USART6_IRQn);
+            NVIC_EnableIRQ(USART2_IRQn);
+            break;
         
         case '1': //MOVE_FORWARD Speed Level 1
             servoR.pulsewidth_us(Servo_high_FORWARD_R);
@@ -470,17 +484,29 @@
     if(TurnTime >=0){
         servoTurnTable.pulsewidth_us(Turntable_NEUTRAL + Turntable_speed);
         wait_ms(Camera_board_wait*TurnTime);
+        /*while(BoardCheck == 0){
+            wait_ms(30);
+            }*/
     }else{
         servoTurnTable.pulsewidth_us(Turntable_NEUTRAL - Turntable_speed);
         wait_ms(-Camera_board_wait*TurnTime);
+        /*while(BoardCheck == 0){
+            wait_ms(30);
+            }*/
     }
     servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
     
+    /*if(jevoisFlag == false ){
+        servoTurnTable.pulsewidth_us(Turntable_NEUTRAL + Turntable_speed);
+        wait_ms((float)Time360/(float)2);
+        servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
+        }*/
+    
     int SetLoop=0;
     
     while(SetLoop<30){
     SensingMPU();
-    wait_ms(20);
+    wait_ms(10);
     //SensingHMC();
     //wait_ms(20);
     //DebugPrint();
@@ -493,15 +519,16 @@
     
     TargetDeg = 360*(float)Camera_board_wait*((float)g_CameraDegCounter - ReturnCount)/(float)Time360;
     
-    if(TargetDeg > 360)TargetDeg = TargetDeg - 360; 
+    if(TargetDeg > 360)TargetDeg = TargetDeg - 360;
+    if(TargetDeg < 0)TargetDeg = TargetDeg + 360; 
     
-    float HighTargetYaw = TargetDeg + Match_wid;
-    float LowTargetYaw = TargetDeg - Match_wid;
+    float HighTargetYaw = nowAngle[YAW] + TargetDeg + Match_wid;
+    float LowTargetYaw = nowAngle[YAW] + TargetDeg - Match_wid;
     
     
-    if(HighTargetYaw >= 360.0) HighTargetYaw = HighTargetYaw - 360.0;
-    
-    if(LowTargetYaw < 0) LowTargetYaw = LowTargetYaw + 360.0;
+    if(HighTargetYaw >= 360.0f) HighTargetYaw = HighTargetYaw - 360.0f;
+    if(LowTargetYaw >= 360.0f) LowTargetYaw = LowTargetYaw - 360.0f;
+    if(LowTargetYaw < 0.0f) LowTargetYaw = LowTargetYaw + 360.0f;
     
     pc.printf("\r\nnow=%f\t,high=%f\t,low=%f\r\n",nowAngle[YAW],HighTargetYaw,LowTargetYaw);
     
@@ -657,7 +684,7 @@
     
     led1 = 1;
     led2 = 1;
-    led3 = 1;
+    //led3 = 1;
     led4 = 1;
     
     servoR.pulsewidth_us(Servo_NEUTRAL_R);       //servo initialize
@@ -704,7 +731,7 @@
         pc.printf("\r\n");
         led1 = !led1;
         led2 = !led2;
-        led3 = !led3;
+        //led3 = !led3;
         led4 = !led4;
     }
     
@@ -721,7 +748,7 @@
     
     led1 = 0;
     led2 = 0;
-    led3 = 0;
+    //led3 = 0;
     led4 = 0;
         
     wait(0.2);
@@ -729,7 +756,7 @@
     
     g_fp = fopen( "/sd/Datalog01.txt" ,"a" );
     if( g_fp == NULL ) {
-        pc.printf( "ファイルオープンエラー\r\n" );
+        pc.printf( "File open error!!!\r\n" );
     }
     fprintf(g_fp,"\r\n-------------------------\r\n");
     fprintf(g_fp,"All initialized.\r\n");
@@ -764,7 +791,7 @@
     
     
     //外れ値対策
-    for(uint8_t i=0; i<3; i++) rpy[i] *= 180.0f/PI;
+    for(uint8_t i=0; i<3; i++) rpy[i] *= 180.0f/(float)PI;
     rpy[ROLL] -= FirstROLL;
     rpy[PITCH] -= FirstPITCH;
     if(!setupFlag){