155 lines
3.4 KiB
C++
155 lines
3.4 KiB
C++
/*
|
|
競技用ランサーロボット 源内
|
|
(C)2013 kou029w - MIT License
|
|
*/
|
|
|
|
#include <Tone.h>
|
|
#include <Servo.h>
|
|
#include <QTRSensors.h>
|
|
#include "Motor.h"
|
|
|
|
const unsigned long LAP_DISTANCE = 500; //ロータリーエンコーダーのカウント数/周
|
|
const unsigned long TURN_DISTANCE = 1500; //カーブ中でのカウント数
|
|
|
|
const int LANCE_ANGLE_PARALLEL_1 = 90 - 62; //度
|
|
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 = 100;
|
|
|
|
//Servo : SC-0352
|
|
// 60deg : 1265 us
|
|
// 90deg : 1535 us
|
|
//120deg : 1805 us
|
|
const int LANCE_DEFAULT = 90; //度
|
|
const unsigned int LANCE_MIN = (1620-780); //us
|
|
const unsigned int LANCE_MAX = (1620+780); //us
|
|
|
|
const unsigned char PIN_BUZZER = 3;
|
|
const unsigned char PIN_ROT = 2;
|
|
|
|
const unsigned char PIN_SERVO_LANCE = 9;
|
|
|
|
const unsigned char NUM_SENSORS = 2;
|
|
unsigned char PIN_SENSORS[NUM_SENSORS] = {13, 12}; // 左端...右端
|
|
|
|
const unsigned char PIN_MOTOR_LEFT = 6;
|
|
const unsigned char PIN_MOTOR_RIGHT = 5;
|
|
|
|
/**************************************/
|
|
|
|
Servo lanceServo;
|
|
Motor motorL;
|
|
Motor motorR;
|
|
|
|
QTRSensorsRC sensor(PIN_SENSORS, NUM_SENSORS);
|
|
unsigned int sensorValues[NUM_SENSORS];
|
|
|
|
volatile unsigned long distance = 0;
|
|
unsigned long modeChangedDistance = 0;
|
|
|
|
int error_count = 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(){
|
|
error_count++;
|
|
for(int i=0; i<NUM_SENSORS; i++){
|
|
if(sensorValues[i] < 600) error_count = 0;
|
|
}
|
|
|
|
if(sensorValues[0] > 600) motorR.speed(SPEED_DEFAULT*0.6);
|
|
else motorR.speed(SPEED_DEFAULT);
|
|
if(sensorValues[1] > 600) motorL.speed(SPEED_DEFAULT*0.6);
|
|
else motorL.speed(SPEED_DEFAULT);
|
|
}
|
|
|
|
/* ランスを中央からa[度]だけ動かす
|
|
<- - 0 + ->
|
|
/ .\
|
|
/ | \
|
|
[]+--{}--+[]
|
|
*/
|
|
void lance(int a){
|
|
lanceServo.write(LANCE_DEFAULT - a);
|
|
}
|
|
void modeSet(){
|
|
if(distance > LAP_DISTANCE || error_count > 100){
|
|
mode = MODE_STOP;
|
|
return;
|
|
}
|
|
}
|
|
|
|
void rot(){
|
|
distance++;
|
|
}
|
|
|
|
void startBeep(){
|
|
tone(PIN_BUZZER,2000);
|
|
delay(100);
|
|
tone(PIN_BUZZER,1000);
|
|
delay(100);
|
|
noTone(PIN_BUZZER);
|
|
}
|
|
/**************************************/
|
|
|
|
void setup(){
|
|
Serial.begin(9600);
|
|
//サーボ
|
|
lanceServo.attach(PIN_SERVO_LANCE, LANCE_MIN, LANCE_MAX);
|
|
lance(0);
|
|
//モーター
|
|
motorL.attach(PIN_MOTOR_LEFT);
|
|
motorR.attach(PIN_MOTOR_RIGHT);
|
|
motorL.mode(STOP);
|
|
motorR.mode(STOP);
|
|
//エンコーダー
|
|
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);
|
|
sensor.read(sensorValues);
|
|
switch(mode){
|
|
case MODE_STOP: //停止
|
|
motorL.mode(STOP);
|
|
motorR.mode(STOP);
|
|
tone(PIN_BUZZER, 2000);
|
|
delay(2000);
|
|
noTone(PIN_BUZZER);
|
|
while(1){
|
|
;
|
|
};
|
|
case MODE_STRAIGHT: //まっすぐ進む
|
|
trace();
|
|
modeSet();
|
|
break;
|
|
}
|
|
}
|