imano yuta / Mbed 2 deprecated sahaquiel_raspi_uart_test

skipper_raspi_uart_test

Dependencies:   mbed MPU6050_2 HMC5883L_4 SDFileSystem3

Diff: main.cpp

Revision:
33:5f77118eacd5
Parent:
32:ab030832d180
Child:
34:0c2b67b31b12
--- a/main.cpp	Thu Feb 28 11:59:51 2019 +0000
+++ b/main.cpp	Fri Mar 01 08:54:00 2019 +0000
@@ -465,6 +465,8 @@
     
     TargetDeg = 360*(float)Camera_board_wait*((float)g_CameraDegCounter - ReturnCount)/(float)Time360;
     
+    if(TargetDeg > 360)TargetDeg = TargetDeg - 360; 
+    
     float HighTargetYaw = TargetDeg + Match_wid;
     float LowTargetYaw = TargetDeg - Match_wid;
     
@@ -501,12 +503,16 @@
 
 void FocusAdjust(){
     if(FocusFlag == false){
-         servoCameraPinto.pulsewidth_us(Focus_NEUTRAL + 200);
-         wait(1);
+         servoCameraPinto.pulsewidth_us(Focus_NEUTRAL + Pint_speed);
+         servoR.pulsewidth_us(Servo_NEUTRAL_R);       //servo initialize
+         servoL.pulsewidth_us(Servo_NEUTRAL_L);
+         wait(Pint_wait);
          FocusFlag = !FocusFlag;
     }else{
-         servoCameraPinto.pulsewidth_us(Focus_NEUTRAL - 200);
-         wait(1);
+         servoCameraPinto.pulsewidth_us(Focus_NEUTRAL - Pint_speed);
+         servoR.pulsewidth_us(Servo_NEUTRAL_R);       //servo initialize
+         servoL.pulsewidth_us(Servo_NEUTRAL_L);
+         wait(Pint_wait);
          FocusFlag = !FocusFlag;
     }
     servoCameraPinto.pulsewidth_us(Focus_NEUTRAL);