コメントの間違いを修正

Motor.cpp

@@ -1,6 +1,6 @@
 /*
 Motor.cpp
-(C)2013 kou029w - MIT License
+(C)2014 kou029w - MIT License
 */
 #include "Motor.h"
 

Motor.h

@@ -3,7 +3,6 @@ Motor.h - モータードライバ(L298P シールド)のためのライブラ
 
 ## 概要 ##
 このライブラリは、モータードライバ(L298P シールド)のためのシンプルなライブラリです。
-PWM対応ピンが1つしか必要ないのがこのライブラリの特徴です。
 
 ## 使い方 ##
 例:
@@ -11,7 +10,7 @@ PWM対応ピンが1つしか必要ないのがこのライブラリの特徴で
     Motor motor;
     void setup(){
     //  motor.attach(pin1, pin2);
-      motor.attach(5, 6);
+      motor.attach(5, 4);
     }
     void loop(){
     //  motor.mode(GO); //正転
@@ -22,29 +21,13 @@ PWM対応ピンが1つしか必要ないのがこのライブラリの特徴で
     }
 
 注意:
-使用するモータードライバの動作は、  
-1ピンの場合:
-    動作 |pin1
-    -----+-----
-    STOP |  L 
-    GO   |  H 
-
-2ピンの場合:
-    動作      |pin1 |pin2
-    ----------+-----+-----
-    STOP      |  L  |  L
-    GO        |  H  |  L
-    STOP      |  L  |  H
-    BACK      |  H  |  H
-
-でなければなりません。また、
   void Motor::mode(char mode, byte speed);
   void Motor::speed(int speed);
 
 これらを使用する場合、pin1はPWM対応でなければなりません。
 
 ## ライセンス ##
-(C)2013 kou029w - MIT License
+(C)2014 kou029w - MIT License
 */
 
 #ifndef Motor_h

README.md

@@ -1,10 +1,11 @@
-# 競技用ランサーロボット 半蔵4.0 制御プログラム #
+
+# 競技用ランサーロボット 半蔵4.1 制御プログラム #
 
 ## これはなに ##
-[ロボットグランプリ](http://www.robotgrandprix.com/)に向けて製作中のマシン、半蔵4.0のための制御プログラムです。
+[ロボットグランプリ](http://www.robotgrandprix.com/)に向けて製作中のマシン、半蔵4.1のための制御プログラムです。
 
 ## システム構成 ##
-以下に、半蔵4.0のシステム構成を示します。
+以下に、半蔵4.1のシステム構成を示します。
 
 ### Hardware ###
     [@ @@@@@@ @] - Sensor array
@@ -28,7 +29,7 @@
     MPU                     - Atmel AVR
     Rotary encoder          - ロータリーエンコーダー
     Sensor array            - 反射型フォトインタラプタSG-2BC
-    Motor for driving       - 駆動用モーター/ハイスピードギアボックスHE
+    Motor for driving       - 駆動用モーター/ダイセン
     Servomotor for steering - ステアリング用サーボモーター
     Servomotor for lance    - ランス用サーボモーター
 
@@ -36,37 +37,18 @@
 * ホイールベース : 約150mm
 * 車重 : 1kg以下
 * 車体材料 : タミヤ ユニバーサルプレートL
-* 使用ギアボックス : タミヤ ハイスピードギアボックスHE
+* 使用ギアボックス : 
-* ギア比 : 11.6:1
+* ギア比 : 
-* 使用モーター : 上記ギアボックスに付属するもの
+* 使用モーター : 
 * 使用サーボモーター : Savox SC-0352(ランス用)
 
 ### 回路仕様 ###
-* 動作電圧 : 7.2V以上
+* 動作電圧 : 11.1V(駆動用モーター), 7.4V(MPU, サーボ)
 * 使用マイコン : Atmel AVR (Arduino)
 * 使用センサーアレイ : SG-2BC x 8 (ロボラボ講習会のもの)
 
-### Software ###
-    [count/mark]
-    |
-    +-><mode>
-        |
-        +->[lance]
-        |
-        | [line]
-        |  |
-        +--+->[motor/steering]
-
-    mode     - 全体の動作を決定
-    count    - ロータリーエンコーダーから得られる情報
-    mark     - コース上のマーカーから得られる情報
-    line     - コース上の線から得られる情報
-    motor    - 駆動用モーターの動作
-    steering - ステアリングの動作
-    lance    - ランスの動作
-
 ### ソフトウェア仕様 ###
 * 使用言語 : Arduino 1.0
 
 ## ライセンス ##
-(C)2013 kou029w - MIT Licence
+(C)2014 kou029w - MIT Licence

hanzo4.ino

@@ -1,304 +0,0 @@
-/*
-競技用ランサーロボット
-半蔵 4.0
-(C)2013 kou029w - MIT License
-*/
-
-#include <Tone.h>
-#include <Servo.h>
-#include "Motor.h"
-
-const unsigned long LAP_DISTANCE  = 28500;  //ロータリーエンコーダーのカウント数/周
-const unsigned long TURN_DISTANCE = 4650;  //カーブ中でのカウント数
-
-const int LANCE_ANGLE_E =  40;  //度
-const int LANCE_ANGLE_F =  61;  //度
-const int LANCE_ANGLE_A =  29;  //度
-const int LANCE_ANGLE_B = -32;  //度
-const int LANCE_ANGLE_C =  34;  //度
-const int LANCE_ANGLE_D = -42;  //度
-
-//const unsigned char SPEED_DEFAULT = 0;
-const unsigned char SPEED_DEFAULT = 127;
-
-//const float STEERING_KP = 3.8;
-
-// Servo : SC-1267SG
-const unsigned int STEERING_CENTER = 1810;  //us
-const unsigned int STEERING_MIN = (STEERING_CENTER-780); //us
-const unsigned int STEERING_MAX = (STEERING_CENTER+780); //us
-
-//Servo  : SC-0352
-const unsigned int LANCE_CENTER = 1530;  //us
-const unsigned int LANCE_MIN = (LANCE_CENTER-780); //us
-const unsigned int LANCE_MAX = (LANCE_CENTER+780); //us
-
-const unsigned char PIN_BUZZER =  3;
-const unsigned char PIN_ROT    =  2;
-
-const unsigned char PIN_SENSOR_0 = 19;  //右端
-const unsigned char PIN_SENSOR_1 = 18;
-const unsigned char PIN_SENSOR_2 = 17;
-const unsigned char PIN_SENSOR_3 = 16;
-const unsigned char PIN_SENSOR_4 = 15;
-const unsigned char PIN_SENSOR_5 = 14;
-const unsigned char PIN_SENSOR_6 = 13;
-const unsigned char PIN_SENSOR_7 = 12;  //左端
-
-const unsigned char PIN_SERVO_STEERING  =  9;
-const unsigned char PIN_SERVO_LANCE     = 10;
-
-const unsigned char PIN_MOTOR_LEFT_E  =  5;
-const unsigned char PIN_MOTOR_LEFT_M  =  4;
-const unsigned char PIN_MOTOR_RIGHT_E =  6;
-const unsigned char PIN_MOTOR_RIGHT_M =  7;
-
-const unsigned char MASK_LINE    = 0b01111110;
-const unsigned char MASK_MARKER  = 0b10000001;
-
-/**************************************/
-
-Servo steeringServo;
-Servo lanceServo;
-Motor motorR;
-Motor motorL;
-
-//ラインが1、地面が0、LSBが左端、MSBが右端
-unsigned char sensor = 0x00;  
-int errorCount = 0;
-
-int speed = SPEED_DEFAULT;
-
-volatile unsigned long distance = 0;
-unsigned long modeChangedDistance = 0;
-
-/**************************************/
-
-enum mode_t {
-  MODE_STOP,      //停止
-  MODE_STRAIGHT,  //まっすぐ進む
-  MODE_TURN,      //左に曲がる
-  MODE_ATTACK_E,  //平行標的1
-  MODE_ATTACK_F,  //平行標的2
-  MODE_ATTACK_A,  //右垂直標的
-  MODE_ATTACK_B,  //左垂直標的
-  MODE_ATTACK_C,  //右垂直標的
-  MODE_ATTACK_D,  //左垂直標的
-  MODE_ATTACK_CYLINDER     //円筒標的
-} 
-mode;
-
-/**************************************/
-
-/* センサーを読んで、車体の動作を決定する */
-void trace(){
-  switch(sensor&MASK_LINE){
-  case 0b01000000:
-    steering(-21);
-    motorL.speed(-speed/2);
-    motorR.speed(speed/2);
-    break;
-  case 0b01100000:
-    steering(-20);
-    motorL.speed(-speed/2);
-    motorR.speed(speed/2);
-    break;
-  case 0b00100000:
-    steering(-19);
-    motorL.speed(-speed/2);
-    motorR.speed(speed/2);
-    break;
-  case 0b00110000:
-    steering(-2);
-    motorL.speed(speed);
-    motorR.speed(speed);
-    break;
-  case 0b00010000:
-    steering(-1);
-    motorL.speed(speed);
-    motorR.speed(speed);
-    break;
-  case 0b00011000:
-    steering(0);
-    motorL.speed(speed);
-    motorR.speed(speed);
-    break;
-  case 0b00001000:
-    steering(1);
-    motorL.speed(speed);
-    motorR.speed(speed);
-    break;
-  case 0b00001100:
-    steering(2);
-    motorL.speed(speed);
-    motorR.speed(speed);
-    break;
-  case 0b00000100:
-    steering(3);
-    motorL.speed(0);
-    motorR.speed(0);
-    break;
-  case 0b00000110:
-    steering(4);
-    motorL.speed(0);
-    motorR.speed(0);
-    break;
-  case 0b00000010:
-    steering(5);
-    motorL.speed(0);
-    motorR.speed(0);
-    break;
-  }
-}
-
-/* モードを決定する */
-void modeSet(){
-  if(sensor == 0) errorCount++;
-  else errorCount = 0;
-  if(distance > LAP_DISTANCE*13 || errorCount > 5000){
-    mode = MODE_STOP;
-    return;
-  }
-  switch(sensor&MASK_MARKER){
-  default:
-  case 0b00000000:
-    mode = MODE_STRAIGHT;
-    break;
-  case 0b10000001:
-    modeChangedDistance = distance;
-    mode = MODE_TURN;
-    break;
-  case 0b00000001:
-    modeChangedDistance = distance;
-    if(distance%LAP_DISTANCE < LAP_DISTANCE/8){  //まずは、E的をねらいたい
-      mode = MODE_ATTACK_E;
-    }else if(distance%LAP_DISTANCE < LAP_DISTANCE/4){  //次に、F的をねらいたい
-      mode = MODE_ATTACK_F;
-    }else if(distance%LAP_DISTANCE > LAP_DISTANCE/2){  //後半は垂直標的をねらいたい
-      if(distance%LAP_DISTANCE < LAP_DISTANCE*0.6){
-        mode = MODE_ATTACK_A;
-      }else if(distance%LAP_DISTANCE < LAP_DISTANCE*0.85){
-        mode = MODE_ATTACK_C;
-      }
-    }
-    break;
-  case 0b10000000:
-    modeChangedDistance = distance;
-    if(distance%LAP_DISTANCE < LAP_DISTANCE/2){
-      ;
-    }else if(distance%LAP_DISTANCE < LAP_DISTANCE*0.75){
-      mode = MODE_ATTACK_B;
-    }else if(distance%LAP_DISTANCE < LAP_DISTANCE*0.85){
-      mode = MODE_ATTACK_D;
-    }
-    break;
-  }
-}
-
-void setup(){
-//  Serial.begin(9600);
-  //サーボ
-  steeringServo.attach(PIN_SERVO_STEERING, STEERING_MIN, STEERING_MAX);
-  lanceServo.attach(PIN_SERVO_LANCE, LANCE_MIN, LANCE_MAX);
-  steering(0);
-  lance(0);
-  //モーター
-  motorL.attach(PIN_MOTOR_LEFT_E, PIN_MOTOR_LEFT_M);
-  motorR.attach(PIN_MOTOR_RIGHT_E, PIN_MOTOR_RIGHT_M);
-  motorL.mode(STOP);
-  motorR.mode(STOP);
-  //センサー
-  sensorInit();
-  //エンコーダー
-  pinMode(PIN_ROT, INPUT);
-  attachInterrupt(0, rot, RISING);
-  //ブザー
-  pinMode(PIN_BUZZER, OUTPUT);
-  startBeep();
-  /* 3秒停止 */
-  motorL.mode(STOP);
-  motorR.mode(STOP);
-  delay(3000);
-  /* スタート */
-  mode = MODE_STRAIGHT;
-}
-
-void loop(){
-//  Serial.println(distance);
-  sensorRead();
-  switch(mode){
-  default:
-  case MODE_STOP:      //停止
-    steering(-20);
-    motorL.mode(STOP);
-    motorR.mode(STOP);
-    stopBeep();
-    while(1){
-      ;
-    };
-    break;
-  case MODE_STRAIGHT:  //まっすぐ進む
-    trace();
-    modeSet();
-    break;
-  case MODE_TURN:      //左に曲がる
-    trace();
-    lance(0);
-    if(distance - modeChangedDistance > TURN_DISTANCE){
-      mode = MODE_STRAIGHT;
-    }
-    break;
-  case MODE_ATTACK_E:  //平行標的1
-    trace();
-    lance(LANCE_ANGLE_E);
-    if(distance - modeChangedDistance > 800){ //マーカーから少し進んで、叩く
-      lance(LANCE_ANGLE_E + 8);
-      mode = MODE_STRAIGHT;
-    }
-    break;
-  case MODE_ATTACK_F:  //平行標的2
-    trace();
-    lance(LANCE_ANGLE_F);
-    if(distance - modeChangedDistance > 1000){ //マーカーから少し進んで、叩く
-      lance(LANCE_ANGLE_F + 8);
-      mode = MODE_STRAIGHT;
-    }
-    break;
-  case MODE_ATTACK_A:  //右垂直標
-    trace();
-    lance(LANCE_ANGLE_A);
-    if(distance - modeChangedDistance > 1500){ //マーカーから少し進んで、まっすぐに戻す
-      lance(0);
-      mode = MODE_STRAIGHT;
-    }
-    break;
-  case MODE_ATTACK_C:  //右垂直標
-    trace();
-    lance(LANCE_ANGLE_C);
-    if(distance - modeChangedDistance > 1500){ //マーカーから少し進んで、まっすぐに戻す
-      lance(0);
-      mode = MODE_STRAIGHT;
-    }
-    break;
-  case MODE_ATTACK_B:  //左垂直標
-    trace();
-    lance(LANCE_ANGLE_B);
-    if(distance - modeChangedDistance > 1500){ //マーカーから少し進んで、まっすぐに戻す
-      lance(0);
-      mode = MODE_STRAIGHT;
-    }
-    break;
-  case MODE_ATTACK_D:  //左垂直標
-    trace();
-    lance(LANCE_ANGLE_D);
-    if(distance - modeChangedDistance > 1500){ //マーカーから少し進んで、まっすぐに戻す
-      lance(0);
-      mode = MODE_STRAIGHT;
-    }
-    break;
-  case MODE_ATTACK_CYLINDER:    //円筒標的
-    trace();
-    mode = MODE_STRAIGHT;
-    break;
-  }
-}

hanzo4Utils.ino

@@ -1,69 +0,0 @@
-/* ステアリングを中央からa[度]だけ動かす
-  <- - 0 + ->
-      ||
-      ||
- []---{}---[]
-     / .\
-*/
-void steering(int a){
-  steeringServo.write(90 + a);
-}
-
-/* ランスを中央からa[度]だけ動かす
-  <- - 0 + ->
-     / .\
-    /  | \
- []+--{}--+[]
-*/
-void lance(int a){
-  lanceServo.write(90 - a);
-}
-
-/* センサーのアレイの初期化 */
-void sensorInit(){
-  pinMode(PIN_SENSOR_0, INPUT);  //右端
-  pinMode(PIN_SENSOR_1, INPUT);
-  pinMode(PIN_SENSOR_2, INPUT);
-  pinMode(PIN_SENSOR_3, INPUT);
-  pinMode(PIN_SENSOR_4, INPUT);
-  pinMode(PIN_SENSOR_5, INPUT);
-  pinMode(PIN_SENSOR_6, INPUT);
-  pinMode(PIN_SENSOR_7, INPUT);  //左端
-}
-
-/* センサーアレイの状態を見る*/
-unsigned char sensorRead(){
-  unsigned char b;
-  //黒が1、白が0
-  if(digitalRead(PIN_SENSOR_0)) b += 0b00000001;  //右端
-  if(digitalRead(PIN_SENSOR_1)) b += 0b00000010;
-  if(digitalRead(PIN_SENSOR_2)) b += 0b00000100;
-  if(digitalRead(PIN_SENSOR_3)) b += 0b00001000;
-  if(digitalRead(PIN_SENSOR_4)) b += 0b00010000;
-  if(digitalRead(PIN_SENSOR_5)) b += 0b00100000;
-  if(digitalRead(PIN_SENSOR_6)) b += 0b01000000;
-  if(digitalRead(PIN_SENSOR_7)) b += 0b10000000;  //左端
-  b ^= 0xff;  //今回ラインが白なので、反転して、ラインを1とする
-  return sensor = b;
-}
-
-/* エンコーダーの割り込み処理 */
-void rot(){
-  distance++;
-}
-
-/* 起動音 */
-void startBeep(){
-  tone(PIN_BUZZER,2000);
-  delay(100);
-  tone(PIN_BUZZER,1000);
-  delay(100);
-  noTone(PIN_BUZZER);
-}
-
-/* 終了音 */
-void stopBeep(){
-  tone(PIN_BUZZER, 2000);
-  delay(2000);
-  noTone(PIN_BUZZER);
-}

hanzo4_1.ino

@@ -0,0 +1,347 @@
+/*
+競技用ランサーロボット
+半蔵 4.1
+(C)2014 kou029w - MIT License
+*/
+
+#include <MsTimer2.h>
+#include <Tone.h>
+#include <Servo.h>
+#include "Motor.h"
+
+/* 標的の直前にあるマーカーを見つけ出す範囲 : {最小, 最大} */
+struct range_t{
+  unsigned long min;
+  unsigned long max;
+};
+
+/* 標的のためのランスの角度 : {before[度], after[度]} */
+struct lanceAngle_t{
+  int before;
+  int after;
+};
+
+/* 標的の情報 */
+struct target_t{
+  struct range_t range;
+  unsigned long way;  //マーカーから標的までの距離
+  struct lanceAngle_t lanceAngle;
+};
+
+/**************************************/
+/* 動作パラメーター */
+
+unsigned long lapDistance  = 28550; //ロータリーエンコーダーのカウント数/周
+unsigned long turnDistance = 4650;  //カーブ中でのカウント数
+
+struct target_t targetA = {{lapDistance/2,    lapDistance*0.6 }, 900, { 29,  0}};
+struct target_t targetB = {{lapDistance/2,    lapDistance*0.75}, 900, {-32,  0}};
+struct target_t targetC = {{lapDistance*0.6,  lapDistance*0.85}, 900, { 34,  0}};
+struct target_t targetD = {{lapDistance*0.75, lapDistance*0.85}, 900, {-42,  0}};
+struct target_t targetE = {{ 500,4000}, 750, { 48, 53}};
+struct target_t targetF = {{4000,7000}, 750, { 60, 67}};
+
+int laps = 3;  //最大周回数
+bool silent = false;  //サイレントモード(true:ブザーを鳴らさない)
+
+int steeringAngleMin = -30; //度
+int steeringAngleMax =  10; //度
+
+int deltaSpeedMax = 500;  //最大速度差
+int speedMax = 255;  //最大速度
+
+int cycle = 1;  //動作周波数(ms)
+
+int errorCountThreshold = 500;  //最大読み取りエラー回数(これを超えると停止する)
+
+/**************************************/
+/* ピンの設定 */
+
+// Servo : SC-1267SG
+unsigned int steeringServoCenter = 1808;  //us
+unsigned int steeringServoMin = (steeringServoCenter-780); //us
+unsigned int steeringServoMax = (steeringServoCenter+780); //us
+
+// Servo : SC-0352
+unsigned int lanceServoCenter = 1560;  //us
+unsigned int lanceServoMin = (lanceServoCenter-780); //us
+unsigned int lanceServoMax = (lanceServoCenter+780); //us
+
+unsigned char buzzerPin  = 3;
+unsigned char rotPin     = 2;
+
+unsigned char numSensors = 8;
+unsigned char sensorPin[] = {19,18,17,16,15,14,13,12};  //右端から順番に左端へ
+
+unsigned char lineSensorMask    = 0b01111110;
+unsigned char markerSensorMask  = 0b10000001;
+
+unsigned char steeringServoPin  =  9;
+unsigned char lanceServoPin     = 10;
+
+unsigned char motorLeftEnablePin     = 5;
+unsigned char motorLeftDirectionPin  = 4;
+unsigned char motorRightEnablePin    = 6;
+unsigned char motorRightDirectionPin = 7;
+
+/**************************************/
+/* グローバル変数 */
+
+Servo steeringServo;
+Servo lanceServo;
+Motor motorR;
+Motor motorL;
+
+//ラインが1、地面が0、MSBが左端、LSBが右端
+unsigned char sensor = 0x00;
+
+// 右のモーターと左のモーターの速度差 : (右のモーター)-(左のモーター)
+int deltaSpeed = 0;
+// 駆動用モーターの速度
+int speed = 0;
+
+// ステアリングの角度
+long angle = 0; // 度
+
+int errorCount = 0;
+
+volatile unsigned long distance = 0;
+unsigned long modeChangedDistance = 0;
+
+/**************************************/
+
+/* 動作モード */
+enum mode_t {
+  MODE_STOP,      //停止
+  MODE_STRAIGHT,  //まっすぐ進む
+  MODE_TURN,      //左に曲がる
+  MODE_ATTACK_E,  //平行標的1
+  MODE_ATTACK_F,  //平行標的2
+  MODE_ATTACK_A,  //右垂直標的
+  MODE_ATTACK_B,  //左垂直標的
+  MODE_ATTACK_C,  //右垂直標的
+  MODE_ATTACK_D,  //左垂直標的
+  MODE_ATTACK_CYLINDER     //円筒標的
+} mode;
+
+/**************************************/
+
+/* センサーを読んで、車体の動作を決定する */
+void trace(){
+  switch(sensor&lineSensorMask){
+  default:
+    errorCount++;
+    return;
+  case 0b01000000:
+    speed = speedMax*0.3;
+    deltaSpeed = -145;
+    angle = -15;
+    break;
+  case 0b01100000:
+    speed = speedMax*0.5;
+    deltaSpeed = -115;
+    angle = -13;
+    break;
+  case 0b00100000:
+    speed = speedMax*0.7;
+    deltaSpeed = -95;
+    angle = -11;
+    break;
+  case 0b00110000:
+    speed = speedMax*0.9;
+    deltaSpeed = -75;
+    angle = -9;
+    break;
+  case 0b00010000:
+    speed = speedMax;
+    deltaSpeed = 0;
+    angle = -1;
+    break;
+  case 0b00011000:
+    speed = speedMax;
+    deltaSpeed = 0;
+    angle = 0;
+    break;
+  case 0b00001000:
+    speed = speedMax;
+    deltaSpeed = 0;
+    angle = 1;
+    break;
+  case 0b00001100:
+    speed = speedMax;
+    deltaSpeed = 0;
+    angle = 2;
+    break;
+  case 0b00000100:
+    speed = speedMax;
+    deltaSpeed = 3;
+    angle = 3;
+    break;
+  case 0b00000110:
+    speed = speedMax;
+    deltaSpeed = 4;
+    angle = 4;
+    break;
+  case 0b00000010:
+    speed = 0;
+    deltaSpeed = 5;
+    angle = 5;
+    break;
+  }
+  errorCount = 0;
+}
+
+/**************************************/
+
+void setup(){
+  //サーボ
+  steeringServo.attach(steeringServoPin, steeringServoMin, steeringServoMax);
+  lanceServo.attach(lanceServoPin, lanceServoMin, lanceServoMax);
+  steering(0);
+  lance(0);
+  //駆動用モーター
+  motorL.attach(motorLeftEnablePin,  motorLeftDirectionPin);
+  motorR.attach(motorRightEnablePin, motorRightDirectionPin);
+  motorL.mode(STOP);
+  motorR.mode(STOP);
+  //センサー
+  sensorInit();
+  //エンコーダー
+  pinMode(rotPin, INPUT);
+  attachInterrupt(0, rot, RISING);
+  //ブザー
+  startBeep();
+  /* 3秒停止 */
+  delay(3000);
+  /* スタート */
+  mode = MODE_STRAIGHT;
+  MsTimer2::set(cycle, run);
+  MsTimer2::start();
+}
+
+void loop(){
+  scanSensor();
+}
+
+/* 車体の動作を決定する */
+void run(){
+  if(distance > lapDistance*laps || errorCount > errorCountThreshold){
+    mode = MODE_STOP;
+  }
+  switch(mode){
+  default:
+  case MODE_STOP:      //停止
+    MsTimer2::stop();
+    lanceServo.detach();
+    steeringServo.detach();
+    motorL.mode(STOP);
+    motorR.mode(STOP);
+    while(1){
+      ;
+    }
+    break;
+  case MODE_STRAIGHT:  //まっすぐ進む
+    trace();
+    modeSet();
+    break;
+  case MODE_TURN:      //左に曲がる
+    trace();
+    lance(0);
+    if(distance - modeChangedDistance > turnDistance){
+      mode = MODE_STRAIGHT;
+    }
+    break;
+  case MODE_ATTACK_E:  //平行標的1
+    trace();
+    lance(targetE.lanceAngle.before);
+    if(distance - modeChangedDistance > targetE.way){ //マーカーから少し進んで、叩く
+      lance(targetE.lanceAngle.after);
+      mode = MODE_STRAIGHT;
+    }
+    break;
+  case MODE_ATTACK_F:  //平行標的2
+    trace();
+    lance(targetF.lanceAngle.before);
+    if(distance - modeChangedDistance > targetF.way){ //マーカーから少し進んで、叩く
+      lance(targetF.lanceAngle.after);
+      mode = MODE_STRAIGHT;
+    }
+    break;
+  case MODE_ATTACK_A:  //右垂直標
+    trace();
+    lance(targetA.lanceAngle.before);
+    if(distance - modeChangedDistance > targetA.way){ //マーカーから少し進んで、まっすぐに戻す
+      lance(targetA.lanceAngle.after);
+      mode = MODE_STRAIGHT;
+    }
+    break;
+  case MODE_ATTACK_C:  //右垂直標
+    trace();
+    lance(targetC.lanceAngle.before);
+    if(distance - modeChangedDistance > targetC.way){ //マーカーから少し進んで、まっすぐに戻す
+      lance(targetC.lanceAngle.after);
+      mode = MODE_STRAIGHT;
+    }
+    break;
+  case MODE_ATTACK_B:  //左垂直標
+    trace();
+    lance(targetB.lanceAngle.before);
+    if(distance - modeChangedDistance > targetB.way){ //マーカーから少し進んで、まっすぐに戻す
+      lance(targetB.lanceAngle.after);
+      mode = MODE_STRAIGHT;
+    }
+    break;
+  case MODE_ATTACK_D:  //左垂直標
+    trace();
+    lance(targetD.lanceAngle.before);
+    if(distance - modeChangedDistance > targetD.way){ //マーカーから少し進んで、まっすぐに戻す
+      lance(targetD.lanceAngle.after);
+      mode = MODE_STRAIGHT;
+    }
+    break;
+  case MODE_ATTACK_CYLINDER:    //円筒標的
+    mode = MODE_STRAIGHT;
+    break;
+  }
+  
+  /* 駆動モーター用 */
+  deltaSpeed = constrain(deltaSpeed, -deltaSpeedMax, deltaSpeedMax);
+  if(deltaSpeed<0){
+    motorL.speed(speed + deltaSpeed);
+    motorR.speed(speed);
+  }else{
+    motorL.speed(speed);
+    motorR.speed(speed - deltaSpeed);
+  }
+  
+  /* ステアリングサーボ用 */
+  angle = constrain(angle, steeringAngleMin, steeringAngleMax);
+  steering(angle);
+}
+
+/* モードを決定する */
+void modeSet(){
+  unsigned long way = distance%lapDistance;
+  switch(sensor&markerSensorMask){
+  default:
+  case 0b00000000:
+    mode = MODE_STRAIGHT;
+    break;
+  case 0b10000001:
+    modeChangedDistance = distance;
+    mode = MODE_TURN;
+    break;
+  case 0b00000001: //E,F,A,C
+    modeChangedDistance = distance;
+    if(targetE.range.min <= way && way < targetE.range.max) mode = MODE_ATTACK_E;
+    if(targetF.range.min <= way && way < targetF.range.max) mode = MODE_ATTACK_F;
+    if(targetA.range.min <= way && way < targetA.range.max) mode = MODE_ATTACK_A;
+    if(targetC.range.min <= way && way < targetC.range.max) mode = MODE_ATTACK_C;
+    break;
+  case 0b10000000: //B,D
+    modeChangedDistance = distance;
+    if(targetB.range.min <= way && way < targetB.range.max) mode = MODE_ATTACK_B;
+    if(targetD.range.min <= way && way < targetD.range.max) mode = MODE_ATTACK_D;
+    break;
+  }
+}

hanzo4_1Utils.ino

@@ -0,0 +1,63 @@
+/* ステアリングを中央からa[度]だけ動かす
+  <- - 0 + ->
+      ||
+      ||
+ []---{}---[]
+     / .\
+*/
+int steering(int a){
+  steeringServo.write(90 + constrain(a, steeringAngleMin, steeringAngleMax));
+  return (steeringServo.read() - 90);
+}
+
+/* ランスを中央からa[度]だけ動かす
+  <- - 0 + ->
+     / .\
+    /  | \
+ []+--{}--+[]
+*/
+int lance(int a){
+  lanceServo.write(90 - a);
+  return (90 - a);
+}
+
+/* センサーのアレイの初期化 */
+void sensorInit(){
+  for(int i=0; i<numSensors; i++){
+    pinMode(sensorPin[i], INPUT);
+  }
+}
+
+/* センサーアレイの状態を見る */
+unsigned char scanSensor(){
+  unsigned char b;
+  //黒が1、白が0
+  for(int i=0; i<numSensors; i++){
+    if(digitalRead(sensorPin[i])) b += 1<<i;  //右端から順番に左端へ
+  }
+  b ^= 0xff;  //今回ラインが白なので、反転して、ラインを1とする
+  return sensor = b;
+}
+
+/* エンコーダーの割り込み処理 */
+void rot(){
+  distance++;
+}
+
+/* 起動音 */
+void startBeep(){
+  pinMode(buzzerPin, OUTPUT);
+  tone(buzzerPin,2000);
+  delay(100);
+  tone(buzzerPin,1000);
+  delay(100);
+  noTone(buzzerPin);
+}
+
+/* 終了音 */
+void stopBeep(){
+  pinMode(buzzerPin, OUTPUT);
+  tone(buzzerPin, 2000);
+  delay(2000);
+  noTone(buzzerPin);
+}