nebel/hanzo
1
0
Fork 0
Code Issues Pull requests Projects Releases Wiki Activity
hanzo/hanzo4_1.ino

348 lines
9.1 KiB
C++
Raw Blame History

/*
競技用ランサーロボット
半蔵 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 }, 900, { 29, 0}};
struct target_t targetB = {{lapDistance/2, lapDistance*0.75}, 900, {-32, 0}};
struct target_t targetC = {{lapDistance*0.6, lapDistance*0.85}, 900, { 34, 0}};
struct target_t targetD = {{lapDistance*0.75, lapDistance*0.85}, 900, {-42, 0}};
struct target_t targetE = {{1500, 2400 }, 800, { 40, 48}};
struct target_t targetF = {{lapDistance/8, lapDistance/4 }, 800, { 61, 60}};
int laps = 3; //最大周回数
bool silent = false; //サイレントモード(true:ブザーを鳴らさない)
int steeringAngleMin = -30; //度
int steeringAngleMax = 10; //度
int deltaSpeedMax = 500; //最大速度差
int speedMax = 255; //最大速度
int cycle = 1; //動作周波数(ms)
int errorCountThreshold = 500; //最大読み取りエラー回数(これを超えると停止する)
/**************************************/
/* ピンの設定 */
// Servo : SC-1267SG
unsigned int steeringServoCenter = 1815; //us
unsigned int steeringServoMin = (steeringServoCenter-780); //us
unsigned int steeringServoMax = (steeringServoCenter+780); //us
// Servo : SC-0352
unsigned int lanceServoCenter = 1550; //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 deltaSpeed = 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 trace(){
switch(sensor&lineSensorMask){
default:
errorCount++;
return;
case 0b01000000:
speed = speedMax*0.3;
deltaSpeed = -150;
angle = -18;
break;
case 0b01100000:
speed = speedMax*0.5;
deltaSpeed = -100;
angle = -16;
break;
case 0b00100000:
speed = speedMax*0.6;
deltaSpeed = -50;
angle = -14;
break;
case 0b00110000:
speed = speedMax;
deltaSpeed = -25;
angle = -10;
break;
case 0b00010000:
speed = speedMax;
deltaSpeed = 0;
angle = -1;
break;
case 0b00011000:
speed = speedMax;
deltaSpeed = 0;
angle = 0;
break;
case 0b00001000:
speed = speedMax;
deltaSpeed = 0;
angle = 1;
break;
case 0b00001100:
speed = speedMax;
deltaSpeed = 0;
angle = 2;
break;
case 0b00000100:
speed = speedMax;
deltaSpeed = 3;
angle = 3;
break;
case 0b00000110:
speed = speedMax;
deltaSpeed = 4;
angle = 4;
break;
case 0b00000010:
speed = 0;
deltaSpeed = 5;
angle = 5;
break;
}
errorCount = 0;
}
/**************************************/
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);
//ブザー
startBeep();
/* 3秒停止 */
motorL.mode(STOP);
motorR.mode(STOP);
delay(3000);
/* スタート */
mode = MODE_STRAIGHT;
MsTimer2::set(cycle, run);
MsTimer2::start();
}
void loop(){
scanSensor();
}
/* 車体の動作を決定する */
void run(){
if(distance > lapDistance*laps || errorCount > errorCountThreshold){
mode = MODE_STOP;
}
switch(mode){
default:
case MODE_STOP: //停止
MsTimer2::stop();
steeringServo.detach();
motorL.mode(STOP);
motorR.mode(STOP);
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;
}
/* 駆動モーター用 */
deltaSpeed = constrain(deltaSpeed, -deltaSpeedMax, deltaSpeedMax);
if(deltaSpeed<0){
motorL.speed(speed + deltaSpeed);
motorR.speed(speed);
}else{
motorL.speed(speed);
motorR.speed(speed - deltaSpeed);
}
/* ステアリングサーボ用 */
angle = constrain(angle, steeringAngleMin, steeringAngleMax);
steering(angle);
}
/* モードを決定する */
void modeSet(){
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;
}
}
Reference in a new issue View git blame Copy permalink