半蔵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