細かいパラメータの調整

hanzo4_1.ino

@@ -34,41 +34,36 @@ struct target_t{
 unsigned long lapDistance  = 28500; //ロータリーエンコーダーのカウント数/周
 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 = {{1500,             2400   }, 800, { 40, 48}};
-struct target_t targetF = {{lapDistance/8,    lapDistance/4   }, 1000, { 61, 69}};
+struct target_t targetF = {{lapDistance/8,    lapDistance/4   }, 800, { 61, 60}};
 
-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 = 1815;  //us
 unsigned int steeringServoMin = (steeringServoCenter-780); //us
 unsigned int steeringServoMax = (steeringServoCenter+780); //us
 
 // Servo : SC-0352
-unsigned int lanceServoCenter = 1530;  //us
+unsigned int lanceServoCenter = 1550;  //us
 unsigned int lanceServoMin = (lanceServoCenter-780); //us
 unsigned int lanceServoMax = (lanceServoCenter+780); //us
 
@@ -100,12 +95,9 @@ Motor motorL;
 //ラインが1、地面が0、LSBが左端、MSBが右端
 unsigned char sensor = 0x00;
 
-int linePosition = 0;
-int lineDifferential = 0;
-int lineDifferential16 = 0;
-long lineIntegral = 0;
-
 // 右のモーターと左のモーターの速度差 : (右のモーター)-(左のモーター)
+int deltaSpeed = 0;
+// 駆動用モーターの速度
 int speed = 0;
 
 // ステアリングの角度
@@ -117,8 +109,8 @@ volatile unsigned long distance = 0;
 unsigned long modeChangedDistance = 0;
 
 /**************************************/
-/* 動作モード */
 
+/* 動作モード */
 enum mode_t {
   MODE_STOP,      //停止
   MODE_STRAIGHT,  //まっすぐ進む
@@ -134,6 +126,73 @@ enum mode_t {
 
 /**************************************/
 
+/* センサーを読んで、車体の動作を決定する */
+void trace(){
+  switch(sensor&lineSensorMask){
+  default:
+    errorCount++;
+    return;
+  case 0b01000000:
+    speed = speedMax*0.3;
+    deltaSpeed = -150;
+    angle = -18;
+    break;
+  case 0b01100000:
+    speed = speedMax*0.5;
+    deltaSpeed = -100;
+    angle = -16;
+    break;
+  case 0b00100000:
+    speed = speedMax*0.6;
+    deltaSpeed = -50;
+    angle = -14;
+    break;
+  case 0b00110000:
+    speed = speedMax;
+    deltaSpeed = -25;
+    angle = -10;
+    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);
@@ -151,24 +210,26 @@ 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(2, run);  //500Hz
+  MsTimer2::set(cycle, run);
   MsTimer2::start();
 }
 
 void loop(){
-  sensorRead();
+  scanSensor();
 }
 
-/* 車体の動作を決定する(500Hz) */
+/* 車体の動作を決定する */
 void run(){
+  if(distance > lapDistance*laps || errorCount > errorCountThreshold){
+    mode = MODE_STOP;
+  }
   switch(mode){
   default:
   case MODE_STOP:      //停止
@@ -176,7 +237,6 @@ void run(){
     steeringServo.detach();
     motorL.mode(STOP);
     motorR.mode(STOP);
-    if(!silent) stopBeep();
     while(1){
       ;
     };
@@ -244,33 +304,24 @@ void run(){
     mode = MODE_STRAIGHT;
     break;
   }
-}
   
-/* センサーを読んで、ライントレースする */
+  /* 駆動モーター用 */
-void trace(){
+  deltaSpeed = constrain(deltaSpeed, -deltaSpeedMax, deltaSpeedMax);
-  scanSensor();
+  if(deltaSpeed<0){
-    
+    motorL.speed(speed + deltaSpeed);
-  speed = speedPIDControl();
+    motorR.speed(speed);
-  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,74 +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;
-  
-  static int forDiv16;
-  if(forDiv16<16){
-    lineDifferential += pos - linePosition;
-    forDiv16++;
-  }else{
-    lineDifferential16 = lineDifferential;
-    lineDifferential = 0;
-    forDiv16 = 0;
-  }
-  linePosition = pos;
-  lineIntegral += pos;
-  return pos;
-}
-
-// 右のモーターと左のモーターの速度差 : (右のモーター)-(左のモーター)
-int speedPIDControl(){
-  return speedKP*linePosition + speedKI*lineIntegral + speedKD*lineDifferential16/16;
-}
-
-// ステアリングの角度(度)
-int steeringPIDControl(){
-  return steeringKP*linePosition + steeringKI*lineIntegral + steeringKD*lineDifferential16/16;
-}
-
 /* エンコーダーの割り込み処理 */
 void rot(){
   distance++;
@@ -112,6 +46,7 @@ void rot(){
 
 /* 起動音 */
 void startBeep(){
+  pinMode(buzzerPin, OUTPUT);
   tone(buzzerPin,2000);
   delay(100);
   tone(buzzerPin,1000);
@@ -121,6 +56,7 @@ void startBeep(){
 
 /* 終了音 */
 void stopBeep(){
+  pinMode(buzzerPin, OUTPUT);
   tone(buzzerPin, 2000);
   delay(2000);
   noTone(buzzerPin);