diff --git a/Motor.cpp b/Motor.cpp index e3b967a..873239a 100644 --- a/Motor.cpp +++ b/Motor.cpp @@ -1,6 +1,6 @@ /* Motor.cpp -(C)2014 kou029w - MIT License +(C)2013 kou029w - MIT License */ #include "Motor.h" diff --git a/Motor.h b/Motor.h index 4d5daca..9c49f4a 100644 --- a/Motor.h +++ b/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 diff --git a/README.md b/README.md index 3a6a196..3103263 100644 --- a/README.md +++ b/README.md @@ -1,11 +1,10 @@ - -# 競技用ランサーロボット 半蔵4.1 制御プログラム # +# 競技用ランサーロボット 半蔵4.0 制御プログラム # ## これはなに ## -[ロボットグランプリ](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 \ No newline at end of file +(C)2013 kou029w - MIT Licence \ No newline at end of file diff --git a/hanzo4_1.ino b/hanzo4.ino similarity index 59% rename from hanzo4_1.ino rename to hanzo4.ino index 7159480..7d87e45 100644 --- a/hanzo4_1.ino +++ b/hanzo4.ino @@ -1,60 +1,60 @@ /* 競技用ランサーロボット -半蔵 4.1 -(C)2014 kou029w - MIT License +半蔵 4.0 +(C)2013 kou029w - MIT License */ -#include #include #include #include "Motor.h" -unsigned long LAP_DISTANCE = 28500; //ロータリーエンコーダーのカウント数/周 -unsigned long TURN_DISTANCE = 4650; //カーブ中でのカウント数 +const unsigned long LAP_DISTANCE = 28500; //ロータリーエンコーダーのカウント数/周 +const unsigned long TURN_DISTANCE = 4650; //カーブ中でのカウント数 -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; //度 +const int LANCE_ANGLE_E = 40; //度 +const int LANCE_ANGLE_F = 61; //度 +const int LANCE_ANGLE_A = 29; //度 +const int LANCE_ANGLE_B = -32; //度 +const int LANCE_ANGLE_C = 34; //度 +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; -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; +//const float STEERING_KP = 3.8; // 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 +const unsigned int STEERING_CENTER = 1810; //us +const unsigned int STEERING_MIN = (STEERING_CENTER-780); //us +const 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 +//Servo : SC-0352 +const unsigned int LANCE_CENTER = 1530; //us +const unsigned int LANCE_MIN = (LANCE_CENTER-780); //us +const unsigned int LANCE_MAX = (LANCE_CENTER+780); //us -unsigned char PIN_BUZZER = 3; -unsigned char PIN_ROT = 2; +const unsigned char PIN_BUZZER = 3; +const unsigned char PIN_ROT = 2; -unsigned char NUM_SENSORS = 8; -unsigned char PIN_SENSOR[] = {19,18,17,16,15,14,13,12}; //右端-左端 +const unsigned char PIN_SENSOR_0 = 19; //右端 +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; -unsigned char PIN_SERVO_LANCE = 10; +const unsigned char PIN_SERVO_STEERING = 9; +const 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; +const unsigned char PIN_MOTOR_LEFT_E = 5; +const unsigned char PIN_MOTOR_LEFT_M = 4; +const unsigned char PIN_MOTOR_RIGHT_E = 6; +const unsigned char PIN_MOTOR_RIGHT_M = 7; -unsigned char MASK_LINE = 0b01111110; -unsigned char MASK_MARKER = 0b10000001; +const unsigned char MASK_LINE = 0b01111110; +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}; //{新,旧} -volatile long integral = 0; - -// 右のモーターと左のモーターの速度差 : (右のモーター)-(左のモーター) -volatile int speed = 0; - -// ステアリングの角度 -volatile long steeringAngle = 0; // 度 - -volatile int errorCount = 0; +int speed = SPEED_DEFAULT; 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(){ - sensorRead(); - int pos = 0; +/* センサーを読んで、車体の動作を決定する */ +void trace(){ 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; - break; - default: - 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; + steering(5); + motorL.speed(0); + motorR.speed(0); 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(linePosition[0]); +// Serial.println(distance); + 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; + } } diff --git a/hanzo4_1Utils.ino b/hanzo4Utils.ino similarity index 57% rename from hanzo4_1Utils.ino rename to hanzo4Utils.ino index 2a8ea48..ed7a9d3 100644 --- a/hanzo4_1Utils.ino +++ b/hanzo4Utils.ino @@ -21,18 +21,28 @@ void lance(int a){ /* センサーのアレイの初期化 */ void sensorInit(){ - for(int i=0; i