364 lines
8 KiB
Text
364 lines
8 KiB
Text
/*
|
|
競技用ランサーロボット
|
|
半蔵 1.0
|
|
(c)2011 kou029w - MIT License [http://kou029w.appspot.com/mit-license.txt]
|
|
*/
|
|
|
|
/*
|
|
<前>
|
|
|
|
[0 1 2 3 4 5]
|
|
|
|
|
|
|
|
[]---*---[]
|
|
| ,
|
|
|/
|
|
[]---#---[]
|
|
|
|
*/
|
|
|
|
#define PIN_ANALOG_SENSOR_0 5
|
|
#define PIN_ANALOG_SENSOR_1 4
|
|
#define PIN_ANALOG_SENSOR_2 3
|
|
#define PIN_ANALOG_SENSOR_3 2
|
|
#define PIN_ANALOG_SENSOR_4 1
|
|
#define PIN_ANALOG_SENSOR_5 0
|
|
|
|
#define PIN_SERVO_HANDLE 10
|
|
#define PIN_SERVO_LANCE 9
|
|
#define PIN_MOTOR_RIGHT_SPEED 3
|
|
#define PIN_MOTOR_RIGHT_0 2
|
|
#define PIN_MOTOR_RIGHT_1 8
|
|
#define PIN_MOTOR_LEFT_SPEED 5
|
|
#define PIN_MOTOR_LEFT_0 7
|
|
#define PIN_MOTOR_LEFT_1 6 // 0:HIGH, 1:LOW => 正転 / 0:LOW, 1:HIGH => 逆転
|
|
|
|
#define STOP 0x00
|
|
#define GO 0b01
|
|
#define BACK 0b10
|
|
#define BRAKE 0b11
|
|
|
|
#define SPEED_MAX 255
|
|
#define SPEED_DEFAULT 255
|
|
|
|
#define HANDLE_DEFAULT 87 //度
|
|
#define HANDLE_MODE_LEFT -25 //度
|
|
#define HANDLE_MODE_RIGHT 20 //度
|
|
|
|
#define LANCE_DEFAULT 95 //度
|
|
|
|
#define SENSOR_LIMIT 95 //センサーのしきい値
|
|
// センサー < SENSOR_LIMIT => 白
|
|
|
|
#define MODE_STOP 0x00
|
|
#define MODE_TRACE 0x10
|
|
#define MODE_RIGHT 0x21 // 右カーブ
|
|
#define MODE_LEFT 0x22 // 左カーブ
|
|
|
|
#define TARGET_NONE 0x40
|
|
#define TARGET_PARALLEL 0x41
|
|
#define TARGET_VERTICAL 0x42
|
|
#define TARGET_CYLINDER 0x43
|
|
|
|
#define MASK_MODE_TRACE 0b011110
|
|
#define MASK_MODE_RIGHT 0b001000
|
|
#define MASK_MODE_LEFT 0b000100
|
|
#define MASK_CHECK_MARKER 0b100001
|
|
#define MASK_CHECK_MARKER_RIGHT 0b000001
|
|
#define MASK_CHECK_MARKER_LEFT 0b100000
|
|
|
|
#define TIMER_INTERVAL 20 // timer()の実行間隔(ms)
|
|
|
|
//#define DEBUG //デバッグ有効(30秒停止機能)
|
|
//#define LINE_BLACK //白地・黒ライン有効
|
|
|
|
/**************************************/
|
|
|
|
#include <Servo.h>
|
|
// #include <MsTimer2.h>
|
|
|
|
/**************************************/
|
|
|
|
Servo servo_handle;
|
|
Servo servo_lance;
|
|
|
|
byte mode;
|
|
byte sensor_old;
|
|
//byte marker_old;
|
|
//byte target;
|
|
byte lance_old;
|
|
//byte marker;
|
|
|
|
/**************************************/
|
|
|
|
byte sensor(){
|
|
byte sensor_result = 0;
|
|
|
|
#ifdef LINE_BLACK
|
|
if(analogRead(PIN_ANALOG_SENSOR_0) >= SENSOR_LIMIT){ //黒
|
|
sensor_result += 0b100000 ;
|
|
}
|
|
if(analogRead(PIN_ANALOG_SENSOR_1) >= SENSOR_LIMIT){
|
|
sensor_result += 0b010000 ;
|
|
}
|
|
if(analogRead(PIN_ANALOG_SENSOR_2) >= SENSOR_LIMIT){
|
|
sensor_result += 0b001000 ;
|
|
}
|
|
if(analogRead(PIN_ANALOG_SENSOR_3) >= SENSOR_LIMIT){
|
|
sensor_result += 0b000100 ;
|
|
}
|
|
if(analogRead(PIN_ANALOG_SENSOR_4) >= SENSOR_LIMIT){
|
|
sensor_result += 0b000010 ;
|
|
}
|
|
if(analogRead(PIN_ANALOG_SENSOR_5) >= SENSOR_LIMIT){
|
|
sensor_result += 0b000001 ;
|
|
}
|
|
#else
|
|
if(analogRead(PIN_ANALOG_SENSOR_0) < SENSOR_LIMIT){ //白
|
|
sensor_result += 0b100000 ;
|
|
}
|
|
if(analogRead(PIN_ANALOG_SENSOR_1) < SENSOR_LIMIT){
|
|
sensor_result += 0b010000 ;
|
|
}
|
|
if(analogRead(PIN_ANALOG_SENSOR_2) < SENSOR_LIMIT){
|
|
sensor_result += 0b001000 ;
|
|
}
|
|
if(analogRead(PIN_ANALOG_SENSOR_3) < SENSOR_LIMIT){
|
|
sensor_result += 0b000100 ;
|
|
}
|
|
if(analogRead(PIN_ANALOG_SENSOR_4) < SENSOR_LIMIT){
|
|
sensor_result += 0b000010 ;
|
|
}
|
|
if(analogRead(PIN_ANALOG_SENSOR_5) < SENSOR_LIMIT){
|
|
sensor_result += 0b000001 ;
|
|
}
|
|
#endif
|
|
|
|
return sensor_result;
|
|
}
|
|
|
|
/*
|
|
bool check_marker(){
|
|
bool check_marker_result = false;
|
|
if(sensor(MASK_CHECK_MARKER)) check_marker_result = true;
|
|
return check_marker_result;
|
|
}
|
|
|
|
bool check_marker_right(){
|
|
bool check_marker_result = false;
|
|
if(sensor(MASK_CHECK_MARKER_RIGHT)) check_marker_result = true;
|
|
return check_marker_result;
|
|
}
|
|
|
|
bool check_marker_left(){
|
|
bool check_marker_result = false;
|
|
if(sensor(MASK_CHECK_MARKER_LEFT)) check_marker_result = true;
|
|
return check_marker_result;
|
|
}
|
|
*/
|
|
|
|
void motorSpeed(byte motor_left_speed, byte motor_right_speed){
|
|
analogWrite(PIN_MOTOR_LEFT_SPEED, motor_left_speed);
|
|
analogWrite(PIN_MOTOR_RIGHT_SPEED, motor_right_speed);
|
|
}
|
|
|
|
void motorMode(byte motor_left_mode, byte motor_right_mode){
|
|
digitalWrite(PIN_MOTOR_LEFT_0, ( motor_left_mode >> 0 ) & 0x01 );
|
|
digitalWrite(PIN_MOTOR_LEFT_1, ( motor_left_mode >> 1 ) & 0x01 );
|
|
digitalWrite(PIN_MOTOR_RIGHT_0, ( motor_right_mode >> 0 ) & 0x01 );
|
|
digitalWrite(PIN_MOTOR_RIGHT_1, ( motor_right_mode >> 1 ) & 0x01 );
|
|
}
|
|
|
|
void handle(char handle_angle){
|
|
servo_handle.write(HANDLE_DEFAULT + handle_angle);
|
|
}
|
|
|
|
void lance(char lance_angle){
|
|
servo_lance.write(LANCE_DEFAULT - lance_angle);
|
|
}
|
|
|
|
#ifdef DEBUG
|
|
void timer(unsigned long millis_now){
|
|
if(millis_now > 30 * 1000){
|
|
mode = MODE_STOP;
|
|
}
|
|
}
|
|
#endif
|
|
|
|
void setup() {
|
|
|
|
pinMode(PIN_ANALOG_SENSOR_0, INPUT);
|
|
pinMode(PIN_ANALOG_SENSOR_1, INPUT);
|
|
pinMode(PIN_ANALOG_SENSOR_2, INPUT);
|
|
pinMode(PIN_ANALOG_SENSOR_3, INPUT);
|
|
pinMode(PIN_ANALOG_SENSOR_4, INPUT);
|
|
pinMode(PIN_ANALOG_SENSOR_5, INPUT);
|
|
|
|
pinMode(PIN_MOTOR_LEFT_SPEED, OUTPUT);
|
|
pinMode(PIN_MOTOR_LEFT_0, OUTPUT);
|
|
pinMode(PIN_MOTOR_LEFT_1, OUTPUT);
|
|
|
|
pinMode(PIN_MOTOR_RIGHT_SPEED, OUTPUT);
|
|
pinMode(PIN_MOTOR_RIGHT_0, OUTPUT);
|
|
pinMode(PIN_MOTOR_RIGHT_1, OUTPUT);
|
|
|
|
motorSpeed(0, 0);
|
|
motorMode(STOP, STOP);
|
|
|
|
servo_handle.attach(PIN_SERVO_HANDLE);
|
|
handle(0);
|
|
|
|
servo_lance.attach(PIN_SERVO_LANCE);
|
|
lance(0);
|
|
|
|
mode = MODE_STOP;
|
|
|
|
// target = TARGET_NONE;
|
|
// marker =0;
|
|
|
|
/*
|
|
MsTimer2::set(TIMER_INTERVAL, timer);
|
|
MsTimer2::start();
|
|
*/
|
|
|
|
//--
|
|
|
|
delay(1000);
|
|
// Serial.begin(9600);
|
|
mode = MODE_TRACE;
|
|
handle(0);
|
|
motorMode(GO, GO);
|
|
motorSpeed(SPEED_DEFAULT, SPEED_DEFAULT);
|
|
|
|
}
|
|
|
|
/**************************************/
|
|
|
|
void loop() {
|
|
sensor_old = sensor();
|
|
|
|
switch(mode){
|
|
case MODE_STOP:
|
|
lance(0);
|
|
handle(0);
|
|
motorMode(BRAKE, BRAKE);
|
|
motorSpeed(0, 0);
|
|
break;
|
|
|
|
case MODE_TRACE:
|
|
|
|
static unsigned long millis_lance = 0;
|
|
if(millis() - millis_lance > 70){
|
|
millis_lance = millis();
|
|
|
|
switch(lance_old){
|
|
case 39:
|
|
lance_old = 29;
|
|
break;
|
|
case 29:
|
|
lance_old = 39;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if( (sensor_old >> 5) & 0x01){
|
|
lance_old = -33;
|
|
}
|
|
|
|
if( (sensor_old >> 0) & 0x01){
|
|
lance_old = 39;
|
|
}
|
|
|
|
lance(lance_old);
|
|
// handle(0);
|
|
// motorMode(GO, GO);
|
|
// motorSpeed(SPEED_DEFAULT, SPEED_DEFAULT);
|
|
/*
|
|
switch(sensor(MASK_CHECK_MARKER)){
|
|
case 0b000001:
|
|
marker = 0x01;
|
|
break;
|
|
case 0b100000:
|
|
marker = 0x02;
|
|
break;
|
|
}
|
|
*/
|
|
switch(sensor_old){
|
|
// case 0b000000:
|
|
// handle(0);
|
|
// motorMode(GO, GO);
|
|
// motorSpeed(SPEED_DEFAULT, SPEED_DEFAULT);
|
|
// break;
|
|
|
|
case 0b001000:
|
|
handle(-3);
|
|
motorMode(GO, GO);
|
|
motorSpeed(SPEED_DEFAULT, SPEED_DEFAULT);
|
|
break;
|
|
|
|
case 0b000100:
|
|
handle(3);
|
|
motorMode(GO, GO);
|
|
motorSpeed(SPEED_DEFAULT,SPEED_DEFAULT);
|
|
break;
|
|
|
|
case 0b010000:
|
|
// delay(0);
|
|
mode = MODE_LEFT;
|
|
// handle(HANDLE_MODE_LEFT);
|
|
// motorMode(GO, GO);
|
|
// motorSpeed(SPEED_DEFAULT*0.7, SPEED_DEFAULT);
|
|
break;
|
|
|
|
case 0b000010:
|
|
handle(12);
|
|
motorMode(GO, GO);
|
|
motorSpeed(SPEED_DEFAULT,SPEED_DEFAULT);
|
|
//mode = MODE_RIGHT;
|
|
break;
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
case MODE_LEFT:
|
|
|
|
switch(sensor_old){
|
|
case 0b010000:
|
|
handle(HANDLE_MODE_LEFT);
|
|
motorMode(BRAKE, GO);
|
|
motorSpeed(0, SPEED_DEFAULT);
|
|
break;
|
|
/* case 0b001000:
|
|
handle(HANDLE_MODE_LEFT);
|
|
motorMode(GO, GO);
|
|
motorSpeed(SPEED_DEFAULT*0.7, SPEED_DEFAULT);
|
|
break;
|
|
*/
|
|
case 0b000100:
|
|
mode = MODE_TRACE;
|
|
break;
|
|
}
|
|
break;
|
|
/*
|
|
case MODE_RIGHT:
|
|
handle(HANDLE_MODE_RIGHT);
|
|
motorMode(GO, GO);
|
|
motorSpeed(SPEED_DEFAULT, SPEED_DEFAULT*0.8);
|
|
switch(sensor(MASK_MODE_RIGHT)){
|
|
case 0b001000:
|
|
mode = MODE_TRACE;
|
|
break;
|
|
}
|
|
break;
|
|
*/
|
|
}
|
|
#ifdef DEBUG
|
|
static unsigned long millis_old = 0;
|
|
if(millis() - millis_old > TIMER_INTERVAL){
|
|
millis_old = millis();
|
|
timer(millis_old);
|
|
}
|
|
#endif
|
|
}
|
|
|