半蔵2.1を作成
This commit is contained in:
parent
c8d77fcb0b
commit
d758cb406d
3 changed files with 354 additions and 0 deletions
47
arduino/hanzo2_1/Motor.cpp
Normal file
47
arduino/hanzo2_1/Motor.cpp
Normal file
|
|
@ -0,0 +1,47 @@
|
|||
/*
|
||||
Motor.cpp
|
||||
(c)2012 kou029w - MIT License [http://kou029w.appspot.com/mit-license.txt]
|
||||
*/
|
||||
|
||||
#if defined(ARDUINO) && ARDUINO >= 100
|
||||
#include <Arduino.h>
|
||||
#else
|
||||
#include <WProgram.h>
|
||||
#endif
|
||||
#include "Motor.h"
|
||||
|
||||
void Motor::mode(byte mode){
|
||||
digitalWrite(Motor::_pin1, (mode>>0)&1);
|
||||
digitalWrite(Motor::_pin2, (mode>>1)&1);
|
||||
}
|
||||
|
||||
void Motor::mode(byte mode, byte speed){
|
||||
switch(mode){
|
||||
case GO:
|
||||
analogWrite(Motor::_pin1, speed);
|
||||
digitalWrite(Motor::_pin2, LOW);
|
||||
break;
|
||||
case BACK:
|
||||
analogWrite(Motor::_pin1, ~speed);
|
||||
digitalWrite(Motor::_pin2, HIGH);
|
||||
break;
|
||||
default:
|
||||
digitalWrite(Motor::_pin1, (mode>>0)&1);
|
||||
digitalWrite(Motor::_pin2, (mode>>1)&1);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void Motor::speed(int speed){
|
||||
if(speed<0) Motor::mode(BACK, -speed);
|
||||
else Motor::mode(GO, speed);
|
||||
}
|
||||
|
||||
void Motor::attach(byte pin1, byte pin2){
|
||||
Motor::_pin1 = pin1;
|
||||
Motor::_pin2 = pin2;
|
||||
pinMode(Motor::_pin1, OUTPUT);
|
||||
pinMode(Motor::_pin2, OUTPUT);
|
||||
digitalWrite(Motor::_pin1, LOW);
|
||||
digitalWrite(Motor::_pin2, LOW);
|
||||
}
|
||||
28
arduino/hanzo2_1/Motor.h
Normal file
28
arduino/hanzo2_1/Motor.h
Normal file
|
|
@ -0,0 +1,28 @@
|
|||
/*
|
||||
Motor.h
|
||||
(c)2012 kou029w - MIT License [http://kou029w.appspot.com/mit-license.txt]
|
||||
*/
|
||||
|
||||
#ifndef Motor_h
|
||||
#define Motor_h
|
||||
|
||||
#if defined(ARDUINO) && ARDUINO >= 100
|
||||
#include <Arduino.h>
|
||||
#else
|
||||
#include <WProgram.h>
|
||||
#endif
|
||||
|
||||
enum { STOP = 0, GO = 1, BACK = 2, BRAKE = 3 };
|
||||
|
||||
class Motor{
|
||||
public:
|
||||
void mode(byte mode);
|
||||
void mode(byte mode, byte speed);
|
||||
void speed(int speed);
|
||||
void attach(byte pin1, byte pin2);
|
||||
private:
|
||||
byte _pin1;
|
||||
byte _pin2;
|
||||
};
|
||||
|
||||
#endif
|
||||
279
arduino/hanzo2_1/hanzo2_1.ino
Normal file
279
arduino/hanzo2_1/hanzo2_1.ino
Normal file
|
|
@ -0,0 +1,279 @@
|
|||
/*
|
||||
競技用ランサーロボット
|
||||
半蔵 2.1
|
||||
(C)2012 kou029w - MIT License [http://kou029w.appspot.com/mit-license.txt]
|
||||
*/
|
||||
|
||||
/*
|
||||
<前>
|
||||
|
||||
[7 654321 0]
|
||||
|
|
||||
|
|
||||
[]---*----[]
|
||||
| /
|
||||
|/
|
||||
[]---#----[]
|
||||
*/
|
||||
|
||||
#include <Servo.h>
|
||||
#include "Motor.h"
|
||||
|
||||
/**************************************/
|
||||
//#define LINE_BLACK //白地・黒ライン有効
|
||||
#define PIN_SENSOR ( ((PINC<<2) & ~0x03) | ((PINB>>4) & 0x03) )
|
||||
|
||||
const unsigned int TARGET_NONE_COUNT = 85;
|
||||
|
||||
const int LANCE_ANGLE0 = 0; //度
|
||||
const int LANCE_ANGLE1 = 43;
|
||||
const int LANCE_ANGLE2 = 54;
|
||||
const int LANCE_ANGLE3 = -33;
|
||||
const int LANCE_ANGLE4 = 34;
|
||||
const unsigned int LANCE_INTERVAL = 70; //ms
|
||||
|
||||
const byte SPEED_DEFAULT = 0xff;
|
||||
|
||||
const int HANDLE_DEFAULT = 90; //度
|
||||
const unsigned int HANDLE_MIN = ( 800+30); //ms
|
||||
const unsigned int HANDLE_MAX = (2300+30); //ms
|
||||
|
||||
const int LANCE_DEFAULT = 90; //度
|
||||
const unsigned int LANCE_MIN = 544; //ms
|
||||
const unsigned int LANCE_MAX = 2400; //ms
|
||||
|
||||
const byte PIN_LED = 4;
|
||||
const byte PIN_ROT = 2;
|
||||
|
||||
const byte PIN_SENSOR_0 = 12;
|
||||
const byte PIN_SENSOR_1 = 13;
|
||||
const byte PIN_SENSOR_2 = 14;
|
||||
const byte PIN_SENSOR_3 = 15;
|
||||
const byte PIN_SENSOR_4 = 16;
|
||||
const byte PIN_SENSOR_5 = 17;
|
||||
const byte PIN_SENSOR_6 = 18;
|
||||
const byte PIN_SENSOR_7 = 19;
|
||||
|
||||
const byte PIN_SERVO_HANDLE = 10;
|
||||
const byte PIN_SERVO_LANCE = 9;
|
||||
|
||||
const byte PIN_MOTOR_LEFT_1 = 5;
|
||||
const byte PIN_MOTOR_LEFT_2 = 4;
|
||||
const byte PIN_MOTOR_RIGHT_1 = 6;
|
||||
const byte PIN_MOTOR_RIGHT_2 = 7;
|
||||
|
||||
const byte MODE_STOP = 0;
|
||||
const byte MODE_TRACE = 10;
|
||||
const byte MODE_RIGHT = 21; // 右カーブ
|
||||
const byte MODE_LEFT = 22; // 左カーブ
|
||||
|
||||
const byte TARGET_NONE = 40;
|
||||
const byte TARGET_PARALLEL = 41;
|
||||
const byte TARGET_VERTICAL = 42;
|
||||
const byte TARGET_CYLINDER = 43;
|
||||
|
||||
const byte MASK_MODE_TRACE = 0b01111110;
|
||||
const byte MASK_CHECK_MARKER = 0b10000001;
|
||||
const byte MASK_CHECK_MARKER_RIGHT = 0b00000001;
|
||||
const byte MASK_CHECK_MARKER_LEFT = 0b10000000;
|
||||
|
||||
/**************************************/
|
||||
|
||||
Servo servoHandle;
|
||||
Servo servoLance;
|
||||
Motor motorR;
|
||||
Motor motorL;
|
||||
|
||||
byte mode;
|
||||
byte target;
|
||||
unsigned int counterOld;
|
||||
char handleAngle;
|
||||
char lanceAngle;
|
||||
|
||||
/**************************************/
|
||||
|
||||
void trace(byte sensor){
|
||||
switch( sensor & MASK_MODE_TRACE ){
|
||||
case 0b01000000:
|
||||
handleAngle = -20;
|
||||
motorMode(GO, GO, 0x40, 0xff);
|
||||
lanceAngle = 0;
|
||||
break;
|
||||
case 0b01100000:
|
||||
handleAngle = -16;
|
||||
motorMode(GO, GO, 0xff, 0xff);
|
||||
lanceAngle = 0;
|
||||
break;
|
||||
case 0b00100000:
|
||||
handleAngle = -12;
|
||||
motorMode(GO, GO, 0xff, 0xff);
|
||||
lanceAngle = 0;
|
||||
break;
|
||||
case 0b00110000:
|
||||
handleAngle = -5;
|
||||
motorMode(GO, GO, 0xff, 0xff);
|
||||
break;
|
||||
case 0b00010000:
|
||||
handleAngle = -2;
|
||||
motorMode(GO, GO, 0xff, 0xff);
|
||||
break;
|
||||
case 0b00011000:
|
||||
handleAngle = 0;
|
||||
motorMode(GO, GO, 0xff, 0xff);
|
||||
break;
|
||||
case 0b00001000:
|
||||
handleAngle = 2;
|
||||
motorMode(GO, GO, 0xff, 0xff);
|
||||
break;
|
||||
case 0b00001100:
|
||||
handleAngle = 5;
|
||||
motorMode(GO, GO, 0xff, 0xff);
|
||||
break;
|
||||
case 0b00000100:
|
||||
handleAngle = 7;
|
||||
motorMode(GO, GO, 0xff, 0xff);
|
||||
break;
|
||||
case 0b00000110:
|
||||
handleAngle = 10;
|
||||
motorMode(GO, GO, 0xff, 0xff);
|
||||
break;
|
||||
case 0b00000010:
|
||||
handleAngle = 12;
|
||||
motorMode(GO, GO, 0xff, 0xff);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/**************************************/
|
||||
|
||||
void motorMode(byte motorLeftMode, byte motorRightMode, byte motorLeftSpeed, byte motorRightSpeed){
|
||||
motorL.mode( motorLeftMode, motorLeftSpeed);
|
||||
motorR.mode(motorRightMode, motorRightSpeed);
|
||||
}
|
||||
|
||||
void handle(char handle_angle){
|
||||
servoHandle.write(HANDLE_DEFAULT + handle_angle);
|
||||
}
|
||||
|
||||
void lance(char lance_angle){
|
||||
servoLance.write(LANCE_DEFAULT - lance_angle);
|
||||
}
|
||||
|
||||
/* ロータリーエンコーダーの変化を見る */
|
||||
volatile unsigned long counter = 0;
|
||||
void count(){
|
||||
static byte rot[2] = {0};
|
||||
rot[1] = digitalRead(PIN_ROT);
|
||||
if(rot[1] != rot[0]){
|
||||
counter++;
|
||||
rot[0] = rot[1];
|
||||
}
|
||||
}
|
||||
|
||||
void sensorInit(){
|
||||
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);
|
||||
}
|
||||
|
||||
void setup() {
|
||||
//サーボ
|
||||
servoHandle.attach(PIN_SERVO_HANDLE, HANDLE_MIN, HANDLE_MAX);
|
||||
servoLance.attach(PIN_SERVO_LANCE, LANCE_MIN, LANCE_MAX);
|
||||
handle(0);
|
||||
lance(0);
|
||||
//モーター
|
||||
motorL.attach( PIN_MOTOR_LEFT_1, PIN_MOTOR_LEFT_2);
|
||||
motorR.attach(PIN_MOTOR_RIGHT_1, PIN_MOTOR_RIGHT_2);
|
||||
motorMode(STOP, STOP, 0, 0);
|
||||
//センサー
|
||||
sensorInit();
|
||||
//エンコーダー
|
||||
pinMode(PIN_ROT, INPUT);
|
||||
//LED
|
||||
pinMode(PIN_LED, OUTPUT);
|
||||
digitalWrite(PIN_LED, HIGH);
|
||||
//モード
|
||||
mode = MODE_STOP;
|
||||
delay(1000);
|
||||
/* スタート */
|
||||
digitalWrite(PIN_LED, LOW);
|
||||
mode = MODE_TRACE;
|
||||
handle(0);
|
||||
lance(0);
|
||||
motorMode(GO, GO, 0xff, 0xff);
|
||||
}
|
||||
|
||||
void loop(){
|
||||
//センサーを見る
|
||||
byte sensor = 0;
|
||||
#ifdef LINE_BLACK //黒ライン
|
||||
sensor = PIN_SENSOR;
|
||||
#else
|
||||
sensor = ~PIN_SENSOR;
|
||||
#endif
|
||||
//ロータリーエンコーダーの変化を見る
|
||||
count();
|
||||
//ランスの動作を決定する
|
||||
switch(target){
|
||||
case TARGET_NONE:
|
||||
lanceAngle = LANCE_ANGLE0;
|
||||
if(counter - counterOld > TARGET_NONE_COUNT ) target = 0;
|
||||
break;
|
||||
default:
|
||||
switch( sensor & MASK_CHECK_MARKER ){
|
||||
case 0x81:
|
||||
target = TARGET_NONE;
|
||||
counterOld = counter;
|
||||
break;
|
||||
case 0x01:
|
||||
lanceAngle = LANCE_ANGLE1;
|
||||
target = TARGET_PARALLEL;
|
||||
break;
|
||||
case 0x80:
|
||||
lanceAngle = LANCE_ANGLE3;
|
||||
target = TARGET_VERTICAL;
|
||||
break;
|
||||
}
|
||||
break;
|
||||
}
|
||||
if( lanceAngle == LANCE_ANGLE1 && counter%840 > 420) lanceAngle = LANCE_ANGLE4;
|
||||
static unsigned long millis_lance = 0;
|
||||
if(millis() - millis_lance > LANCE_INTERVAL){
|
||||
millis_lance = millis();
|
||||
switch(lanceAngle){
|
||||
case LANCE_ANGLE1:
|
||||
lanceAngle = LANCE_ANGLE2;
|
||||
break;
|
||||
case LANCE_ANGLE2:
|
||||
lanceAngle = 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);
|
||||
handleAngle = 0;
|
||||
lanceAngle = -90;
|
||||
motorMode(GO, GO, 0xff, 0xff);
|
||||
if( counter - counterOld > 40 ){
|
||||
motorMode(STOP, STOP, 0xff, 0xff);
|
||||
}
|
||||
}
|
||||
|
||||
handle(handleAngle);
|
||||
lance(lanceAngle);
|
||||
}
|
||||
Loading…
Add table
Reference in a new issue