README.md

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

@@ -31,39 +31,44 @@ struct target_t{
 /**************************************/
 /* 動作パラメーター */
 
-unsigned long lapDistance  = 28550; //ロータリーエンコーダーのカウント数/周
+unsigned long lapDistance  = 28500; //ロータリーエンコーダーのカウント数/周
 unsigned long turnDistance = 4650;  //カーブ中でのカウント数
 
-struct target_t targetA = {{lapDistance/2,    lapDistance*0.6 }, 900, { 29,  0}};
+struct target_t targetA = {{lapDistance/2,    lapDistance*0.6 }, 1500, { 29,  0}};
-struct target_t targetB = {{lapDistance/2,    lapDistance*0.75}, 900, {-32,  0}};
+struct target_t targetB = {{lapDistance/2,    lapDistance*0.75}, 1500, {-32,  0}};
-struct target_t targetC = {{lapDistance*0.6,  lapDistance*0.85}, 900, { 34,  0}};
+struct target_t targetC = {{lapDistance*0.6,  lapDistance*0.85}, 1500, { 34,  0}};
-struct target_t targetD = {{lapDistance*0.75, lapDistance*0.85}, 900, {-42,  0}};
+struct target_t targetD = {{lapDistance*0.75, lapDistance*0.85}, 1500, {-42,  0}};
-struct target_t targetE = {{ 500,4000}, 750, { 48, 53}};
+struct target_t targetE = {{0,                lapDistance/8   },  800, { 40, 48}};
-struct target_t targetF = {{4000,7000}, 750, { 60, 67}};
+struct target_t targetF = {{lapDistance/8,    lapDistance/4   }, 1000, { 61, 69}};
 
-int laps = 3;  //最大周回数
+int laps = 1;  //最大周回数
-bool silent = false;  //サイレントモード(true:ブザーを鳴らさない)
+bool silent = true;  //サイレントモード(true:ブザーを鳴らさない)
 
 int steeringAngleMin = -30; //度
-int steeringAngleMax =  10; //度
+int steeringAngleMax =  30; //度
 
-int deltaSpeedMax = 500;  //最大速度差
+int speedMax = 100;  //最大速度
-int speedMax = 255;  //最大速度
 
-int cycle = 1;  //動作周波数(ms)
+float steeringKP = 3.8;
+float steeringKI = 0.0;
+float steeringKD = 0.0;
 
-int errorCountThreshold = 500;  //最大読み取りエラー回数(これを超えると停止する)
+float speedKP = 50.0;
+float speedKI = 0.0;
+float speedKD = 0.0;
+
+int errorCountThreshold = 5000;  //最大読み取りエラー回数(これを超えると停止する)
 
 /**************************************/
 /* ピンの設定 */
 
 // Servo : SC-1267SG
-unsigned int steeringServoCenter = 1808;  //us
+unsigned int steeringServoCenter = 1810;  //us
 unsigned int steeringServoMin = (steeringServoCenter-780); //us
 unsigned int steeringServoMax = (steeringServoCenter+780); //us
 
 // Servo : SC-0352
-unsigned int lanceServoCenter = 1560;  //us
+unsigned int lanceServoCenter = 1530;  //us
 unsigned int lanceServoMin = (lanceServoCenter-780); //us
 unsigned int lanceServoMax = (lanceServoCenter+780); //us
 
@@ -92,12 +97,14 @@ Servo lanceServo;
 Motor motorR;
 Motor motorL;
 
-//ラインが1、地面が0、MSBが左端、LSBが右端
+//ラインが1、地面が0、LSBが左端、MSBが右端
 unsigned char sensor = 0x00;
 
+int linePosition = 0;
+int lineDifferential = 0;
+long lineIntegral = 0;
+
 // 右のモーターと左のモーターの速度差 : (右のモーター)-(左のモーター)
-int deltaSpeed = 0;
-// 駆動用モーターの速度
 int speed = 0;
 
 // ステアリングの角度
@@ -109,8 +116,8 @@ volatile unsigned long distance = 0;
 unsigned long modeChangedDistance = 0;
 
 /**************************************/
-
 /* 動作モード */
+
 enum mode_t {
   MODE_STOP,      //停止
   MODE_STRAIGHT,  //まっすぐ進む
@@ -126,73 +133,6 @@ enum mode_t {
 
 /**************************************/
 
-/* センサーを読んで、車体の動作を決定する */
-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);
@@ -210,35 +150,35 @@ void setup(){
   pinMode(rotPin, INPUT);
   attachInterrupt(0, rot, RISING);
   //ブザー
-  startBeep();
+  pinMode(buzzerPin, OUTPUT);
+  if(!silent) startBeep();
   /* 3秒停止 */
+  motorL.mode(STOP);
+  motorR.mode(STOP);
   delay(3000);
   /* スタート */
   mode = MODE_STRAIGHT;
-  MsTimer2::set(cycle, run);
+  MsTimer2::set(1, run);
   MsTimer2::start();
 }
 
 void loop(){
-  scanSensor();
+  sensorRead();
 }
 
-/* 車体の動作を決定する */
+/* 車体の動作を決定する(1kHz) */
 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);
+    if(!silent) stopBeep();
     while(1){
       ;
-    }
+    };
     break;
   case MODE_STRAIGHT:  //まっすぐ進む
     trace();
@@ -303,24 +243,33 @@ void run(){
     mode = MODE_STRAIGHT;
     break;
   }
+}
 
-  /* 駆動モーター用 */
+/* センサーを読んで、ライントレースする */
-  deltaSpeed = constrain(deltaSpeed, -deltaSpeedMax, deltaSpeedMax);
+void trace(){
-  if(deltaSpeed<0){
+  scanSensor();
-    motorL.speed(speed + deltaSpeed);
+    
-    motorR.speed(speed);
+  speed = speedPIDControl();
+  speed = constrain(speed, -speedMax, speedMax);
+  if(speed<0){
+    motorL.speed(speedMax + speed);
+    motorR.speed(speedMax);
   }else{
-    motorL.speed(speed);
+    motorL.speed(speedMax);
-    motorR.speed(speed - deltaSpeed);
+    motorR.speed(speedMax - speed);
   }
   
-  /* ステアリングサーボ用 */
+  angle = steeringPIDControl();
   angle = constrain(angle, steeringAngleMin, steeringAngleMax);
   steering(angle);
 }
 
 /* モードを決定する */
 void modeSet(){
+  if(distance > lapDistance*laps || errorCount > errorCountThreshold){
+    mode = MODE_STOP;
+    return;
+  }
   unsigned long way = distance%lapDistance;
   switch(sensor&markerSensorMask){
   default:

hanzo4_1Utils.ino

@@ -5,9 +5,8 @@
  []---{}---[]
      / .\
 */
-int steering(int a){
+void steering(int a){
-  steeringServo.write(90 + constrain(a, steeringAngleMin, steeringAngleMax));
+  steeringServo.write(90 + a);
-  return (steeringServo.read() - 90);
 }
 
 /* ランスを中央からa[度]だけ動かす
@@ -16,9 +15,8 @@ int steering(int a){
     /  | \
  []+--{}--+[]
 */
-int lance(int a){
+void lance(int a){
   lanceServo.write(90 - a);
-  return (90 - a);
 }
 
 /* センサーのアレイの初期化 */
@@ -29,7 +27,7 @@ void sensorInit(){
 }
 
 /* センサーアレイの状態を見る */
-unsigned char scanSensor(){
+unsigned char sensorRead(){
   unsigned char b;
   //黒が1、白が0
   for(int i=0; i<numSensors; i++){
@@ -39,6 +37,64 @@ unsigned char scanSensor(){
   return sensor = b;
 }
 
+/* センサーを読んでラインの位置を返す */
+int scanSensor(){
+  int pos = 0;
+  switch(sensor&lineSensorMask){
+  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;
+  default:
+    errorCount++;
+    return 127;
+  }
+  errorCount = 0;
+  lineDifferential = pos - linePosition;
+  linePosition = pos;
+  lineIntegral += pos;
+  return pos;
+}
+
+// 右のモーターと左のモーターの速度差 : (右のモーター)-(左のモーター)
+int speedPIDControl(){
+  return speedKP*linePosition + speedKI*lineIntegral + speedKD*lineDifferential;
+}
+
+// ステアリングの角度(度)
+int steeringPIDControl(){
+  return steeringKP*linePosition + steeringKI*lineIntegral + steeringKD*lineDifferential;
+}
+
 /* エンコーダーの割り込み処理 */
 void rot(){
   distance++;
@@ -46,7 +102,6 @@ void rot(){
 
 /* 起動音 */
 void startBeep(){
-  pinMode(buzzerPin, OUTPUT);
   tone(buzzerPin,2000);
   delay(100);
   tone(buzzerPin,1000);
@@ -56,7 +111,6 @@ void startBeep(){
 
 /* 終了音 */
 void stopBeep(){
-  pinMode(buzzerPin, OUTPUT);
   tone(buzzerPin, 2000);
   delay(2000);
   noTone(buzzerPin);