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
(C)2013 kou029w - MIT License
(C)2014 kou029w - MIT License
*/
#include "Motor.h"

21
Motor.h
View file

@ -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

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 ###
[@ @@@@@@ @] - 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
(C)2014 kou029w - MIT Licence

337
hanzo4.ino → hanzo4_1.ino
View file

@ -1,60 +1,60 @@
/*
競技用ランサーロボット
半蔵 4.0
(C)2013 kou029w - MIT License
半蔵 4.1
(C)2014 kou029w - MIT License
*/
#include <MsTimer2.h>
#include <Tone.h>
#include <Servo.h>
#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]);
}

22
hanzo4Utils.ino → hanzo4_1Utils.ino
View file

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