diff --git a/arduino/hanzo2_1/Motor.cpp b/arduino/hanzo2_1/Motor.cpp new file mode 100644 index 0000000..205038a --- /dev/null +++ b/arduino/hanzo2_1/Motor.cpp @@ -0,0 +1,47 @@ +/* +Motor.cpp +(c)2012 kou029w - MIT License [http://kou029w.appspot.com/mit-license.txt] +*/ + +#if defined(ARDUINO) && ARDUINO >= 100 +#include +#else +#include +#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); +} diff --git a/arduino/hanzo2_1/Motor.h b/arduino/hanzo2_1/Motor.h new file mode 100644 index 0000000..c8b3a70 --- /dev/null +++ b/arduino/hanzo2_1/Motor.h @@ -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 +#else +#include +#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 diff --git a/arduino/hanzo2_1/hanzo2_1.ino b/arduino/hanzo2_1/hanzo2_1.ino new file mode 100644 index 0000000..e3c6fc7 --- /dev/null +++ b/arduino/hanzo2_1/hanzo2_1.ino @@ -0,0 +1,279 @@ +/* +競技用ランサーロボット +半蔵 2.1 +(C)2012 kou029w - MIT License [http://kou029w.appspot.com/mit-license.txt] +*/ + +/* + <前> + +[7 654321 0] + | + | +[]---*----[] + | / + |/ +[]---#----[] +*/ + +#include +#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); +}