Revert "半蔵4.1を作成した"

This reverts commit 6c6e9ac02b.

Motor.cpp

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

Motor.h

@@ -3,6 +3,7 @@ Motor.h - モータードライバ(L298P シールド)のためのライブラ
 
 ## 概要 ##
 このライブラリは、モータードライバ(L298P シールド)のためのシンプルなライブラリです。
+PWM対応ピンが1つしか必要ないのがこのライブラリの特徴です。
 
 ## 使い方 ##
 例:
@@ -10,7 +11,7 @@ Motor.h - モータードライバ(L298P シールド)のためのライブラ
     Motor motor;
     void setup(){
     //  motor.attach(pin1, pin2);
-      motor.attach(5, 4);
+      motor.attach(5, 6);
     }
     void loop(){
     //  motor.mode(GO); //正転
@@ -21,13 +22,29 @@ Motor.h - モータードライバ(L298P シールド)のためのライブラ
     }
 
 注意:
+使用するモータードライバの動作は、  
+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)2014 kou029w - MIT License
+(C)2013 kou029w - MIT License
 */
 
 #ifndef Motor_h

README.md

@@ -1,11 +1,10 @@
-
+# 競技用ランサーロボット 半蔵4.0 制御プログラム #
-# 競技用ランサーロボット 半蔵4.1 制御プログラム #
 
 ## これはなに ##
-[ロボットグランプリ](http://www.robotgrandprix.com/)に向けて製作中のマシン、半蔵4.1のための制御プログラムです。
+[ロボットグランプリ](http://www.robotgrandprix.com/)に向けて製作中のマシン、半蔵4.0のための制御プログラムです。
 
 ## システム構成 ##
-以下に、半蔵4.1のシステム構成を示します。
+以下に、半蔵4.0のシステム構成を示します。
 
 ### Hardware ###
     [@ @@@@@@ @] - Sensor array
@@ -29,7 +28,7 @@
     MPU                     - Atmel AVR
     Rotary encoder          - ロータリーエンコーダー
     Sensor array            - 反射型フォトインタラプタSG-2BC
-    Motor for driving       - 駆動用モーター/ダイセン
+    Motor for driving       - 駆動用モーター/ハイスピードギアボックスHE
     Servomotor for steering - ステアリング用サーボモーター
     Servomotor for lance    - ランス用サーボモーター
 
@@ -43,7 +42,7 @@
 * 使用サーボモーター : Savox SC-0352(ランス用)
 
 ### 回路仕様 ###
-* 動作電圧 : 11.1V(駆動用モーター), 7.4V(MPU, サーボ)
+* 動作電圧 : 7.2V以上
 * 使用マイコン : Atmel AVR (Arduino)
 * 使用センサーアレイ : SG-2BC x 8 (ロボラボ講習会のもの)
 
@@ -70,4 +69,4 @@
 * 使用言語 : Arduino 1.0
 
 ## ライセンス ##
-(C)2014 kou029w - MIT Licence
+(C)2013 kou029w - MIT Licence

hanzo4.ino

@@ -1,60 +1,60 @@
 /*
 競技用ランサーロボット
-半蔵 4.1
+半蔵 4.0
-(C)2014 kou029w - MIT License
+(C)2013 kou029w - MIT License
 */
 
-#include <MsTimer2.h>
 #include <Tone.h>
 #include <Servo.h>
 #include "Motor.h"
 
-unsigned long LAP_DISTANCE  = 28500; //ロータリーエンコーダーのカウント数/周
+const unsigned long LAP_DISTANCE  = 28500;  //ロータリーエンコーダーのカウント数/周
-unsigned long TURN_DISTANCE = 4650;  //カーブ中でのカウント数
+const unsigned long TURN_DISTANCE = 4650;  //カーブ中でのカウント数
 
-int LANCE_ANGLE_E =  40;  //度
+const int LANCE_ANGLE_E =  40;  //度
-int LANCE_ANGLE_F =  61;  //度
+const int LANCE_ANGLE_F =  61;  //度
-int LANCE_ANGLE_A =  29;  //度
+const int LANCE_ANGLE_A =  29;  //度
-int LANCE_ANGLE_B = -32;  //度
+const int LANCE_ANGLE_B = -32;  //度
-int LANCE_ANGLE_C =  34;  //度
+const int LANCE_ANGLE_C =  34;  //度
-int LANCE_ANGLE_D = -42;  //度
+const int LANCE_ANGLE_D = -42;  //度
 
-int SPEED_DEFAULT = 100;
+//const unsigned char SPEED_DEFAULT = 0;
+const unsigned char SPEED_DEFAULT = 127;
 
-float STEERING_KP = 3.8;
+//const 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
+const unsigned int STEERING_CENTER = 1810;  //us
-unsigned int STEERING_MIN = (STEERING_CENTER-780); //us
+const unsigned int STEERING_MIN = (STEERING_CENTER-780); //us
-unsigned int STEERING_MAX = (STEERING_CENTER+780); //us
+const unsigned int STEERING_MAX = (STEERING_CENTER+780); //us
 
-// Servo : SC-0352
+//Servo  : SC-0352
-unsigned int LANCE_CENTER = 1530;  //us
+const unsigned int LANCE_CENTER = 1530;  //us
-unsigned int LANCE_MIN = (LANCE_CENTER-780); //us
+const unsigned int LANCE_MIN = (LANCE_CENTER-780); //us
-unsigned int LANCE_MAX = (LANCE_CENTER+780); //us
+const unsigned int LANCE_MAX = (LANCE_CENTER+780); //us
 
-unsigned char PIN_BUZZER =  3;
+const unsigned char PIN_BUZZER =  3;
-unsigned char PIN_ROT    =  2;
+const unsigned char PIN_ROT    =  2;
 
-unsigned char NUM_SENSORS = 8;
+const unsigned char PIN_SENSOR_0 = 19;  //右端
-unsigned char PIN_SENSOR[] = {19,18,17,16,15,14,13,12};  //右端-左端
+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;  //左端
 
-unsigned char PIN_SERVO_STEERING  =  9;
+const unsigned char PIN_SERVO_STEERING  =  9;
-unsigned char PIN_SERVO_LANCE     = 10;
+const unsigned char PIN_SERVO_LANCE     = 10;
 
-unsigned char PIN_MOTOR_LEFT_E  =  5;
+const unsigned char PIN_MOTOR_LEFT_E  =  5;
-unsigned char PIN_MOTOR_LEFT_M  =  4;
+const unsigned char PIN_MOTOR_LEFT_M  =  4;
-unsigned char PIN_MOTOR_RIGHT_E =  6;
+const unsigned char PIN_MOTOR_RIGHT_E =  6;
-unsigned char PIN_MOTOR_RIGHT_M =  7;
+const unsigned char PIN_MOTOR_RIGHT_M =  7;
 
-unsigned char MASK_LINE    = 0b01111110;
+const unsigned char MASK_LINE    = 0b01111110;
-unsigned char MASK_MARKER  = 0b10000001;
+const unsigned char MASK_MARKER  = 0b10000001;
 
 /**************************************/
 
@@ -64,21 +64,13 @@ Motor motorR;
 Motor motorL;
 
 //ラインが1、地面が0、LSBが左端、MSBが右端
-volatile unsigned char sensor = 0x00;
+unsigned char sensor = 0x00;  
+int errorCount = 0;
 
-volatile int linePosition[] = {0,0}; //{新,旧}
+int speed = SPEED_DEFAULT;
-volatile long integral = 0;
-
-// 右のモーターと左のモーターの速度差 : (右のモーター)-(左のモーター)
-volatile int speed = 0;
-
-// ステアリングの角度
-volatile long steeringAngle = 0; // 度
-
-volatile int errorCount = 0;
 
 volatile unsigned long distance = 0;
-volatile unsigned long modeChangedDistance = 0;
+unsigned long modeChangedDistance = 0;
 
 /**************************************/
 
@@ -93,157 +85,77 @@ enum mode_t {
   MODE_ATTACK_C,  //右垂直標的
   MODE_ATTACK_D,  //左垂直標的
   MODE_ATTACK_CYLINDER     //円筒標的
-} mode;
+} 
+mode;
 
 /**************************************/
 
-/* センサーを読んでラインの位置を返す */
+/* センサーを読んで、車体の動作を決定する */
-int scanSensor(){
+void trace(){
-  sensorRead();
-  int pos = 0;
   switch(sensor&MASK_LINE){
   case 0b01000000:
-    pos = -5;
+    steering(-21);
+    motorL.speed(-speed/2);
+    motorR.speed(speed/2);
     break;
   case 0b01100000:
-    pos = -4;
+    steering(-20);
+    motorL.speed(-speed/2);
+    motorR.speed(speed/2);
     break;
   case 0b00100000:
-    pos = -3;
+    steering(-19);
+    motorL.speed(-speed/2);
+    motorR.speed(speed/2);
     break;
   case 0b00110000:
-    pos = -2;
+    steering(-2);
+    motorL.speed(speed);
+    motorR.speed(speed);
     break;
   case 0b00010000:
-    pos = -1;
+    steering(-1);
+    motorL.speed(speed);
+    motorR.speed(speed);
     break;
   case 0b00011000:
-    pos = 0;
+    steering(0);
+    motorL.speed(speed);
+    motorR.speed(speed);
     break;
   case 0b00001000:
-    pos = 1;
+    steering(1);
+    motorL.speed(speed);
+    motorR.speed(speed);
     break;
   case 0b00001100:
-    pos = 2;
+    steering(2);
+    motorL.speed(speed);
+    motorR.speed(speed);
     break;
   case 0b00000100:
-    pos = 3;
+    steering(3);
+    motorL.speed(0);
+    motorR.speed(0);
     break;
   case 0b00000110:
-    pos = 4;
+    steering(4);
+    motorL.speed(0);
+    motorR.speed(0);
     break;
   case 0b00000010:
-    pos = 5;
+    steering(5);
-    break;
+    motorL.speed(0);
-  default:
+    motorR.speed(0);
-    errorCount++;
-    return 127;
-  }
-  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);
-  }
-  
-  steeringAngle = steeringPDControl();
-  steeringAngle = constrain(steeringAngle, -25, 25);
-  steering(steeringAngle);
-  
-  switch(mode){
-  default:
-  case MODE_STOP:      //停止
-    MsTimer2::stop();
-    steering(-20);
-    motorL.mode(STOP);
-    motorR.mode(STOP);
-    stopBeep();
-    while(1){
-      ;
-    };
-    break;
-  case MODE_STRAIGHT:  //まっすぐ進む
-    modeSet();
-    break;
-  case MODE_TURN:      //左に曲がる
-    lance(0);
-    if(distance - modeChangedDistance > TURN_DISTANCE){
-      mode = MODE_STRAIGHT;
-    }
-    break;
-  case MODE_ATTACK_E:  //平行標的1
-    lance(LANCE_ANGLE_E);
-    if(distance - modeChangedDistance > 800){ //マーカーから少し進んで、叩く
-      lance(LANCE_ANGLE_E + 8);
-      mode = MODE_STRAIGHT;
-    }
-    break;
-  case MODE_ATTACK_F:  //平行標的2
-    lance(LANCE_ANGLE_F);
-    if(distance - modeChangedDistance > 1000){ //マーカーから少し進んで、叩く
-      lance(LANCE_ANGLE_F + 8);
-      mode = MODE_STRAIGHT;
-    }
-    break;
-  case MODE_ATTACK_A:  //右垂直標
-    lance(LANCE_ANGLE_A);
-    if(distance - modeChangedDistance > 1500){ //マーカーから少し進んで、まっすぐに戻す
-      lance(0);
-      mode = MODE_STRAIGHT;
-    }
-    break;
-  case MODE_ATTACK_C:  //右垂直標
-    lance(LANCE_ANGLE_C);
-    if(distance - modeChangedDistance > 1500){ //マーカーから少し進んで、まっすぐに戻す
-      lance(0);
-      mode = MODE_STRAIGHT;
-    }
-    break;
-  case MODE_ATTACK_B:  //左垂直標
-    lance(LANCE_ANGLE_B);
-    if(distance - modeChangedDistance > 1500){ //マーカーから少し進んで、まっすぐに戻す
-      lance(0);
-      mode = MODE_STRAIGHT;
-    }
-    break;
-  case MODE_ATTACK_D:  //左垂直標
-    lance(LANCE_ANGLE_D);
-    if(distance - modeChangedDistance > 1500){ //マーカーから少し進んで、まっすぐに戻す
-      lance(0);
-      mode = MODE_STRAIGHT;
-    }
-    break;
-  case MODE_ATTACK_CYLINDER:    //円筒標的
-    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]);
-}
-
 /* モードを決定する */
 void modeSet(){
-  if(distance > LAP_DISTANCE || errorCount > 5000){
+  if(sensor == 0) errorCount++;
+  else errorCount = 0;
+  if(distance > LAP_DISTANCE*13 || errorCount > 5000){
     mode = MODE_STOP;
     return;
   }
@@ -284,7 +196,7 @@ void modeSet(){
 }
 
 void setup(){
-  Serial.begin(9600);
+//  Serial.begin(9600);
   //サーボ
   steeringServo.attach(PIN_SERVO_STEERING, STEERING_MIN, STEERING_MAX);
   lanceServo.attach(PIN_SERVO_LANCE, LANCE_MIN, LANCE_MAX);
@@ -302,18 +214,91 @@ void setup(){
   attachInterrupt(0, rot, RISING);
   //ブザー
   pinMode(PIN_BUZZER, OUTPUT);
-//  startBeep();
+  startBeep();
   /* 3秒停止 */
   motorL.mode(STOP);
   motorR.mode(STOP);
   delay(3000);
   /* スタート */
   mode = MODE_STRAIGHT;
-  MsTimer2::set(1, trace);
-  MsTimer2::start();
 }
 
 void loop(){
-  scanSensor();
+//  Serial.println(distance);
-  Serial.println(linePosition[0]);
+  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

@@ -21,18 +21,28 @@ void lance(int a){
 
 /* センサーのアレイの初期化 */
 void sensorInit(){
-  for(int i=0; i<NUM_SENSORS; i++){
+  pinMode(PIN_SENSOR_0, INPUT);  //右端
-    pinMode(PIN_SENSOR[i], 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
-  for(int i=0; i<NUM_SENSORS; i++){
+  if(digitalRead(PIN_SENSOR_0)) b += 0b00000001;  //右端
-    if(digitalRead(PIN_SENSOR[i])) b += 1<<i;  //右端-左端
+  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;
 }