296 lines
8.2 KiB
C++
296 lines
8.2 KiB
C++
/*
|
|
競技用ランサーロボット
|
|
半蔵 4.1
|
|
(C)2014 kou029w - MIT License
|
|
*/
|
|
|
|
#include <MsTimer2.h>
|
|
#include <Tone.h>
|
|
#include <Servo.h>
|
|
#include "Motor.h"
|
|
|
|
/* 標的の直前にあるマーカーを見つけ出す範囲 : {最小, 最大} */
|
|
struct range_t{
|
|
unsigned long min;
|
|
unsigned long max;
|
|
};
|
|
|
|
/* 標的のためのランスの角度 : {before[度], after[度]} */
|
|
struct lanceAngle_t{
|
|
int before;
|
|
int after;
|
|
};
|
|
|
|
/* 標的の情報 */
|
|
struct target_t{
|
|
struct range_t range;
|
|
unsigned long way; //マーカーから標的までの距離
|
|
struct lanceAngle_t lanceAngle;
|
|
};
|
|
|
|
/**************************************/
|
|
/* 動作パラメーター */
|
|
|
|
unsigned long lapDistance = 28500; //ロータリーエンコーダーのカウント数/周
|
|
unsigned long turnDistance = 4650; //カーブ中でのカウント数
|
|
|
|
struct target_t targetA = {{lapDistance/2, lapDistance*0.6 }, 1500, { 29, 0}};
|
|
struct target_t targetB = {{lapDistance/2, lapDistance*0.75}, 1500, {-32, 0}};
|
|
struct target_t targetC = {{lapDistance*0.6, lapDistance*0.85}, 1500, { 34, 0}};
|
|
struct target_t targetD = {{lapDistance*0.75, lapDistance*0.85}, 1500, {-42, 0}};
|
|
struct target_t targetE = {{0, lapDistance/8 }, 800, { 40, 48}};
|
|
struct target_t targetF = {{lapDistance/8, lapDistance/4 }, 1000, { 61, 69}};
|
|
|
|
int laps = 1; //最大周回数
|
|
bool silent = true; //サイレントモード(true:ブザーを鳴らさない)
|
|
|
|
int steeringAngleMin = -30; //度
|
|
int steeringAngleMax = 30; //度
|
|
|
|
int speedMax = 100; //最大速度
|
|
|
|
float steeringKP = 3.8;
|
|
float steeringKI = 0.0;
|
|
float steeringKD = 0.0;
|
|
|
|
float speedKP = 50.0;
|
|
float speedKI = 0.0;
|
|
float speedKD = 0.0;
|
|
|
|
int errorCountThreshold = 5000; //最大読み取りエラー回数(これを超えると停止する)
|
|
|
|
/**************************************/
|
|
/* ピンの設定 */
|
|
|
|
// Servo : SC-1267SG
|
|
unsigned int steeringServoCenter = 1810; //us
|
|
unsigned int steeringServoMin = (steeringServoCenter-780); //us
|
|
unsigned int steeringServoMax = (steeringServoCenter+780); //us
|
|
|
|
// Servo : SC-0352
|
|
unsigned int lanceServoCenter = 1530; //us
|
|
unsigned int lanceServoMin = (lanceServoCenter-780); //us
|
|
unsigned int lanceServoMax = (lanceServoCenter+780); //us
|
|
|
|
unsigned char buzzerPin = 3;
|
|
unsigned char rotPin = 2;
|
|
|
|
unsigned char numSensors = 8;
|
|
unsigned char sensorPin[] = {19,18,17,16,15,14,13,12}; //右端から順番に左端へ
|
|
|
|
unsigned char lineSensorMask = 0b01111110;
|
|
unsigned char markerSensorMask = 0b10000001;
|
|
|
|
unsigned char steeringServoPin = 9;
|
|
unsigned char lanceServoPin = 10;
|
|
|
|
unsigned char motorLeftEnablePin = 5;
|
|
unsigned char motorLeftDirectionPin = 4;
|
|
unsigned char motorRightEnablePin = 6;
|
|
unsigned char motorRightDirectionPin = 7;
|
|
|
|
/**************************************/
|
|
/* グローバル変数 */
|
|
|
|
Servo steeringServo;
|
|
Servo lanceServo;
|
|
Motor motorR;
|
|
Motor motorL;
|
|
|
|
//ラインが1、地面が0、LSBが左端、MSBが右端
|
|
unsigned char sensor = 0x00;
|
|
|
|
int linePosition = 0;
|
|
int lineDifferential = 0;
|
|
long lineIntegral = 0;
|
|
|
|
// 右のモーターと左のモーターの速度差 : (右のモーター)-(左のモーター)
|
|
int speed = 0;
|
|
|
|
// ステアリングの角度
|
|
long angle = 0; // 度
|
|
|
|
int errorCount = 0;
|
|
|
|
volatile unsigned long distance = 0;
|
|
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;
|
|
|
|
/**************************************/
|
|
|
|
void setup(){
|
|
//サーボ
|
|
steeringServo.attach(steeringServoPin, steeringServoMin, steeringServoMax);
|
|
lanceServo.attach(lanceServoPin, lanceServoMin, lanceServoMax);
|
|
steering(0);
|
|
lance(0);
|
|
//駆動用モーター
|
|
motorL.attach(motorLeftEnablePin, motorLeftDirectionPin);
|
|
motorR.attach(motorRightEnablePin, motorRightDirectionPin);
|
|
motorL.mode(STOP);
|
|
motorR.mode(STOP);
|
|
//センサー
|
|
sensorInit();
|
|
//エンコーダー
|
|
pinMode(rotPin, INPUT);
|
|
attachInterrupt(0, rot, RISING);
|
|
//ブザー
|
|
pinMode(buzzerPin, OUTPUT);
|
|
if(!silent) startBeep();
|
|
/* 3秒停止 */
|
|
motorL.mode(STOP);
|
|
motorR.mode(STOP);
|
|
delay(3000);
|
|
/* スタート */
|
|
mode = MODE_STRAIGHT;
|
|
MsTimer2::set(1, run);
|
|
MsTimer2::start();
|
|
}
|
|
|
|
void loop(){
|
|
sensorRead();
|
|
}
|
|
|
|
/* 車体の動作を決定する(1kHz) */
|
|
void run(){
|
|
switch(mode){
|
|
default:
|
|
case MODE_STOP: //停止
|
|
MsTimer2::stop();
|
|
steeringServo.detach();
|
|
motorL.mode(STOP);
|
|
motorR.mode(STOP);
|
|
if(!silent) stopBeep();
|
|
while(1){
|
|
;
|
|
};
|
|
break;
|
|
case MODE_STRAIGHT: //まっすぐ進む
|
|
trace();
|
|
modeSet();
|
|
break;
|
|
case MODE_TURN: //左に曲がる
|
|
trace();
|
|
lance(0);
|
|
if(distance - modeChangedDistance > turnDistance){
|
|
mode = MODE_STRAIGHT;
|
|
}
|
|
break;
|
|
case MODE_ATTACK_E: //平行標的1
|
|
trace();
|
|
lance(targetE.lanceAngle.before);
|
|
if(distance - modeChangedDistance > targetE.way){ //マーカーから少し進んで、叩く
|
|
lance(targetE.lanceAngle.after);
|
|
mode = MODE_STRAIGHT;
|
|
}
|
|
break;
|
|
case MODE_ATTACK_F: //平行標的2
|
|
trace();
|
|
lance(targetF.lanceAngle.before);
|
|
if(distance - modeChangedDistance > targetF.way){ //マーカーから少し進んで、叩く
|
|
lance(targetF.lanceAngle.after);
|
|
mode = MODE_STRAIGHT;
|
|
}
|
|
break;
|
|
case MODE_ATTACK_A: //右垂直標
|
|
trace();
|
|
lance(targetA.lanceAngle.before);
|
|
if(distance - modeChangedDistance > targetA.way){ //マーカーから少し進んで、まっすぐに戻す
|
|
lance(targetA.lanceAngle.after);
|
|
mode = MODE_STRAIGHT;
|
|
}
|
|
break;
|
|
case MODE_ATTACK_C: //右垂直標
|
|
trace();
|
|
lance(targetC.lanceAngle.before);
|
|
if(distance - modeChangedDistance > targetC.way){ //マーカーから少し進んで、まっすぐに戻す
|
|
lance(targetC.lanceAngle.after);
|
|
mode = MODE_STRAIGHT;
|
|
}
|
|
break;
|
|
case MODE_ATTACK_B: //左垂直標
|
|
trace();
|
|
lance(targetB.lanceAngle.before);
|
|
if(distance - modeChangedDistance > targetB.way){ //マーカーから少し進んで、まっすぐに戻す
|
|
lance(targetB.lanceAngle.after);
|
|
mode = MODE_STRAIGHT;
|
|
}
|
|
break;
|
|
case MODE_ATTACK_D: //左垂直標
|
|
trace();
|
|
lance(targetD.lanceAngle.before);
|
|
if(distance - modeChangedDistance > targetD.way){ //マーカーから少し進んで、まっすぐに戻す
|
|
lance(targetD.lanceAngle.after);
|
|
mode = MODE_STRAIGHT;
|
|
}
|
|
break;
|
|
case MODE_ATTACK_CYLINDER: //円筒標的
|
|
mode = MODE_STRAIGHT;
|
|
break;
|
|
}
|
|
}
|
|
|
|
/* センサーを読んで、ライントレースする */
|
|
void trace(){
|
|
scanSensor();
|
|
|
|
speed = speedPIDControl();
|
|
speed = constrain(speed, -speedMax, speedMax);
|
|
if(speed<0){
|
|
motorL.speed(speedMax + speed);
|
|
motorR.speed(speedMax);
|
|
}else{
|
|
motorL.speed(speedMax);
|
|
motorR.speed(speedMax - speed);
|
|
}
|
|
|
|
angle = steeringPIDControl();
|
|
angle = constrain(angle, steeringAngleMin, steeringAngleMax);
|
|
steering(angle);
|
|
}
|
|
|
|
/* モードを決定する */
|
|
void modeSet(){
|
|
if(distance > lapDistance*laps || errorCount > errorCountThreshold){
|
|
mode = MODE_STOP;
|
|
return;
|
|
}
|
|
unsigned long way = distance%lapDistance;
|
|
switch(sensor&markerSensorMask){
|
|
default:
|
|
case 0b00000000:
|
|
mode = MODE_STRAIGHT;
|
|
break;
|
|
case 0b10000001:
|
|
modeChangedDistance = distance;
|
|
mode = MODE_TURN;
|
|
break;
|
|
case 0b00000001: //E,F,A,C
|
|
modeChangedDistance = distance;
|
|
if(targetE.range.min <= way && way < targetE.range.max) mode = MODE_ATTACK_E;
|
|
if(targetF.range.min <= way && way < targetF.range.max) mode = MODE_ATTACK_F;
|
|
if(targetA.range.min <= way && way < targetA.range.max) mode = MODE_ATTACK_A;
|
|
if(targetC.range.min <= way && way < targetC.range.max) mode = MODE_ATTACK_C;
|
|
break;
|
|
case 0b10000000: //B,D
|
|
modeChangedDistance = distance;
|
|
if(targetB.range.min <= way && way < targetB.range.max) mode = MODE_ATTACK_B;
|
|
if(targetD.range.min <= way && way < targetD.range.max) mode = MODE_ATTACK_D;
|
|
break;
|
|
}
|
|
}
|