342 lines
7.7 KiB
C++
342 lines
7.7 KiB
C++
/*
|
|
競技用ランサーロボット
|
|
半蔵 3.0
|
|
(C)2013 kou029w - MIT License
|
|
*/
|
|
|
|
#include <Tone.h>
|
|
#include <Servo.h>
|
|
#include "Motor.h"
|
|
|
|
const unsigned long LAP_DISTANCE = 10000; //ロータリーエンコーダーのカウント数/周
|
|
const unsigned long TURN_DISTANCE = LAP_DISTANCE*90/848; //カーブ中でのカウント数
|
|
|
|
/*ここまだ*/
|
|
const int LANCE_ANGLE_PARALLEL_1 = 90 - 53; //度
|
|
const int LANCE_ANGLE_PARALLEL_2 = 90 - 43; //度
|
|
const int LANCE_ANGLE_VERTICAL_L = 90 - 120; //度
|
|
const int LANCE_ANGLE_VERTICAL_R = 90 - 57; //度
|
|
|
|
const byte SPEED_DEFAULT = 128;
|
|
|
|
// Servo : DHCM (2013-03-13)
|
|
// 60deg : 1250 us
|
|
// 90deg : 1440 us
|
|
//120deg : 1630 us
|
|
const int STEERING_DEFAULT = 90; //度
|
|
const unsigned int STEERING_MIN = (1440-570); //us
|
|
const unsigned int STEERING_MAX = (1440+570); //us
|
|
|
|
const float STEERING_KP = 4.0;
|
|
|
|
//Servo : SC-0352
|
|
// 60deg : 1265 us
|
|
// 90deg : 1535 us
|
|
//120deg : 1805 us
|
|
const int LANCE_DEFAULT = 90; //度
|
|
const unsigned int LANCE_MIN = (1535-780); //us
|
|
const unsigned int LANCE_MAX = (1535+780); //us
|
|
|
|
const byte PIN_BUZZER = 3;
|
|
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_STEERING = 9;
|
|
const byte PIN_SERVO_LANCE = 10;
|
|
|
|
const byte PIN_MOTOR_LEFT = 6;
|
|
const byte PIN_MOTOR_RIGHT = 5;
|
|
|
|
const byte MASK_LINE = 0b01111110;
|
|
const byte MASK_MARKER = 0b10000001;
|
|
|
|
/**************************************/
|
|
|
|
Servo steeringServo;
|
|
Servo lanceServo;
|
|
Motor motorR;
|
|
Motor motorL;
|
|
|
|
//ラインが1、地面が0、LSBが左端、MSBが右端
|
|
byte sensor = 0x00;
|
|
|
|
volatile unsigned long distance = 0;
|
|
unsigned long modeChangedDistance = 0;
|
|
|
|
/**************************************/
|
|
|
|
enum mode_t {
|
|
MODE_STOP, //停止
|
|
MODE_STRAIGHT, //まっすぐ進む
|
|
MODE_TURN, //左に曲がる
|
|
MODE_ATTACK //標的を狙う
|
|
}
|
|
mode;
|
|
|
|
enum target_t {
|
|
TARGET_NONE,
|
|
TARGET_PARALLEL_1, //平行標的1
|
|
TARGET_PARALLEL_2, //平行標的2
|
|
TARGET_VERTICAL_R, //右垂直標的
|
|
TARGET_VERTICAL_L, //左垂直標的
|
|
TARGET_CYLINDER //円筒標的
|
|
}
|
|
target;
|
|
|
|
/**************************************/
|
|
|
|
/* センサーを読んで、車体の動作を決定する */
|
|
void trace(){
|
|
switch(sensor&MASK_LINE){
|
|
case 0b01000000:
|
|
steering(-5*STEERING_KP);
|
|
motorL.speed(0);
|
|
motorR.speed(SPEED_DEFAULT*0.7);
|
|
break;
|
|
case 0b01100000:
|
|
steering(-4*STEERING_KP);
|
|
motorL.speed(0);
|
|
motorR.speed(SPEED_DEFAULT*0.7);
|
|
break;
|
|
case 0b00100000:
|
|
steering(-3*STEERING_KP);
|
|
motorL.speed(0);
|
|
motorR.speed(SPEED_DEFAULT);
|
|
break;
|
|
case 0b00110000:
|
|
steering(-2*STEERING_KP);
|
|
motorL.speed(0);
|
|
motorR.speed(SPEED_DEFAULT);
|
|
break;
|
|
case 0b00010000:
|
|
steering(-1);
|
|
motorL.speed(SPEED_DEFAULT);
|
|
motorR.speed(SPEED_DEFAULT);
|
|
break;
|
|
case 0b00011000:
|
|
steering(0);
|
|
motorL.speed(SPEED_DEFAULT);
|
|
motorR.speed(SPEED_DEFAULT);
|
|
break;
|
|
case 0b00001000:
|
|
steering(1);
|
|
motorL.speed(SPEED_DEFAULT);
|
|
motorR.speed(SPEED_DEFAULT);
|
|
break;
|
|
case 0b00001100:
|
|
steering(2*STEERING_KP);
|
|
motorL.speed(SPEED_DEFAULT);
|
|
motorR.speed(SPEED_DEFAULT);
|
|
break;
|
|
case 0b00000100:
|
|
steering(3*STEERING_KP);
|
|
motorL.speed(0);
|
|
motorR.speed(0);
|
|
break;
|
|
case 0b00000110:
|
|
steering(4*STEERING_KP);
|
|
motorL.speed(0);
|
|
motorR.speed(0);
|
|
break;
|
|
case 0b00000010:
|
|
steering(5*STEERING_KP);
|
|
motorL.speed(0);
|
|
motorR.speed(0);
|
|
break;
|
|
}
|
|
}
|
|
|
|
/* ステアリングを中央からa[度]だけ動かす
|
|
<- - 0 + ->
|
|
| |
|
|
| |
|
|
[]---{}---[]
|
|
/ .\
|
|
*/
|
|
void steering(int a){
|
|
steeringServo.write(STEERING_DEFAULT - a);
|
|
}
|
|
|
|
/* ランスを中央からa[度]だけ動かす
|
|
<- - 0 + ->
|
|
/ .\
|
|
/ | \
|
|
[]+--{}--+[]
|
|
*/
|
|
void lance(int a){
|
|
lanceServo.write(LANCE_DEFAULT - 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); //左端
|
|
}
|
|
|
|
/* センサーアレイの状態を見る*/
|
|
byte sensorRead(){
|
|
byte 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; //左端
|
|
//今回ラインが白なので、反転して、ラインを1とする
|
|
b ^= 0xff;
|
|
return sensor = b;
|
|
}
|
|
|
|
void modeSet(){
|
|
if(distance > LAP_DISTANCE*5){ //5周したら停止
|
|
mode = MODE_STOP;
|
|
return;
|
|
}
|
|
switch(sensor&MASK_MARKER){
|
|
default:
|
|
case 0b00000000:
|
|
mode = MODE_STRAIGHT;
|
|
target = TARGET_NONE;
|
|
case 0b10000001:
|
|
modeChangedDistance = distance;
|
|
mode = MODE_TURN;
|
|
target = TARGET_NONE;
|
|
break;
|
|
case 0b00000001:
|
|
modeChangedDistance = distance;
|
|
mode = MODE_ATTACK;
|
|
if(distance%LAP_DISTANCE < LAP_DISTANCE/7){ //まずは、E的をねらいたい
|
|
target = TARGET_PARALLEL_1;
|
|
}else if(distance%LAP_DISTANCE < LAP_DISTANCE/3){ //次に、F的をねらいたい
|
|
target = TARGET_PARALLEL_2;
|
|
}else{ //後半は垂直標的をねらいたい
|
|
target = TARGET_VERTICAL_R;
|
|
}
|
|
break;
|
|
case 0b10000000:
|
|
modeChangedDistance = distance;
|
|
mode = MODE_ATTACK;
|
|
target = TARGET_VERTICAL_L;
|
|
break;
|
|
}
|
|
}
|
|
|
|
void stop(){
|
|
motorL.mode(STOP);
|
|
motorR.mode(STOP);
|
|
}
|
|
|
|
void straight(){
|
|
trace();
|
|
modeSet();
|
|
}
|
|
|
|
void turn(){
|
|
trace();
|
|
lance(0);
|
|
if(distance - modeChangedDistance > TURN_DISTANCE){
|
|
modeSet();
|
|
}
|
|
}
|
|
|
|
void attack(){
|
|
switch(target){
|
|
default:
|
|
case TARGET_NONE:
|
|
trace();
|
|
modeSet();
|
|
break;
|
|
case TARGET_PARALLEL_1: //平行標的1
|
|
trace();
|
|
lance(LANCE_ANGLE_PARALLEL_1);
|
|
if(distance - modeChangedDistance > 24){ //マーカーから少し進んで、叩く
|
|
lance(LANCE_ANGLE_PARALLEL_1+8);
|
|
modeSet();
|
|
}
|
|
break;
|
|
case TARGET_PARALLEL_2: //平行標的2
|
|
trace();
|
|
lance(LANCE_ANGLE_PARALLEL_2);
|
|
if(distance - modeChangedDistance > 26){ //マーカーから少し進んで、叩く
|
|
lance(LANCE_ANGLE_PARALLEL_2+8);
|
|
modeSet();
|
|
}
|
|
break;
|
|
case TARGET_VERTICAL_R: //右垂直標
|
|
trace();
|
|
lance(LANCE_ANGLE_VERTICAL_R);
|
|
modeSet();
|
|
break;
|
|
case TARGET_VERTICAL_L: //左垂直標
|
|
trace();
|
|
lance(LANCE_ANGLE_VERTICAL_L);
|
|
modeSet();
|
|
break;
|
|
case TARGET_CYLINDER: //円筒標的
|
|
trace();
|
|
modeSet();
|
|
break;
|
|
}
|
|
}
|
|
|
|
void rot(){
|
|
distance++;
|
|
}
|
|
|
|
void setup(){
|
|
//サーボ
|
|
steeringServo.attach(PIN_SERVO_STEERING, STEERING_MIN, STEERGING_MAX);
|
|
lanceServo.attach(PIN_SERVO_LANCE, LANCE_MIN, LANCE_MAX);
|
|
steering(0);
|
|
lance(0);
|
|
//モーター
|
|
motorL.attach(PIN_MOTOR_LEFT);
|
|
motorR.attach(PIN_MOTOR_RIGHT);
|
|
motorL.mode(STOP);
|
|
motorR.mode(STOP);
|
|
//センサー
|
|
sensorInit();
|
|
//エンコーダー
|
|
pinMode(PIN_ROT, INPUT);
|
|
attachInterrupt(0, rot, FALLING);
|
|
/* 3秒停止 */
|
|
stop();
|
|
delay(3000);
|
|
/* スタート */
|
|
modeSet();
|
|
}
|
|
|
|
void loop(){
|
|
sensorRead();
|
|
switch(mode){
|
|
default:
|
|
case MODE_STOP: //停止
|
|
stop();
|
|
break;
|
|
case MODE_STRAIGHT: //まっすぐ進む
|
|
straight();
|
|
break;
|
|
case MODE_TURN: //左に曲がる
|
|
turn();
|
|
break;
|
|
case MODE_ATTACK: //標的を狙う
|
|
attack();
|
|
break;
|
|
}
|
|
}
|