From 6c6e9ac02b8472514948e7f86d05443e6e553dcd Mon Sep 17 00:00:00 2001 From: kou029w Date: Wed, 19 Mar 2014 01:45:38 +0900 Subject: [PATCH] =?UTF-8?q?=E5=8D=8A=E8=94=B54.1=E3=82=92=E4=BD=9C?= =?UTF-8?q?=E6=88=90=E3=81=97=E3=81=9F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Motor.cpp | 2 +- Motor.h | 21 +- README.md | 13 +- hanzo4.ino => hanzo4_1.ino | 337 ++++++++++++++------------- hanzo4Utils.ino => hanzo4_1Utils.ino | 22 +- 5 files changed, 192 insertions(+), 203 deletions(-) rename hanzo4.ino => hanzo4_1.ino (59%) rename hanzo4Utils.ino => hanzo4_1Utils.ino (57%) diff --git a/Motor.cpp b/Motor.cpp index 873239a..e3b967a 100644 --- a/Motor.cpp +++ b/Motor.cpp @@ -1,6 +1,6 @@ /* Motor.cpp -(C)2013 kou029w - MIT License +(C)2014 kou029w - MIT License */ #include "Motor.h" diff --git a/Motor.h b/Motor.h index 9c49f4a..4d5daca 100644 --- a/Motor.h +++ b/Motor.h @@ -3,7 +3,6 @@ Motor.h - モータードライバ(L298P シールド)のためのライブラ ## 概要 ## このライブラリは、モータードライバ(L298P シールド)のためのシンプルなライブラリです。 -PWM対応ピンが1つしか必要ないのがこのライブラリの特徴です。 ## 使い方 ## 例: @@ -11,7 +10,7 @@ PWM対応ピンが1つしか必要ないのがこのライブラリの特徴で Motor motor; void setup(){ // motor.attach(pin1, pin2); - motor.attach(5, 6); + motor.attach(5, 4); } void loop(){ // motor.mode(GO); //正転 @@ -22,29 +21,13 @@ PWM対応ピンが1つしか必要ないのがこのライブラリの特徴で } 注意: -使用するモータードライバの動作は、 -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)2013 kou029w - MIT License +(C)2014 kou029w - MIT License */ #ifndef Motor_h diff --git a/README.md b/README.md index 3103263..3a6a196 100644 --- a/README.md +++ b/README.md @@ -1,10 +1,11 @@ -# 競技用ランサーロボット 半蔵4.0 制御プログラム # + +# 競技用ランサーロボット 半蔵4.1 制御プログラム # ## これはなに ## -[ロボットグランプリ](http://www.robotgrandprix.com/)に向けて製作中のマシン、半蔵4.0のための制御プログラムです。 +[ロボットグランプリ](http://www.robotgrandprix.com/)に向けて製作中のマシン、半蔵4.1のための制御プログラムです。 ## システム構成 ## -以下に、半蔵4.0のシステム構成を示します。 +以下に、半蔵4.1のシステム構成を示します。 ### Hardware ### [@ @@@@@@ @] - Sensor array @@ -28,7 +29,7 @@ MPU - Atmel AVR Rotary encoder - ロータリーエンコーダー Sensor array - 反射型フォトインタラプタSG-2BC - Motor for driving - 駆動用モーター/ハイスピードギアボックスHE + Motor for driving - 駆動用モーター/ダイセン Servomotor for steering - ステアリング用サーボモーター Servomotor for lance - ランス用サーボモーター @@ -42,7 +43,7 @@ * 使用サーボモーター : Savox SC-0352(ランス用) ### 回路仕様 ### -* 動作電圧 : 7.2V以上 +* 動作電圧 : 11.1V(駆動用モーター), 7.4V(MPU, サーボ) * 使用マイコン : Atmel AVR (Arduino) * 使用センサーアレイ : SG-2BC x 8 (ロボラボ講習会のもの) @@ -69,4 +70,4 @@ * 使用言語 : Arduino 1.0 ## ライセンス ## -(C)2013 kou029w - MIT Licence \ No newline at end of file +(C)2014 kou029w - MIT Licence \ No newline at end of file diff --git a/hanzo4.ino b/hanzo4_1.ino similarity index 59% rename from hanzo4.ino rename to hanzo4_1.ino index 7d87e45..7159480 100644 --- a/hanzo4.ino +++ b/hanzo4_1.ino @@ -1,60 +1,60 @@ /* 競技用ランサーロボット -半蔵 4.0 -(C)2013 kou029w - MIT License +半蔵 4.1 +(C)2014 kou029w - MIT License */ +#include #include #include #include "Motor.h" -const unsigned long LAP_DISTANCE = 28500; //ロータリーエンコーダーのカウント数/周 -const unsigned long TURN_DISTANCE = 4650; //カーブ中でのカウント数 +unsigned long LAP_DISTANCE = 28500; //ロータリーエンコーダーのカウント数/周 +unsigned long TURN_DISTANCE = 4650; //カーブ中でのカウント数 -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 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 unsigned char SPEED_DEFAULT = 0; -const unsigned char SPEED_DEFAULT = 127; +int SPEED_DEFAULT = 100; -//const float STEERING_KP = 3.8; +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 -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 +unsigned int STEERING_CENTER = 1810; //us +unsigned int STEERING_MIN = (STEERING_CENTER-780); //us +unsigned int STEERING_MAX = (STEERING_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 +// 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 -const unsigned char PIN_BUZZER = 3; -const unsigned char PIN_ROT = 2; +unsigned char PIN_BUZZER = 3; +unsigned char PIN_ROT = 2; -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 NUM_SENSORS = 8; +unsigned char PIN_SENSOR[] = {19,18,17,16,15,14,13,12}; //右端-左端 -const unsigned char PIN_SERVO_STEERING = 9; -const unsigned char PIN_SERVO_LANCE = 10; +unsigned char PIN_SERVO_STEERING = 9; +unsigned char PIN_SERVO_LANCE = 10; -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 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 MASK_LINE = 0b01111110; -const unsigned char MASK_MARKER = 0b10000001; +unsigned char MASK_LINE = 0b01111110; +unsigned char MASK_MARKER = 0b10000001; /**************************************/ @@ -64,13 +64,21 @@ Motor motorR; Motor motorL; //ラインが1、地面が0、LSBが左端、MSBが右端 -unsigned char sensor = 0x00; -int errorCount = 0; +volatile unsigned char sensor = 0x00; -int speed = SPEED_DEFAULT; +volatile int linePosition[] = {0,0}; //{新,旧} +volatile long integral = 0; + +// 右のモーターと左のモーターの速度差 : (右のモーター)-(左のモーター) +volatile int speed = 0; + +// ステアリングの角度 +volatile long steeringAngle = 0; // 度 + +volatile int errorCount = 0; volatile unsigned long distance = 0; -unsigned long modeChangedDistance = 0; +volatile unsigned long modeChangedDistance = 0; /**************************************/ @@ -85,77 +93,157 @@ enum mode_t { MODE_ATTACK_C, //右垂直標的 MODE_ATTACK_D, //左垂直標的 MODE_ATTACK_CYLINDER //円筒標的 -} -mode; +} mode; /**************************************/ -/* センサーを読んで、車体の動作を決定する */ -void trace(){ +/* センサーを読んでラインの位置を返す */ +int scanSensor(){ + sensorRead(); + int pos = 0; switch(sensor&MASK_LINE){ case 0b01000000: - steering(-21); - motorL.speed(-speed/2); - motorR.speed(speed/2); + pos = -5; break; case 0b01100000: - steering(-20); - motorL.speed(-speed/2); - motorR.speed(speed/2); + pos = -4; break; case 0b00100000: - steering(-19); - motorL.speed(-speed/2); - motorR.speed(speed/2); + pos = -3; break; case 0b00110000: - steering(-2); - motorL.speed(speed); - motorR.speed(speed); + pos = -2; break; case 0b00010000: - steering(-1); - motorL.speed(speed); - motorR.speed(speed); + pos = -1; break; case 0b00011000: - steering(0); - motorL.speed(speed); - motorR.speed(speed); + pos = 0; break; case 0b00001000: - steering(1); - motorL.speed(speed); - motorR.speed(speed); + pos = 1; break; case 0b00001100: - steering(2); - motorL.speed(speed); - motorR.speed(speed); + pos = 2; break; case 0b00000100: - steering(3); - motorL.speed(0); - motorR.speed(0); + pos = 3; break; case 0b00000110: - steering(4); - motorL.speed(0); - motorR.speed(0); + pos = 4; break; case 0b00000010: - steering(5); - motorL.speed(0); - motorR.speed(0); + 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; 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(sensor == 0) errorCount++; - else errorCount = 0; - if(distance > LAP_DISTANCE*13 || errorCount > 5000){ + if(distance > LAP_DISTANCE || errorCount > 5000){ mode = MODE_STOP; return; } @@ -196,7 +284,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); @@ -214,91 +302,18 @@ 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(){ -// 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; - } + scanSensor(); + Serial.println(linePosition[0]); } diff --git a/hanzo4Utils.ino b/hanzo4_1Utils.ino similarity index 57% rename from hanzo4Utils.ino rename to hanzo4_1Utils.ino index ed7a9d3..2a8ea48 100644 --- a/hanzo4Utils.ino +++ b/hanzo4_1Utils.ino @@ -21,28 +21,18 @@ void lance(int a){ /* センサーのアレイの初期化 */ void sensorInit(){ - pinMode(PIN_SENSOR_0, 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); //左端 + for(int i=0; i