nebel/hanzo
1
0
Fork 0
Code Issues Pull requests Projects Releases Wiki Activity

半蔵4.1を作成した

Browse source
This commit is contained in:
Nebel 2014-03-19 01:45:38 +09:00
parent cd7fe687b8
commit 6c6e9ac02b
5 changed files with 192 additions and 203 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
Motor.cpp
View file

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

21
Motor.h
View file

@ -3,7 +3,6 @@ Motor.h - モータードライバ(L298P シールド)のためのライブラ
## 概要 ## ## 概要 ##
このライブラリは、モータードライバ(L298P シールド)のためのシンプルなライブラリです。 このライブラリは、モータードライバ(L298P シールド)のためのシンプルなライブラリです。
PWM対応ピンが1つしか必要ないのがこのライブラリの特徴です。
## 使い方 ## ## 使い方 ##
例: 例:
@ -11,7 +10,7 @@ PWM対応ピンが1つしか必要ないのがこのライブラリの特徴で
Motor motor; Motor motor;
void setup(){ void setup(){
// motor.attach(pin1, pin2); // motor.attach(pin1, pin2);
motor.attach(5, 6); motor.attach(5, 4);
} }
void loop(){ void loop(){
// motor.mode(GO); //正転 // 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::mode(char mode, byte speed);
void Motor::speed(int speed); void Motor::speed(int speed);
これらを使用する場合、pin1はPWM対応でなければなりません。 これらを使用する場合、pin1はPWM対応でなければなりません。
## ライセンス ## ## ライセンス ##
(C)2013 kou029w - MIT License (C)2014 kou029w - MIT License
*/ */
#ifndef Motor_h #ifndef Motor_h

13
README.md
View file

@ -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 ### ### Hardware ###
[@ @@@@@@ @] - Sensor array [@ @@@@@@ @] - Sensor array
@ -28,7 +29,7 @@
MPU - Atmel AVR MPU - Atmel AVR
Rotary encoder - ロータリーエンコーダー Rotary encoder - ロータリーエンコーダー
Sensor array - 反射型フォトインタラプタSG-2BC Sensor array - 反射型フォトインタラプタSG-2BC
Motor for driving - 駆動用モーター/ハイスピードギアボックスHE Motor for driving - 駆動用モーター/ダイセン
Servomotor for steering - ステアリング用サーボモーター Servomotor for steering - ステアリング用サーボモーター
Servomotor for lance - ランス用サーボモーター Servomotor for lance - ランス用サーボモーター
@ -42,7 +43,7 @@
* 使用サーボモーター : Savox SC-0352(ランス用) * 使用サーボモーター : Savox SC-0352(ランス用)
### 回路仕様 ### ### 回路仕様 ###
* 動作電圧 : 7.2V以上 * 動作電圧 : 11.1V(駆動用モーター), 7.4V(MPU, サーボ)
* 使用マイコン : Atmel AVR (Arduino) * 使用マイコン : Atmel AVR (Arduino)
* 使用センサーアレイ : SG-2BC x 8 (ロボラボ講習会のもの) * 使用センサーアレイ : SG-2BC x 8 (ロボラボ講習会のもの)
@ -69,4 +70,4 @@
* 使用言語 : Arduino 1.0 * 使用言語 : Arduino 1.0
## ライセンス ## ## ライセンス ##
(C)2013 kou029w - MIT Licence (C)2014 kou029w - MIT Licence

337
hanzo4.ino → hanzo4_1.ino
View file

@ -1,60 +1,60 @@
/* /*
競技用ランサーロボット 競技用ランサーロボット
半蔵 4.0 半蔵 4.1
(C)2013 kou029w - MIT License (C)2014 kou029w - MIT License
*/ */
#include <MsTimer2.h>
#include <Tone.h> #include <Tone.h>
#include <Servo.h> #include <Servo.h>
#include "Motor.h" #include "Motor.h"
const unsigned long LAP_DISTANCE = 28500; //ロータリーエンコーダーのカウント数/周 unsigned long LAP_DISTANCE = 28500; //ロータリーエンコーダーのカウント数/周
const unsigned long TURN_DISTANCE = 4650; //カーブ中でのカウント数 unsigned long TURN_DISTANCE = 4650; //カーブ中でのカウント数
const int LANCE_ANGLE_E = 40; //度 int LANCE_ANGLE_E = 40; //度
const int LANCE_ANGLE_F = 61; //度 int LANCE_ANGLE_F = 61; //度
const int LANCE_ANGLE_A = 29; //度 int LANCE_ANGLE_A = 29; //度
const int LANCE_ANGLE_B = -32; //度 int LANCE_ANGLE_B = -32; //度
const int LANCE_ANGLE_C = 34; //度 int LANCE_ANGLE_C = 34; //度
const int LANCE_ANGLE_D = -42; //度 int LANCE_ANGLE_D = -42; //度
//const unsigned char SPEED_DEFAULT = 0; int SPEED_DEFAULT = 100;
const unsigned char SPEED_DEFAULT = 127;
//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 // Servo : SC-1267SG
const unsigned int STEERING_CENTER = 1810; //us unsigned int STEERING_CENTER = 1810; //us
const unsigned int STEERING_MIN = (STEERING_CENTER-780); //us unsigned int STEERING_MIN = (STEERING_CENTER-780); //us
const unsigned int STEERING_MAX = (STEERING_CENTER+780); //us unsigned int STEERING_MAX = (STEERING_CENTER+780); //us
//Servo : SC-0352 // Servo : SC-0352
const unsigned int LANCE_CENTER = 1530; //us unsigned int LANCE_CENTER = 1530; //us
const unsigned int LANCE_MIN = (LANCE_CENTER-780); //us unsigned int LANCE_MIN = (LANCE_CENTER-780); //us
const unsigned int LANCE_MAX = (LANCE_CENTER+780); //us unsigned int LANCE_MAX = (LANCE_CENTER+780); //us
const unsigned char PIN_BUZZER = 3; unsigned char PIN_BUZZER = 3;
const unsigned char PIN_ROT = 2; unsigned char PIN_ROT = 2;
const unsigned char PIN_SENSOR_0 = 19; //右端 unsigned char NUM_SENSORS = 8;
const unsigned char PIN_SENSOR_1 = 18; unsigned char PIN_SENSOR[] = {19,18,17,16,15,14,13,12}; //右端-左端
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; //左端
const unsigned char PIN_SERVO_STEERING = 9; unsigned char PIN_SERVO_STEERING = 9;
const unsigned char PIN_SERVO_LANCE = 10; unsigned char PIN_SERVO_LANCE = 10;
const unsigned char PIN_MOTOR_LEFT_E = 5; unsigned char PIN_MOTOR_LEFT_E = 5;
const unsigned char PIN_MOTOR_LEFT_M = 4; unsigned char PIN_MOTOR_LEFT_M = 4;
const unsigned char PIN_MOTOR_RIGHT_E = 6; unsigned char PIN_MOTOR_RIGHT_E = 6;
const unsigned char PIN_MOTOR_RIGHT_M = 7; unsigned char PIN_MOTOR_RIGHT_M = 7;
const unsigned char MASK_LINE = 0b01111110; unsigned char MASK_LINE = 0b01111110;
const unsigned char MASK_MARKER = 0b10000001; unsigned char MASK_MARKER = 0b10000001;
/**************************************/ /**************************************/
@ -64,13 +64,21 @@ Motor motorR;
Motor motorL; Motor motorL;
//ラインが1、地面が0、LSBが左端、MSBが右端 //ラインが1、地面が0、LSBが左端、MSBが右端
unsigned char sensor = 0x00; volatile unsigned char sensor = 0x00;
int errorCount = 0;
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; 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_C, //右垂直標的
MODE_ATTACK_D, //左垂直標的 MODE_ATTACK_D, //左垂直標的
MODE_ATTACK_CYLINDER //円筒標的 MODE_ATTACK_CYLINDER //円筒標的
} } mode;
mode;
/**************************************/ /**************************************/
/* センサーを読んで、車体の動作を決定する */ /* センサーを読んでラインの位置を返す */
void trace(){ int scanSensor(){
sensorRead();
int pos = 0;
switch(sensor&MASK_LINE){ switch(sensor&MASK_LINE){
case 0b01000000: case 0b01000000:
steering(-21); pos = -5;
motorL.speed(-speed/2);
motorR.speed(speed/2);
break; break;
case 0b01100000: case 0b01100000:
steering(-20); pos = -4;
motorL.speed(-speed/2);
motorR.speed(speed/2);
break; break;
case 0b00100000: case 0b00100000:
steering(-19); pos = -3;
motorL.speed(-speed/2);
motorR.speed(speed/2);
break; break;
case 0b00110000: case 0b00110000:
steering(-2); pos = -2;
motorL.speed(speed);
motorR.speed(speed);
break; break;
case 0b00010000: case 0b00010000:
steering(-1); pos = -1;
motorL.speed(speed);
motorR.speed(speed);
break; break;
case 0b00011000: case 0b00011000:
steering(0); pos = 0;
motorL.speed(speed);
motorR.speed(speed);
break; break;
case 0b00001000: case 0b00001000:
steering(1); pos = 1;
motorL.speed(speed);
motorR.speed(speed);
break; break;
case 0b00001100: case 0b00001100:
steering(2); pos = 2;
motorL.speed(speed);
motorR.speed(speed);
break; break;
case 0b00000100: case 0b00000100:
steering(3); pos = 3;
motorL.speed(0);
motorR.speed(0);
break; break;
case 0b00000110: case 0b00000110:
steering(4); pos = 4;
motorL.speed(0);
motorR.speed(0);
break; break;
case 0b00000010: case 0b00000010:
steering(5); pos = 5;
motorL.speed(0); break;
motorR.speed(0); 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; 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(){ void modeSet(){
if(sensor == 0) errorCount++; if(distance > LAP_DISTANCE || errorCount > 5000){
else errorCount = 0;
if(distance > LAP_DISTANCE*13 || errorCount > 5000){
mode = MODE_STOP; mode = MODE_STOP;
return; return;
} }
@ -196,7 +284,7 @@ void modeSet(){
} }
void setup(){ void setup(){
// Serial.begin(9600); Serial.begin(9600);
//サーボ //サーボ
steeringServo.attach(PIN_SERVO_STEERING, STEERING_MIN, STEERING_MAX); steeringServo.attach(PIN_SERVO_STEERING, STEERING_MIN, STEERING_MAX);
lanceServo.attach(PIN_SERVO_LANCE, LANCE_MIN, LANCE_MAX); lanceServo.attach(PIN_SERVO_LANCE, LANCE_MIN, LANCE_MAX);
@ -214,91 +302,18 @@ void setup(){
attachInterrupt(0, rot, RISING); attachInterrupt(0, rot, RISING);
//ブザー //ブザー
pinMode(PIN_BUZZER, OUTPUT); pinMode(PIN_BUZZER, OUTPUT);
startBeep(); // startBeep();
/* 3秒停止 */ /* 3秒停止 */
motorL.mode(STOP); motorL.mode(STOP);
motorR.mode(STOP); motorR.mode(STOP);
delay(3000); delay(3000);
/* スタート */ /* スタート */
mode = MODE_STRAIGHT; mode = MODE_STRAIGHT;
MsTimer2::set(1, trace);
MsTimer2::start();
} }
void loop(){ void loop(){
// Serial.println(distance); scanSensor();
sensorRead(); Serial.println(linePosition[0]);
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;
}
} }

22
hanzo4Utils.ino → hanzo4_1Utils.ino
View file

@ -21,28 +21,18 @@ void lance(int a){
/* センサーのアレイの初期化 */ /* センサーのアレイの初期化 */
void sensorInit(){ void sensorInit(){
pinMode(PIN_SENSOR_0, INPUT); //右端 for(int i=0; i<NUM_SENSORS; i++){
pinMode(PIN_SENSOR_1, INPUT); pinMode(PIN_SENSOR[i], 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 sensorRead(){
unsigned char b; unsigned char b;
//黒が1、白が0 //黒が1、白が0
if(digitalRead(PIN_SENSOR_0)) b += 0b00000001; //右端 for(int i=0; i<NUM_SENSORS; i++){
if(digitalRead(PIN_SENSOR_1)) b += 0b00000010; if(digitalRead(PIN_SENSOR[i])) b += 1<<i; //右端-左端
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とする b ^= 0xff; //今回ラインが白なので、反転して、ラインを1とする
return sensor = b; return sensor = b;
} }