Diff: main.cpp

Revision:

16:26cee2aaf61d

Parent:

15:1519fccfcae3

--- a/main.cpp	Tue Feb 12 13:39:26 2019 +0000
+++ b/main.cpp	Wed Feb 13 08:18:10 2019 +0000
@@ -25,6 +25,8 @@
 #define focus_NEUTRAL  1455  //焦点合わせ用サーボ
 #define camera_deg_A 1400   //カメラ角度調整
 #define camera_deg_B 1800
+#define pint_speed 25
+#define pint_wait 3.5
 
 #define MOVE_NEUTRAL    0
 #define MOVE_FORWARD    1   
@@ -60,7 +62,8 @@
             int *Servo_high_FORWARD_R,int *Servo_high_FORWARD_L,
             int *Servo_slow_FORWARD_R,int *Servo_slow_FORWARD_L,
             int *Turntable_NEUTRAL,int *Matchspeed,int *Focus_NEUTRAL,
-            int *Camera_deg_A, int *Camera_deg_B
+            int *Camera_deg_A, int *Camera_deg_B,
+            int *Pint_speed, float *Pint_wait
             );
 
 static float nowAngle[3] = {0,0,0},nowAngle_HMC=0;
@@ -307,9 +310,9 @@
     //pc.printf("set\r\n");
     wait(1);
     
-    servoCameraPinto.pulsewidth_us(focus_NEUTRAL+20);
+    servoCameraPinto.pulsewidth_us(focus_NEUTRAL+pint_speed);
     //pc.printf("check\r\n");
-    wait(4);
+    wait(pint_wait);
     servoCameraPinto.pulsewidth_us(focus_NEUTRAL);
       
   return;      
@@ -419,7 +422,9 @@
                &Servo_high_FORWARD_R, &Servo_high_FORWARD_L,
                &Servo_slow_FORWARD_R,&Servo_slow_FORWARD_L,
                &Turntable_NEUTRAL,&Matchspeed,&Focus_NEUTRAL,
-               &Camera_deg_A, &Camera_deg_B
+               &Camera_deg_A, &Camera_deg_B,
+               &Pint_speed,&Pint_wait
+               
                );
      
     Init_sensors();
@@ -581,7 +586,8 @@
                int *Servo_high_FORWARD_R, int *Servo_high_FORWARD_L,
                int *Servo_slow_FORWARD_R,int *Servo_slow_FORWARD_L,
                int *Turntable_NEUTRAL,int *Matchspeed,int *Focus_NEUTRAL,
-               int *Camera_deg_A, int *Camera_deg_B
+               int *Camera_deg_A, int *Camera_deg_B,
+               int *Pint_speed, float *Pint_wait
                ){
 
     pc.printf("SDsetup start.\r\n");    
@@ -600,7 +606,9 @@
         "MATCH_SPEED",
         "FOCUS_NEUTRAL",
         "CAMERA_DEG_A",
-        "CAMERA_DEG_B"
+        "CAMERA_DEG_B",
+        "PINT_SPEED",
+        "PINT_WAIT"
     };
 
     fp = fopen("/sd/option.txt","r");
@@ -652,6 +660,14 @@
         else{                                         *Camera_deg_B = camera_deg_B;
                                                       SDerrorcount++;
         }
+         if(GetParameter(fp,paramNames[11],parameter)) *Pint_speed = atof(parameter);
+        else{                                         *Pint_speed = pint_speed;
+                                                      SDerrorcount++;
+        }
+         if(GetParameter(fp,paramNames[12],parameter)) *Pint_wait = atof(parameter);
+        else{                                         *Pint_wait = pint_wait;
+                                                      SDerrorcount++;
+        }
         
         fclose(fp);
 
@@ -669,6 +685,8 @@
         *Focus_NEUTRAL = focus_NEUTRAL;
         *Camera_deg_A = camera_deg_A;
         *Camera_deg_B = camera_deg_B;
+        *Pint_speed = pint_speed;
+        *Pint_wait = pint_wait;
         SDerrorcount = -1;
     }
     pc.printf("SDsetup finished.\r\n");
@@ -682,6 +700,7 @@
     pc.printf("Turntable_NEUTRAL = %d, Matchspeed =  %d\r\n", *Turntable_NEUTRAL, *Matchspeed);
     pc.printf("Focus_NEUTRAL = %d\r\n", *Focus_NEUTRAL);
     pc.printf("Camera_deg_A = %d, Camera_deg_B =  %d\r\n", *Camera_deg_A, *Camera_deg_B);
+    pc.printf("Pint_speed = %d, Pint_wait =  %f\r\n", *Pint_speed, *Pint_wait);
     
     return SDerrorcount;
 }