コメントの間違いを修正

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

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