コメントの間違いを修正

README.md

@@ -37,9 +37,9 @@
 * ホイールベース : 約150mm
 * 車重 : 1kg以下
 * 車体材料 : タミヤ ユニバーサルプレートL
-* 使用ギアボックス : タミヤ ハイスピードギアボックスHE
-* ギア比 : 11.6:1
-* 使用モーター : 上記ギアボックスに付属するもの
+* 使用ギアボックス : 
+* ギア比 : 
+* 使用モーター : 
 * 使用サーボモーター : Savox SC-0352(ランス用)
 
 ### 回路仕様 ###
@@ -47,25 +47,6 @@
 * 使用マイコン : 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
 

hanzo4_1.ino

@@ -9,79 +9,108 @@
 #include <Servo.h>
 #include "Motor.h"
 
-unsigned long LAP_DISTANCE  = 28500; //ロータリーエンコーダーのカウント数/周
-unsigned long TURN_DISTANCE = 4650;  //カーブ中でのカウント数
+/* 標的の直前にあるマーカーを見つけ出す範囲 : {最小, 最大} */
+struct range_t{
+  unsigned long min;
+  unsigned long max;
+};
 
-int LANCE_ANGLE_E =  40;  //度
-int LANCE_ANGLE_F =  61;  //度
-int LANCE_ANGLE_A =  29;  //度
-int LANCE_ANGLE_B = -32;  //度
-int LANCE_ANGLE_C =  34;  //度
-int LANCE_ANGLE_D = -42;  //度
+/* 標的のためのランスの角度 : {before[度], after[度]} */
+struct lanceAngle_t{
+  int before;
+  int after;
+};
 
-int SPEED_DEFAULT = 100;
-
-float STEERING_KP = 3.8;
-float STEERING_KI = 0.0;
-float STEERING_KD = 0.0;
-
-float SPEED_KP = 50.0;
-float SPEED_KI = 0.0;
-float SPEED_KD = 0.0;
-
-// Servo : SC-1267SG
-unsigned int STEERING_CENTER = 1810;  //us
-unsigned int STEERING_MIN = (STEERING_CENTER-780); //us
-unsigned int STEERING_MAX = (STEERING_CENTER+780); //us
-
-// Servo : SC-0352
-unsigned int LANCE_CENTER = 1530;  //us
-unsigned int LANCE_MIN = (LANCE_CENTER-780); //us
-unsigned int LANCE_MAX = (LANCE_CENTER+780); //us
-
-unsigned char PIN_BUZZER =  3;
-unsigned char PIN_ROT    =  2;
-
-unsigned char NUM_SENSORS = 8;
-unsigned char PIN_SENSOR[] = {19,18,17,16,15,14,13,12};  //右端-左端
-
-unsigned char PIN_SERVO_STEERING  =  9;
-unsigned char PIN_SERVO_LANCE     = 10;
-
-unsigned char PIN_MOTOR_LEFT_E  =  5;
-unsigned char PIN_MOTOR_LEFT_M  =  4;
-unsigned char PIN_MOTOR_RIGHT_E =  6;
-unsigned char PIN_MOTOR_RIGHT_M =  7;
-
-unsigned char MASK_LINE    = 0b01111110;
-unsigned char MASK_MARKER  = 0b10000001;
+/* 標的の情報 */
+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、LSBが左端、MSBが右端
-volatile unsigned char sensor = 0x00;
-
-volatile int linePosition[] = {0,0}; //{新,旧}
-volatile long integral = 0;
+//ラインが1、地面が0、MSBが左端、LSBが右端
+unsigned char sensor = 0x00;
 
 // 右のモーターと左のモーターの速度差 : (右のモーター)-(左のモーター)
-volatile int speed = 0;
+int deltaSpeed = 0;
+// 駆動用モーターの速度
+int speed = 0;
 
 // ステアリングの角度
-volatile long steeringAngle = 0; // 度
+long angle = 0; // 度
 
-volatile int errorCount = 0;
+int errorCount = 0;
 
 volatile unsigned long distance = 0;
-volatile unsigned long modeChangedDistance = 0;
+unsigned long modeChangedDistance = 0;
 
 /**************************************/
 
+/* 動作モード */
 enum mode_t {
   MODE_STOP,      //停止
   MODE_STRAIGHT,  //まっすぐ進む
@@ -97,131 +126,176 @@ enum mode_t {
 
 /**************************************/
 
-/* センサーを読んでラインの位置を返す */
-int scanSensor(){
-  sensorRead();
-  int pos = 0;
-  switch(sensor&MASK_LINE){
-  case 0b01000000:
-    pos = -5;
-    break;
-  case 0b01100000:
-    pos = -4;
-    break;
-  case 0b00100000:
-    pos = -3;
-    break;
-  case 0b00110000:
-    pos = -2;
-    break;
-  case 0b00010000:
-    pos = -1;
-    break;
-  case 0b00011000:
-    pos = 0;
-    break;
-  case 0b00001000:
-    pos = 1;
-    break;
-  case 0b00001100:
-    pos = 2;
-    break;
-  case 0b00000100:
-    pos = 3;
-    break;
-  case 0b00000110:
-    pos = 4;
-    break;
-  case 0b00000010:
-    pos = 5;
-    break;
+/* センサーを読んで、車体の動作を決定する */
+void trace(){
+  switch(sensor&lineSensorMask){
   default:
     errorCount++;
-    return 127;
+    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;
-  linePosition[1] = linePosition[0];
-  linePosition[0] = pos;
-  integral += (linePosition[0] + linePosition[1])>>1;
-  return pos;
 }
 
-/* センサーを読んで、車体の動作を決定する(1kHz) */
-void trace(){
-  speed = speedPDControl();
-  speed = constrain(speed, -SPEED_DEFAULT, SPEED_DEFAULT);
-  if(speed<0){
-    motorL.speed(SPEED_DEFAULT + speed);
-    motorR.speed(SPEED_DEFAULT);
-  }else{
-    motorL.speed(SPEED_DEFAULT);
-    motorR.speed(SPEED_DEFAULT - speed);
+/**************************************/
+
+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;
   }
-  
-  steeringAngle = steeringPDControl();
-  steeringAngle = constrain(steeringAngle, -25, 25);
-  steering(steeringAngle);
-  
   switch(mode){
   default:
   case MODE_STOP:      //停止
     MsTimer2::stop();
-    steering(-20);
+    lanceServo.detach();
+    steeringServo.detach();
     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){
+    if(distance - modeChangedDistance > turnDistance){
       mode = MODE_STRAIGHT;
     }
     break;
   case MODE_ATTACK_E:  //平行標的1
-    lance(LANCE_ANGLE_E);
-    if(distance - modeChangedDistance > 800){ //マーカーから少し進んで、叩く
-      lance(LANCE_ANGLE_E + 8);
+    trace();
+    lance(targetE.lanceAngle.before);
+    if(distance - modeChangedDistance > targetE.way){ //マーカーから少し進んで、叩く
+      lance(targetE.lanceAngle.after);
       mode = MODE_STRAIGHT;
     }
     break;
   case MODE_ATTACK_F:  //平行標的2
-    lance(LANCE_ANGLE_F);
-    if(distance - modeChangedDistance > 1000){ //マーカーから少し進んで、叩く
-      lance(LANCE_ANGLE_F + 8);
+    trace();
+    lance(targetF.lanceAngle.before);
+    if(distance - modeChangedDistance > targetF.way){ //マーカーから少し進んで、叩く
+      lance(targetF.lanceAngle.after);
       mode = MODE_STRAIGHT;
     }
     break;
   case MODE_ATTACK_A:  //右垂直標
-    lance(LANCE_ANGLE_A);
-    if(distance - modeChangedDistance > 1500){ //マーカーから少し進んで、まっすぐに戻す
-      lance(0);
+    trace();
+    lance(targetA.lanceAngle.before);
+    if(distance - modeChangedDistance > targetA.way){ //マーカーから少し進んで、まっすぐに戻す
+      lance(targetA.lanceAngle.after);
       mode = MODE_STRAIGHT;
     }
     break;
   case MODE_ATTACK_C:  //右垂直標
-    lance(LANCE_ANGLE_C);
-    if(distance - modeChangedDistance > 1500){ //マーカーから少し進んで、まっすぐに戻す
-      lance(0);
+    trace();
+    lance(targetC.lanceAngle.before);
+    if(distance - modeChangedDistance > targetC.way){ //マーカーから少し進んで、まっすぐに戻す
+      lance(targetC.lanceAngle.after);
       mode = MODE_STRAIGHT;
     }
     break;
   case MODE_ATTACK_B:  //左垂直標
-    lance(LANCE_ANGLE_B);
-    if(distance - modeChangedDistance > 1500){ //マーカーから少し進んで、まっすぐに戻す
-      lance(0);
+    trace();
+    lance(targetB.lanceAngle.before);
+    if(distance - modeChangedDistance > targetB.way){ //マーカーから少し進んで、まっすぐに戻す
+      lance(targetB.lanceAngle.after);
       mode = MODE_STRAIGHT;
     }
     break;
   case MODE_ATTACK_D:  //左垂直標
-    lance(LANCE_ANGLE_D);
-    if(distance - modeChangedDistance > 1500){ //マーカーから少し進んで、まっすぐに戻す
-      lance(0);
+    trace();
+    lance(targetD.lanceAngle.before);
+    if(distance - modeChangedDistance > targetD.way){ //マーカーから少し進んで、まっすぐに戻す
+      lance(targetD.lanceAngle.after);
       mode = MODE_STRAIGHT;
     }
     break;
@@ -229,25 +303,26 @@ void trace(){
     mode = MODE_STRAIGHT;
     break;
   }
-}
-
-// 右のモーターと左のモーターの速度差 : (右のモーター)-(左のモーター)
-int speedPDControl(){
-  return SPEED_KP*linePosition[0] + SPEED_KI*integral + SPEED_KD*(linePosition[0] - linePosition[1]);
-}
-
-// ステアリングの角度(度)
-int steeringPDControl(){
-  return STEERING_KP*linePosition[0] + STEERING_KI*integral + STEERING_KD*(linePosition[0] - linePosition[1]);
+  
+  /* 駆動モーター用 */
+  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(){
-  if(distance > LAP_DISTANCE || errorCount > 5000){
-    mode = MODE_STOP;
-    return;
-  }
-  switch(sensor&MASK_MARKER){
+  unsigned long way = distance%lapDistance;
+  switch(sensor&markerSensorMask){
   default:
   case 0b00000000:
     mode = MODE_STRAIGHT;
@@ -256,64 +331,17 @@ void modeSet(){
     modeChangedDistance = distance;
     mode = MODE_TURN;
     break;
-  case 0b00000001:
+  case 0b00000001: //E,F,A,C
     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;
-      }
-    }
+    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:
+  case 0b10000000: //B,D
     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;
-    }
+    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;
   }
 }
-
-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;
-  MsTimer2::set(1, trace);
-  MsTimer2::start();
-}
-
-void loop(){
-  scanSensor();
-  Serial.println(linePosition[0]);
-}

hanzo4_1Utils.ino

@@ -5,8 +5,9 @@
  []---{}---[]
      / .\
 */
-void steering(int a){
-  steeringServo.write(90 + a);
+int steering(int a){
+  steeringServo.write(90 + constrain(a, steeringAngleMin, steeringAngleMax));
+  return (steeringServo.read() - 90);
 }
 
 /* ランスを中央からa[度]だけ動かす
@@ -15,23 +16,24 @@ void steering(int a){
     /  | \
  []+--{}--+[]
 */
-void lance(int a){
+int lance(int a){
   lanceServo.write(90 - a);
+  return (90 - a);
 }
 
 /* センサーのアレイの初期化 */
 void sensorInit(){
-  for(int i=0; i<NUM_SENSORS; i++){
-    pinMode(PIN_SENSOR[i], INPUT);
+  for(int i=0; i<numSensors; i++){
+    pinMode(sensorPin[i], INPUT);
   }
 }
 
-/* センサーアレイの状態を見る*/
-unsigned char sensorRead(){
+/* センサーアレイの状態を見る */
+unsigned char scanSensor(){
   unsigned char b;
   //黒が1、白が0
-  for(int i=0; i<NUM_SENSORS; i++){
-    if(digitalRead(PIN_SENSOR[i])) b += 1<<i;  //右端-左端
+  for(int i=0; i<numSensors; i++){
+    if(digitalRead(sensorPin[i])) b += 1<<i;  //右端から順番に左端へ
   }
   b ^= 0xff;  //今回ラインが白なので、反転して、ラインを1とする
   return sensor = b;
@@ -44,16 +46,18 @@ void rot(){
 
 /* 起動音 */
 void startBeep(){
-  tone(PIN_BUZZER,2000);
+  pinMode(buzzerPin, OUTPUT);
+  tone(buzzerPin,2000);
   delay(100);
-  tone(PIN_BUZZER,1000);
+  tone(buzzerPin,1000);
   delay(100);
-  noTone(PIN_BUZZER);
+  noTone(buzzerPin);
 }
 
 /* 終了音 */
 void stopBeep(){
-  tone(PIN_BUZZER, 2000);
+  pinMode(buzzerPin, OUTPUT);
+  tone(buzzerPin, 2000);
   delay(2000);
-  noTone(PIN_BUZZER);
+  noTone(buzzerPin);
 }