update

arduino/hanzo2_1/hanzo2_1.ino

@@ -35,8 +35,8 @@ const unsigned int HANDLE_MIN = ( 800+30); //us
 const unsigned int HANDLE_MAX = (2300+30); //us
 
 const int LANCE_DEGREE_DEFAULT = 90; //度
-const unsigned int LANCE_MIN =  544; //us
+const unsigned int LANCE_MIN = ( 544-62); //us
-const unsigned int LANCE_MAX = 2400; //us
+const unsigned int LANCE_MAX = (2400-62); //us
 
 const byte PIN_LED           =  4;
 const byte PIN_ROT           =  2;
@@ -82,6 +82,7 @@ Motor motorR;
 Motor motorL;
 
 byte mode;
+volatile unsigned long counter = 0;
 unsigned long counterOld;
 
 /**************************************/
@@ -90,18 +91,15 @@ void trace(byte sensor){
   switch(sensor&MASK_MODE_TRACE){
   case 0b01000000:
     handle(-20);
-    motorMode(GO, GO, SPEED_DEFAULT/4, SPEED_DEFAULT);
+    motorMode(GO, GO, 0, SPEED_DEFAULT);
-//    lance(0);
     break;
   case 0b01100000:
     handle(-16);
     motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
-//    lance(0);
     break;
   case 0b00100000:
     handle(-12);
     motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
-//    lance(0);
     break;
   case 0b00110000:
     handle(-5);
@@ -138,6 +136,31 @@ void trace(byte sensor){
   }
 }
 
+void motorMode(byte leftMode, byte rightMode, byte leftSpeed, byte rightSpeed){
+  motorL.mode( leftMode,  leftSpeed);
+  motorR.mode(rightMode, rightSpeed);
+}
+
+void handle(int degree){
+  servoHandle.write(HANDLE_DEGREE_DEFAULT + degree);
+}
+
+void lance(int degree){
+  servoLance.write(LANCE_DEGREE_DEFAULT - degree);
+}
+
+/* ロータリーエンコーダーの変化を見る
+  todo:外部割り込み使ってみたい、、、
+*/
+void count(){
+  static byte rot[2] = {0}; //rot[0]:今の状態, rot[1]:前回の状態
+  rot[0] = digitalRead(PIN_ROT);
+  if(rot[0] != rot[1]){
+    rot[1] = rot[0];
+    counter++;
+  }
+}
+
 int checkMarker(byte sensor){
   int mode = MODE_TRACE;
   switch(sensor & MASK_CHECK_MARKER){
@@ -159,32 +182,6 @@ int checkMarker(byte sensor){
   return mode;
 }
 
-void motorMode(byte leftMode, byte rightMode, byte leftSpeed, byte rightSpeed){
-  motorL.mode( leftMode,  leftSpeed);
-  motorR.mode(rightMode, rightSpeed);
-}
-
-void handle(int degree){
-  servoHandle.write(HANDLE_DEGREE_DEFAULT + degree);
-}
-
-void lance(int degree){
-  servoLance.write(LANCE_DEGREE_DEFAULT - degree);
-}
-
-/* ロータリーエンコーダーの変化を見る
-  todo:外部割り込み使ってみたい、、、
-*/
-volatile unsigned long counter = 0;
-void count(){
-  static byte rot[2] = {0}; //rot[0]:今の状態, rot[1]:前回の状態
-  rot[0] = digitalRead(PIN_ROT);
-  if(rot[0] != rot[1]){
-    rot[1] = rot[0];
-    counter++;
-  }
-}
-
 void sensorInit(){
   pinMode(PIN_SENSOR_0, INPUT);
   pinMode(PIN_SENSOR_1, INPUT);
@@ -267,9 +264,11 @@ void loop(){
   case MODE_TRACE:
     trace(sensor);
     mode = checkMarker(sensor);
-    //6周する前に、円筒標的を狙いに行く
+//    6周する前に、円筒標的を狙いに行く
-    if( counter > LAP_COUNT*6 - LAP_COUNT/14 ){
+//    if( counter > LAP_COUNT*6 - LAP_COUNT/14 ){
-      mode = MODE_TARGET_CYLINDER;
+//      mode = MODE_TARGET_CYLINDER;
+    if(counter > LAP_COUNT){
+      mode = MODE_STOP;
     }
     break;
   case MODE_LEFT: //左カーブ
@@ -298,10 +297,10 @@ void loop(){
   if(millis() - millis_lance > LANCE_INTERVAL){
     millis_lance = millis();
     switch(servoLance.read()){
-    case LANCE_ANGLE1:
+    case LANCE_DEGREE_DEFAULT - LANCE_ANGLE1:
       lance(LANCE_ANGLE2);
       break;
-    case LANCE_ANGLE2:
+    case LANCE_DEGREE_DEFAULT - LANCE_ANGLE2:
       lance(LANCE_ANGLE1);
       break;
     }