hanzo/hanzo4.ino
2013-09-26 22:09:41 +09:00

273 lines
6.8 KiB
C++

/*
競技用ランサーロボット
半蔵 4.0
(C)2013 kou029w - MIT License
*/
#include <Tone.h>
#include <Servo.h>
#include "Motor.h"
const unsigned long LAP_DISTANCE = 28500; //ロータリーエンコーダーのカウント数/周
const unsigned long TURN_DISTANCE = 2000; //カーブ中でのカウント数
const int LANCE_ANGLE_PARALLEL_1 = 55; //度
const int LANCE_ANGLE_PARALLEL_2 = 90 - 50; //度
const int LANCE_ANGLE_VERTICAL_L = 90 - 120; //度
const int LANCE_ANGLE_VERTICAL_R = 90 - 62; //度
const unsigned char SPEED_DEFAULT = 127;
//const float STEERING_KP = 3.8;
// 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
//Servo : SC-0352
const unsigned int LANCE_CENTER = 1450; //us
const unsigned int LANCE_MIN = (LANCE_CENTER-780); //us
const unsigned int LANCE_MAX = (LANCE_CENTER+780); //us
const unsigned char PIN_BUZZER = 3;
const 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; //左端
const unsigned char PIN_SERVO_STEERING = 9;
const 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;
const unsigned char MASK_LINE = 0b01111110;
const unsigned char MASK_MARKER = 0b10000001;
/**************************************/
Servo steeringServo;
Servo lanceServo;
Motor motorR;
Motor motorL;
//ラインが1、地面が0、LSBが左端、MSBが右端
unsigned char sensor = 0x00;
int errorCount = 0;
int speed = SPEED_DEFAULT;
volatile unsigned long distance = 0;
unsigned long modeChangedDistance = 0;
/**************************************/
enum mode_t {
MODE_STOP, //停止
MODE_STRAIGHT, //まっすぐ進む
MODE_TURN, //左に曲がる
MODE_ATTACK_PARALLEL_1, //平行標的1
MODE_ATTACK_PARALLEL_2, //平行標的2
MODE_ATTACK_VERTICAL_R, //右垂直標的
MODE_ATTACK_VERTICAL_L, //左垂直標的
MODE_ATTACK_CYLINDER //円筒標的
}
mode;
/**************************************/
/* センサーを読んで、車体の動作を決定する */
void trace(){
switch(sensor&MASK_LINE){
case 0b01000000:
steering(-22);
motorL.speed(-speed/2);
motorR.speed(speed/2);
break;
case 0b01100000:
steering(-20);
motorL.speed(-speed/2);
motorR.speed(speed/2);
break;
case 0b00100000:
steering(-18);
motorL.speed(-speed/2);
motorR.speed(speed/2);
break;
case 0b00110000:
steering(-2);
motorL.speed(speed);
motorR.speed(speed);
break;
case 0b00010000:
steering(-1);
motorL.speed(speed);
motorR.speed(speed);
break;
case 0b00011000:
steering(0);
motorL.speed(speed);
motorR.speed(speed);
break;
case 0b00001000:
steering(1);
motorL.speed(speed);
motorR.speed(speed);
break;
case 0b00001100:
steering(2);
motorL.speed(speed);
motorR.speed(speed);
break;
case 0b00000100:
steering(3);
motorL.speed(0);
motorR.speed(0);
break;
case 0b00000110:
steering(4);
motorL.speed(0);
motorR.speed(0);
break;
case 0b00000010:
steering(5);
motorL.speed(0);
motorR.speed(0);
break;
}
}
/* モードを決定する */
void modeSet(){
if(sensor == 0) errorCount++;
else errorCount = 0;
if(distance > LAP_DISTANCE*5 || errorCount > 10000){
mode = MODE_STOP;
return;
}
switch(sensor&MASK_MARKER){
default:
case 0b00000000:
mode = MODE_STRAIGHT;
break;
case 0b10000001:
modeChangedDistance = distance;
mode = MODE_TURN;
break;
case 0b00000001:
modeChangedDistance = distance;
if(distance%LAP_DISTANCE < 1429){ //まずは、E的をねらいたい
mode = MODE_ATTACK_PARALLEL_1;
}else if(distance%LAP_DISTANCE < 3333){ //次に、F的をねらいたい
mode = MODE_ATTACK_PARALLEL_2;
}else{ //後半は垂直標的をねらいたい
mode = MODE_ATTACK_VERTICAL_R;
}
break;
case 0b10000000:
modeChangedDistance = distance;
if(distance%LAP_DISTANCE > 5000) mode = MODE_ATTACK_VERTICAL_L;
break;
}
}
void setup(){
// Serial.begin(9600);
//サーボ
steeringServo.attach(PIN_SERVO_STEERING, STEERING_MIN, STEERING_MAX);
lanceServo.attach(PIN_SERVO_LANCE, LANCE_MIN, LANCE_MAX);
steering(0);
lance(0);
//モーター
motorL.attach(PIN_MOTOR_LEFT_E, PIN_MOTOR_LEFT_M);
motorR.attach(PIN_MOTOR_RIGHT_E, PIN_MOTOR_RIGHT_M);
motorL.mode(STOP);
motorR.mode(STOP);
//センサー
sensorInit();
//エンコーダー
pinMode(PIN_ROT, INPUT);
attachInterrupt(0, rot, RISING);
//ブザー
pinMode(PIN_BUZZER, OUTPUT);
startBeep();
/* 3秒停止 */
motorL.mode(STOP);
motorR.mode(STOP);
delay(3000);
/* スタート */
mode = MODE_STRAIGHT;
}
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_PARALLEL_1: //平行標的1
trace();
lance(LANCE_ANGLE_PARALLEL_1);
if(distance - modeChangedDistance > 1200){ //マーカーから少し進んで、叩く
lance(LANCE_ANGLE_PARALLEL_1 + 10);
mode = MODE_STRAIGHT;
}
break;
case MODE_ATTACK_PARALLEL_2: //平行標的2
trace();
lance(LANCE_ANGLE_PARALLEL_2);
if(distance - modeChangedDistance > 1200){ //マーカーから少し進んで、叩く
lance(LANCE_ANGLE_PARALLEL_2 + 10);
mode = MODE_STRAIGHT;
}
break;
case MODE_ATTACK_VERTICAL_R: //右垂直標
trace();
lance(LANCE_ANGLE_VERTICAL_R);
if(distance - modeChangedDistance > 300){ //マーカーから少し進んで、まっすぐに戻す
lance(0);
mode = MODE_STRAIGHT;
}
break;
case MODE_ATTACK_VERTICAL_L: //左垂直標
trace();
lance(LANCE_ANGLE_VERTICAL_L);
if(distance - modeChangedDistance > 300){ //マーカーから少し進んで、まっすぐに戻す
lance(0);
mode = MODE_STRAIGHT;
}
break;
case MODE_ATTACK_CYLINDER: //円筒標的
trace();
mode = MODE_STRAIGHT;
break;
}
}