319 lines
8 KiB
C++
319 lines
8 KiB
C++
/*
|
|
競技用ランサーロボット
|
|
半蔵 4.1
|
|
(C)2014 kou029w - MIT License
|
|
*/
|
|
|
|
#include <MsTimer2.h>
|
|
#include <Tone.h>
|
|
#include <Servo.h>
|
|
#include "Motor.h"
|
|
|
|
unsigned long LAP_DISTANCE = 28500; //ロータリーエンコーダーのカウント数/周
|
|
unsigned long TURN_DISTANCE = 4650; //カーブ中でのカウント数
|
|
|
|
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; //度
|
|
|
|
int SPEED_DEFAULT = 100;
|
|
|
|
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
|
|
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
|
|
unsigned int LANCE_CENTER = 1530; //us
|
|
unsigned int LANCE_MIN = (LANCE_CENTER-780); //us
|
|
unsigned int LANCE_MAX = (LANCE_CENTER+780); //us
|
|
|
|
unsigned char PIN_BUZZER = 3;
|
|
unsigned char PIN_ROT = 2;
|
|
|
|
unsigned char NUM_SENSORS = 8;
|
|
unsigned char PIN_SENSOR[] = {19,18,17,16,15,14,13,12}; //右端-左端
|
|
|
|
unsigned char PIN_SERVO_STEERING = 9;
|
|
unsigned char PIN_SERVO_LANCE = 10;
|
|
|
|
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;
|
|
|
|
unsigned char MASK_LINE = 0b01111110;
|
|
unsigned char MASK_MARKER = 0b10000001;
|
|
|
|
/**************************************/
|
|
|
|
Servo steeringServo;
|
|
Servo lanceServo;
|
|
Motor motorR;
|
|
Motor motorL;
|
|
|
|
//ラインが1、地面が0、LSBが左端、MSBが右端
|
|
volatile unsigned char sensor = 0x00;
|
|
|
|
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 modeChangedDistance = 0;
|
|
|
|
/**************************************/
|
|
|
|
enum mode_t {
|
|
MODE_STOP, //停止
|
|
MODE_STRAIGHT, //まっすぐ進む
|
|
MODE_TURN, //左に曲がる
|
|
MODE_ATTACK_E, //平行標的1
|
|
MODE_ATTACK_F, //平行標的2
|
|
MODE_ATTACK_A, //右垂直標的
|
|
MODE_ATTACK_B, //左垂直標的
|
|
MODE_ATTACK_C, //右垂直標的
|
|
MODE_ATTACK_D, //左垂直標的
|
|
MODE_ATTACK_CYLINDER //円筒標的
|
|
} mode;
|
|
|
|
/**************************************/
|
|
|
|
/* センサーを読んでラインの位置を返す */
|
|
int scanSensor(){
|
|
sensorRead();
|
|
int pos = 0;
|
|
switch(sensor&MASK_LINE){
|
|
case 0b01000000:
|
|
pos = -5;
|
|
break;
|
|
case 0b01100000:
|
|
pos = -4;
|
|
break;
|
|
case 0b00100000:
|
|
pos = -3;
|
|
break;
|
|
case 0b00110000:
|
|
pos = -2;
|
|
break;
|
|
case 0b00010000:
|
|
pos = -1;
|
|
break;
|
|
case 0b00011000:
|
|
pos = 0;
|
|
break;
|
|
case 0b00001000:
|
|
pos = 1;
|
|
break;
|
|
case 0b00001100:
|
|
pos = 2;
|
|
break;
|
|
case 0b00000100:
|
|
pos = 3;
|
|
break;
|
|
case 0b00000110:
|
|
pos = 4;
|
|
break;
|
|
case 0b00000010:
|
|
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(distance > LAP_DISTANCE || errorCount > 5000){
|
|
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 < LAP_DISTANCE/8){ //まずは、E的をねらいたい
|
|
mode = MODE_ATTACK_E;
|
|
}else if(distance%LAP_DISTANCE < LAP_DISTANCE/4){ //次に、F的をねらいたい
|
|
mode = MODE_ATTACK_F;
|
|
}else if(distance%LAP_DISTANCE > LAP_DISTANCE/2){ //後半は垂直標的をねらいたい
|
|
if(distance%LAP_DISTANCE < LAP_DISTANCE*0.6){
|
|
mode = MODE_ATTACK_A;
|
|
}else if(distance%LAP_DISTANCE < LAP_DISTANCE*0.85){
|
|
mode = MODE_ATTACK_C;
|
|
}
|
|
}
|
|
break;
|
|
case 0b10000000:
|
|
modeChangedDistance = distance;
|
|
if(distance%LAP_DISTANCE < LAP_DISTANCE/2){
|
|
;
|
|
}else if(distance%LAP_DISTANCE < LAP_DISTANCE*0.75){
|
|
mode = MODE_ATTACK_B;
|
|
}else if(distance%LAP_DISTANCE < LAP_DISTANCE*0.85){
|
|
mode = MODE_ATTACK_D;
|
|
}
|
|
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;
|
|
MsTimer2::set(1, trace);
|
|
MsTimer2::start();
|
|
}
|
|
|
|
void loop(){
|
|
scanSensor();
|
|
Serial.println(linePosition[0]);
|
|
}
|