nebel/2014-git-work
1
0
Fork 0
Code Issues Pull requests Projects Releases Wiki Activity
2014-git-work/arduino/hanzo2_1/hanzo2_1.ino

372 lines
9.9 KiB
C++
Raw Permalink Blame History

/*
競技用ランサーロボット
半蔵 2.1
[7 654321 0]
| |
| |
[]---{}---[]
/ .\
/ | \
[]+--{}--+[]
(c)2012 kou029w - MIT License [http://kou029w.appspot.com/mit-license.txt]
*/
#include <Servo.h>
#include "Motor.h"
const unsigned long LAP_COUNT = 848; //カウント/周
const unsigned long TARGET_NONE_COUNT = LAP_COUNT*90/848; //カーブ中でのカウント数
const int LANCE_ANGLE_PARALLEL1 = 90 - 53; //度
const int LANCE_ANGLE_PARALLEL2 = 90 - 43; //度
const int LANCE_ANGLE_LEFT_VERTICAL = 90 - 120; //度
const int LANCE_ANGLE_RIGHT_VERTICAL = 90 - 57; //度
const byte SPEED_DEFAULT = 255;
const int HANDLE_DEGREE_DEFAULT = 90; //度
const unsigned int HANDLE_MIN = ( 800+30); //us
const unsigned int HANDLE_MAX = (2300+30); //us
const float HANDLE_KP = 4.0;
const int LANCE_DEGREE_DEFAULT = 90; //度
const unsigned int LANCE_MIN = ( 544-62); //us
const unsigned int LANCE_MAX = (2400-62); //us
const byte PIN_LED = 4;
const byte PIN_ROT = 2;
const byte PIN_SENSOR_0 = 12;
const byte PIN_SENSOR_1 = 13;
const byte PIN_SENSOR_2 = 14;
const byte PIN_SENSOR_3 = 15;
const byte PIN_SENSOR_4 = 16;
const byte PIN_SENSOR_5 = 17;
const byte PIN_SENSOR_6 = 18;
const byte PIN_SENSOR_7 = 19;
const byte PIN_SERVO_HANDLE = 10;
const byte PIN_SERVO_LANCE = 9;
const byte PIN_MOTOR_LEFT_1 = 5;
const byte PIN_MOTOR_LEFT_2 = 7;
const byte PIN_MOTOR_RIGHT_1 = 6;
const byte PIN_MOTOR_RIGHT_2 = 8;
enum {
MODE_STOP, //停止
MODE_TRACE, //標準
MODE_LEFT, //左カーブ
MODE_TARGET_PARALLEL1, //平行標的1
MODE_TARGET_PARALLEL2, //平行標的2
MODE_TARGET_RIGHT_VERTICAL, //右垂直標的
MODE_TARGET_LEFT_VERTICAL, //左垂直標的
MODE_TARGET_CYLINDER //円筒標的
};
const byte MASK_MODE_TRACE = 0b01111110;
const byte MASK_CHECK_MARKER = 0b10000001;
/**************************************/
Servo servoHandle;
Servo servoLance;
Motor motorR;
Motor motorL;
byte mode;
volatile unsigned long counter = 0;
unsigned long modeChangeCounter;
/**************************************/
/* センサーを読んで、車体の動作を決定する */
void trace(byte sensor){
switch(sensor&MASK_MODE_TRACE){
case 0b01000000:
handle(-5*HANDLE_KP);
motorMode(GO, GO, 0, SPEED_DEFAULT*0.1);
break;
case 0b01100000:
handle(-4*HANDLE_KP);
motorMode(GO, GO, 0, SPEED_DEFAULT*0.1);
break;
case 0b00100000:
handle(-3*HANDLE_KP);
motorMode(GO, GO, SPEED_DEFAULT*0.333, SPEED_DEFAULT);
break;
case 0b00110000:
handle(-2*HANDLE_KP);
motorMode(GO, GO, SPEED_DEFAULT*0.667, SPEED_DEFAULT);
break;
case 0b00010000:
handle(-2);
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
break;
case 0b00011000:
handle(0);
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
break;
case 0b00001000:
handle(2);
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
break;
case 0b00001100:
handle(2*HANDLE_KP);
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
break;
case 0b00000100:
handle(3*HANDLE_KP);
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
break;
case 0b00000110:
handle(4*HANDLE_KP);
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
break;
case 0b00000010:
handle(5*HANDLE_KP);
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
break;
}
}
/* MODE_LEFT時の再加速用trace() */
void traceLeft(byte sensor){
switch(sensor&MASK_MODE_TRACE){
case 0b01000000:
handle(-5*HANDLE_KP);
motorMode(GO, GO, 0, SPEED_DEFAULT);
break;
case 0b01100000:
handle(-4*HANDLE_KP);
motorMode(GO, GO, 0, SPEED_DEFAULT);
break;
case 0b00100000:
handle(-3*HANDLE_KP);
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
break;
case 0b00110000:
handle(-2*HANDLE_KP);
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
break;
case 0b00010000:
handle(-2);
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
break;
case 0b00011000:
handle(0);
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
break;
case 0b00001000:
handle(2);
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
break;
case 0b00001100:
handle(2*HANDLE_KP);
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
break;
case 0b00000100:
handle(3*HANDLE_KP);
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
break;
case 0b00000110:
handle(4*HANDLE_KP);
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
break;
case 0b00000010:
handle(5*HANDLE_KP);
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
break;
}
}
/* モーターの動作を決定する */
void motorMode(byte leftMode, byte rightMode, byte leftSpeed, byte rightSpeed){
motorL.mode( leftMode, leftSpeed);
motorR.mode(rightMode, rightSpeed);
}
/* ステアリングを中央からdegree[度]だけ動かす
<- - 0 + ->
| |
| |
[]---{}---[]
/ .\
*/
void handle(int degree){
servoHandle.write(HANDLE_DEGREE_DEFAULT + degree);
}
/* ランスを中央からdegree[度]だけ動かす
<- - 0 + ->
/ .\
/ | \
[]+--{}--+[]
*/
void lance(int degree){
servoLance.write(LANCE_DEGREE_DEFAULT - degree);
}
/* ロータリーエンコーダーの変化を見る
todo:外部割り込み使ってみたい、、、
*/
void count(){
static byte rot[2] = {0}; //rot[0]:今の状態, rot[1]:前回の状態
rot[0] = digitalRead(PIN_ROT);
if(rot[0] != rot[1]){
rot[1] = rot[0];
counter++;
}
}
/* マーカーを読んで、モードを返す */
int checkMarker(byte sensor){
int mode = MODE_TRACE;
switch(sensor & MASK_CHECK_MARKER){
case 0b10000001:
modeChangeCounter = counter;
mode = MODE_LEFT;
break;
case 0b00000001:
modeChangeCounter = counter;
if(counter%LAP_COUNT > LAP_COUNT/2){ //後半は垂直標的をねらいたい
mode = MODE_TARGET_RIGHT_VERTICAL;
}else if(counter%LAP_COUNT < LAP_COUNT/7){ //まずは、E的をねらいたい
mode = MODE_TARGET_PARALLEL1;
}else if(counter%LAP_COUNT < LAP_COUNT/3){ //次に、F的をねらいたい
mode = MODE_TARGET_PARALLEL2;
}else{
mode = MODE_TARGET_RIGHT_VERTICAL;
}
break;
case 0b10000000:
modeChangeCounter = counter;
mode = MODE_TARGET_LEFT_VERTICAL;
break;
}
return mode;
}
/* 前方センサーの初期化 */
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);
}
/* センサーの状態を返す */
byte sensorRead(){
byte sensor = 0; //黒が1、白が0
if(digitalRead(PIN_SENSOR_0)) sensor += 0b00000001;
if(digitalRead(PIN_SENSOR_1)) sensor += 0b00000010;
if(digitalRead(PIN_SENSOR_2)) sensor += 0b00000100;
if(digitalRead(PIN_SENSOR_3)) sensor += 0b00001000;
if(digitalRead(PIN_SENSOR_4)) sensor += 0b00010000;
if(digitalRead(PIN_SENSOR_5)) sensor += 0b00100000;
if(digitalRead(PIN_SENSOR_6)) sensor += 0b01000000;
if(digitalRead(PIN_SENSOR_7)) sensor += 0b10000000;
return sensor;
}
void setup() {
//サーボ
servoHandle.attach(PIN_SERVO_HANDLE, HANDLE_MIN, HANDLE_MAX);
servoLance.attach(PIN_SERVO_LANCE, LANCE_MIN, LANCE_MAX);
handle(0);
lance(0);
//モーター
motorL.attach( PIN_MOTOR_LEFT_1, PIN_MOTOR_LEFT_2);
motorR.attach(PIN_MOTOR_RIGHT_1, PIN_MOTOR_RIGHT_2);
motorL.mode(STOP);
motorR.mode(STOP);
//センサー
sensorInit();
//エンコーダー
pinMode(PIN_ROT, INPUT);
//LED
pinMode(PIN_LED, OUTPUT);
digitalWrite(PIN_LED, HIGH);
//モード
mode = MODE_TRACE;
delay(1000);
/* スタート */
digitalWrite(PIN_LED, LOW);
handle(0);
lance(0);
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
}
void loop(){
/* センサーを見る */
byte sensor = 0;
sensor = ~sensorRead(); //ライン(白)が1、地面(黒)が0
count(); //ロータリーエンコーダーの変化を見る
/* 全体の動作を決定する */
switch(mode){
case MODE_TARGET_RIGHT_VERTICAL: //右垂直標的
lance(LANCE_ANGLE_RIGHT_VERTICAL);
mode = MODE_TRACE;
break;
case MODE_TARGET_LEFT_VERTICAL: //左垂直標的
lance(LANCE_ANGLE_LEFT_VERTICAL);
mode = MODE_TRACE;
break;
case MODE_TARGET_PARALLEL1: //平行標的1
trace(sensor);
lance(LANCE_ANGLE_PARALLEL1);
if(counter - modeChangeCounter > 24){ //マーカーから少し進んで、叩く
lance(LANCE_ANGLE_PARALLEL1+8);
mode = MODE_TRACE;
}
break;
case MODE_TARGET_PARALLEL2: //平行標的2
trace(sensor);
lance(LANCE_ANGLE_PARALLEL2);
if(counter - modeChangeCounter > 26){ //マーカーから少し進んで、叩く
lance(LANCE_ANGLE_PARALLEL2+8);
mode = MODE_TRACE;
}
break;
case MODE_TRACE:
trace(sensor);
mode = checkMarker(sensor);
if(counter > LAP_COUNT*10){ //10周したら停止
mode = MODE_STOP;
}
break;
case MODE_LEFT: //左カーブ
// trace(sensor);
if(counter - modeChangeCounter < TARGET_NONE_COUNT/2) trace(sensor);
else traceLeft(sensor); //カーブの後半は加速したい
if(counter < LAP_COUNT) lance(30); //1周目は円筒標的をねらいたい
else lance(-30);
if(counter - modeChangeCounter > TARGET_NONE_COUNT){
lance(0);
mode = MODE_TRACE;
}
break;
// case MODE_TARGET_CYLINDER: //円筒標的
// digitalWrite(PIN_LED, HIGH);
// handle(0);
// lance(-90);
// motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
// if(sensor&MASK_CHECK_MARKER == 0b10000001){
// modeChangeCounter = counter;
// }else if( counter - modeChangeCounter > LAP_COUNT/21 ){
// mode = MODE_STOP;
// }
// break;
case MODE_STOP: //停止
digitalWrite(PIN_LED, HIGH);
motorL.mode(STOP);
motorR.mode(STOP);
break;
}
}
Reference in a new issue View git blame Copy permalink