291 lines
6.4 KiB
C++
291 lines
6.4 KiB
C++
/*
|
|
競技用ランサーロボット
|
|
半蔵 2.0
|
|
(C)2012 kou029w - MIT License [http://kou029w.appspot.com/mit-license.txt]
|
|
*/
|
|
|
|
/*
|
|
<前>
|
|
|
|
[7 654321 0]
|
|
|
|
|
|
|
|
[]---*----[]
|
|
| /
|
|
|/
|
|
[]---#----[]
|
|
*/
|
|
|
|
//#define LINE_BLACK //白地・黒ライン有効
|
|
|
|
#define TARGET_NONE_COUNT 85
|
|
|
|
#define LANCE_ANGLE0 0 //度
|
|
#define LANCE_ANGLE1 43
|
|
#define LANCE_ANGLE2 54
|
|
#define LANCE_ANGLE3 -33
|
|
#define LANCE_ANGLE4 34
|
|
#define LANCE_INTERVAL 70 //ms
|
|
|
|
#define PIN_LED 4
|
|
#define PIN_ROT 2
|
|
|
|
#define PIN_SENSOR_0 12
|
|
#define PIN_SENSOR_1 13
|
|
#define PIN_SENSOR_2 14
|
|
#define PIN_SENSOR_3 15
|
|
#define PIN_SENSOR_4 16
|
|
#define PIN_SENSOR_5 17
|
|
#define PIN_SENSOR_6 18
|
|
#define PIN_SENSOR_7 19
|
|
|
|
#define PIN_SENSOR ( ((PINC<<2) & ~0x03) | ((PINB>>4) & 0x03) )
|
|
|
|
#define PIN_SERVO_HANDLE 10
|
|
#define PIN_SERVO_LANCE 9
|
|
|
|
#define PIN_MOTOR_RIGHT_1 6
|
|
#define PIN_MOTOR_RIGHT_2 8
|
|
#define PIN_MOTOR_LEFT_1 5
|
|
#define PIN_MOTOR_LEFT_2 7 // ex) RIGHT_1:HIGH, RIGHT_2:LOW => 正転 / RIGHT_1:LOW, RIGHT_2:HIGH => 逆転
|
|
|
|
#define STOP 0
|
|
#define GO 1
|
|
#define BACK 2
|
|
#define BRAKE 3
|
|
|
|
#define SPEED_DEFAULT 0xff
|
|
|
|
#define HANDLE_DEFAULT 90 //度
|
|
#define HANDLE_MIN ( 800+30) //ms
|
|
#define HANDLE_MAX (2300+30) //ms
|
|
|
|
#define LANCE_DEFAULT 90 //度
|
|
#define LANCE_MIN 544 //ms
|
|
#define LANCE_MAX 2400 //ms
|
|
|
|
#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 0b01111110
|
|
#define MASK_CHECK_MARKER 0b10000001
|
|
#define MASK_CHECK_MARKER_RIGHT 0b00000001
|
|
#define MASK_CHECK_MARKER_LEFT 0b10000000
|
|
|
|
/**************************************/
|
|
|
|
#include <Servo.h>
|
|
|
|
/**************************************/
|
|
|
|
Servo servo_handle;
|
|
Servo servo_lance;
|
|
|
|
byte mode;
|
|
byte target;
|
|
unsigned int counter_old;
|
|
char handle_angle;
|
|
char lance_angle;
|
|
|
|
/**************************************/
|
|
|
|
void trace(byte sensor){
|
|
switch( sensor & MASK_MODE_TRACE ){
|
|
case 0b01000000:
|
|
handle_angle = -20;
|
|
motorMode(GO, GO, 0x40, 0xff);
|
|
lance_angle = 0;
|
|
break;
|
|
case 0b01100000:
|
|
handle_angle = -16;
|
|
motorMode(GO, GO, 0xff, 0xff);
|
|
lance_angle = 0;
|
|
break;
|
|
case 0b00100000:
|
|
handle_angle = -12;
|
|
motorMode(GO, GO, 0xff, 0xff);
|
|
lance_angle = 0;
|
|
break;
|
|
case 0b00110000:
|
|
handle_angle = -5;
|
|
motorMode(GO, GO, 0xff, 0xff);
|
|
break;
|
|
case 0b00010000:
|
|
handle_angle = -2;
|
|
motorMode(GO, GO, 0xff, 0xff);
|
|
break;
|
|
case 0b00011000:
|
|
handle_angle = 0;
|
|
motorMode(GO, GO, 0xff, 0xff);
|
|
break;
|
|
case 0b00001000:
|
|
handle_angle = 2;
|
|
motorMode(GO, GO, 0xff, 0xff);
|
|
break;
|
|
case 0b00001100:
|
|
handle_angle = 5;
|
|
motorMode(GO, GO, 0xff, 0xff);
|
|
break;
|
|
case 0b00000100:
|
|
handle_angle = 7;
|
|
motorMode(GO, GO, 0xff, 0xff);
|
|
break;
|
|
case 0b00000110:
|
|
handle_angle = 10;
|
|
motorMode(GO, GO, 0xff, 0xff);
|
|
break;
|
|
case 0b00000010:
|
|
handle_angle = 12;
|
|
motorMode(GO, GO, 0xff, 0xff);
|
|
break;
|
|
}
|
|
}
|
|
|
|
void motorMode(byte left_mode, byte right_mode, byte left_speed, byte right_speed){
|
|
analogWrite(PIN_MOTOR_LEFT_1 , left_speed * (left_mode & 0x01) );
|
|
analogWrite(PIN_MOTOR_RIGHT_1, right_speed * (right_mode & 0x01) );
|
|
digitalWrite(PIN_MOTOR_LEFT_2 , left_mode >> 1 );
|
|
digitalWrite(PIN_MOTOR_RIGHT_2, right_mode >> 1 );
|
|
}
|
|
|
|
void handle(char handle_angle){
|
|
servo_handle.write(HANDLE_DEFAULT + handle_angle);
|
|
}
|
|
|
|
void lance(char lance_angle){
|
|
servo_lance.write(LANCE_DEFAULT - lance_angle);
|
|
}
|
|
|
|
/* ロータリーエンコーダーの変化を見る */
|
|
volatile unsigned int counter = 0;
|
|
void count(){
|
|
static byte rot_old = LOW;
|
|
static byte rot;
|
|
rot = digitalRead(PIN_ROT);
|
|
if(rot_old != rot){
|
|
counter++;
|
|
rot_old = rot;
|
|
}
|
|
}
|
|
|
|
void setup() {
|
|
pinMode(PIN_SENSOR_0, INPUT);
|
|
pinMode(PIN_SENSOR_1, INPUT);
|
|
pinMode(PIN_SENSOR_2, INPUT);
|
|
pinMode(PIN_SENSOR_3, INPUT);
|
|
pinMode(PIN_SENSOR_4, INPUT);
|
|
pinMode(PIN_SENSOR_5, INPUT);
|
|
pinMode(PIN_SENSOR_6, INPUT);
|
|
pinMode(PIN_SENSOR_7, INPUT);
|
|
|
|
pinMode(PIN_MOTOR_RIGHT_1, OUTPUT);
|
|
pinMode(PIN_MOTOR_RIGHT_2, OUTPUT);
|
|
pinMode(PIN_MOTOR_LEFT_1 , OUTPUT);
|
|
pinMode(PIN_MOTOR_LEFT_2 , OUTPUT);
|
|
|
|
pinMode(PIN_ROT, INPUT);
|
|
|
|
motorMode(STOP, STOP, 0, 0);
|
|
|
|
servo_handle.attach(PIN_SERVO_HANDLE, HANDLE_MIN, HANDLE_MAX);
|
|
handle(0);
|
|
|
|
servo_lance.attach(PIN_SERVO_LANCE, LANCE_MIN, LANCE_MAX);
|
|
lance(0);
|
|
|
|
mode = MODE_STOP;
|
|
|
|
digitalWrite(PIN_LED, HIGH);
|
|
|
|
delay(1000);
|
|
|
|
/**************************************/
|
|
|
|
digitalWrite(PIN_LED, LOW);
|
|
mode = MODE_TRACE;
|
|
handle(0);
|
|
motorMode(GO, GO, 0xff, 0xff);
|
|
|
|
}
|
|
|
|
/**************************************/
|
|
|
|
void loop(){
|
|
|
|
count(); //ロータリーエンコーダーの変化を見る
|
|
|
|
static byte sensor = 0;
|
|
#ifdef LINE_BLACK //黒ライン
|
|
sensor = PIN_SENSOR;
|
|
#else
|
|
sensor = ~PIN_SENSOR;
|
|
#endif
|
|
|
|
switch(target){
|
|
case TARGET_NONE:
|
|
lance_angle = LANCE_ANGLE0;
|
|
if(counter - counter_old > TARGET_NONE_COUNT ) target = 0;
|
|
break;
|
|
default:
|
|
switch( sensor & MASK_CHECK_MARKER ){
|
|
case 0x81:
|
|
target = TARGET_NONE;
|
|
counter_old = counter;
|
|
break;
|
|
case 0x01:
|
|
lance_angle = LANCE_ANGLE1;
|
|
target = TARGET_PARALLEL;
|
|
break;
|
|
case 0x80:
|
|
lance_angle = LANCE_ANGLE3;
|
|
target = TARGET_VERTICAL;
|
|
break;
|
|
}
|
|
break;
|
|
}
|
|
if( lance_angle == LANCE_ANGLE1 && counter%840 > 420) lance_angle = LANCE_ANGLE4;
|
|
static unsigned long millis_lance = 0;
|
|
if(millis() - millis_lance > LANCE_INTERVAL){
|
|
millis_lance = millis();
|
|
switch(lance_angle){
|
|
case LANCE_ANGLE1:
|
|
lance_angle = LANCE_ANGLE2;
|
|
break;
|
|
case LANCE_ANGLE2:
|
|
lance_angle = LANCE_ANGLE1;
|
|
break;
|
|
}
|
|
}
|
|
|
|
switch(mode){
|
|
case MODE_TRACE:
|
|
trace(sensor);
|
|
|
|
if( counter > 854*6 - 60 ){ // 6周(1周854カウント)したら、円筒標的を狙いに行く
|
|
mode = MODE_STOP;
|
|
}
|
|
|
|
break;
|
|
|
|
case MODE_STOP:
|
|
default:
|
|
digitalWrite(PIN_LED, HIGH);
|
|
handle_angle = 0;
|
|
lance_angle = -90;
|
|
motorMode(GO, GO, 0xff, 0xff);
|
|
if( counter - counter_old > 40 ){
|
|
motorMode(STOP, STOP, 0xff, 0xff);
|
|
}
|
|
}
|
|
|
|
handle(handle_angle);
|
|
lance(lance_angle);
|
|
|
|
}
|