diff --git a/arduino/gennai/Motor.cpp b/arduino/gennai/Motor.cpp new file mode 100644 index 0000000..c7e7958 --- /dev/null +++ b/arduino/gennai/Motor.cpp @@ -0,0 +1,61 @@ +/* +Motor.cpp +(C)2013 kou029w - MIT License +*/ +#include "Motor.h" + +Motor::Motor(){ + _pin1 = -1; + _pin2 = -1; +} + +void Motor::mode(uint8_t mode){ + if(_pin1 != -1) digitalWrite(_pin1, (mode>>0)&1); + if(_pin2 != -1) digitalWrite(_pin2, (mode>>1)&1); +} + +void Motor::mode(uint8_t mode, uint8_t speed){ + switch(mode){ + case GO: + if(_pin1 != -1) analogWrite(_pin1, speed); + if(_pin2 != -1) digitalWrite(_pin2, LOW); + break; + case BACK: + if(_pin1 != -1) analogWrite(_pin1, ~speed); + if(_pin2 != -1) digitalWrite(_pin2, HIGH); + break; + default: + if(_pin1 != -1) digitalWrite(_pin1, (mode>>0)&1); + if(_pin2 != -1) digitalWrite(_pin2, (mode>>1)&1); + break; + } +} + +void Motor::speed(int speed){ + if(speed>=0) mode(GO, speed); + else mode(BACK, -speed); +} + +void Motor::attach(uint8_t pin1){ + _pin1 = pin1; + _pin2 = -1; + pinMode(_pin1, OUTPUT); + digitalWrite(_pin1, LOW); +} + +void Motor::attach(uint8_t pin1, uint8_t pin2){ + _pin1 = pin1; + _pin2 = pin2; + pinMode(_pin1, OUTPUT); + pinMode(_pin2, OUTPUT); + digitalWrite(_pin1, LOW); + digitalWrite(_pin2, LOW); +} + +void Motor::detach(){ + mode(STOP); + pinMode(_pin1, INPUT); + pinMode(_pin2, INPUT); + _pin1 = -1; + _pin2 = -1; +} diff --git a/arduino/gennai/Motor.h b/arduino/gennai/Motor.h new file mode 100644 index 0000000..6783d68 --- /dev/null +++ b/arduino/gennai/Motor.h @@ -0,0 +1,75 @@ +/* +Motor.h - モータードライバ(1ピン/2ピン)のためのライブラリ + +## 概要 ## +このライブラリは、1ピンあるいは2ピンを占有するモータードライバのためのシンプルなライブラリです。 +PWM対応ピンが1つしか必要ないのがこのライブラリの特徴です。 +ただし、その副作用として、GOとBACKの速度に差があることがあります。 + +## 使い方 ## +例: + #include "Motor.h" + Motor motor; + void setup(){ + // motor.attach(pin1, pin2); + motor.attach(5, 6); + } + void loop(){ + // motor.mode(GO); //正転 + motor.mode(GO, 100); //0-255(ここでは、100)のスピードで正転 + delay(1000); + motor.mode(STOP); //停止 + delay(1000); + } + +注意: +使用するモータードライバの動作は、 +1ピンの場合: + 動作 |pin1 + -----+----- + STOP | L + GO | H + +2ピンの場合: + 動作 |pin1 |pin2 + ----------+-----+----- + STOP | L | L + GO | H | L + BACK | L | H + STOP/BRAKE| H | H + +でなければなりません。また、 + void Motor::mode(char mode, byte speed); + void Motor::speed(int speed); + +これらを使用する場合、pin1はPWM対応でなければなりません。 + +## ライセンス ## +(C)2013 kou029w - MIT License +*/ + +#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: + Motor(); + void mode(uint8_t mode); + void mode(uint8_t mode, uint8_t speed); + void speed(int speed); + void attach(uint8_t pin1); + void attach(uint8_t pin1, uint8_t pin2); + void detach(); +private: + char _pin1; + char _pin2; +}; + +#endif diff --git a/arduino/gennai/gennai.ino b/arduino/gennai/gennai.ino new file mode 100644 index 0000000..6c5c32b --- /dev/null +++ b/arduino/gennai/gennai.ino @@ -0,0 +1,155 @@ +/* +競技用ランサーロボット 源内 +(C)2013 kou029w - MIT License +*/ + +#include +#include +#include +#include "Motor.h" + +const unsigned long LAP_DISTANCE = 500; //ロータリーエンコーダーのカウント数/周 +const unsigned long TURN_DISTANCE = 1500; //カーブ中でのカウント数 + +const int LANCE_ANGLE_PARALLEL_1 = 90 - 62; //度 +const int LANCE_ANGLE_PARALLEL_2 = 90 - 50; //度 +const int LANCE_ANGLE_VERTICAL_L = 90 - 120; //度 +const int LANCE_ANGLE_VERTICAL_R = 90 - 62; //度 + +const unsigned char SPEED_DEFAULT = 100; + +//Servo : SC-0352 +// 60deg : 1265 us +// 90deg : 1535 us +//120deg : 1805 us +const int LANCE_DEFAULT = 90; //度 +const unsigned int LANCE_MIN = (1620-780); //us +const unsigned int LANCE_MAX = (1620+780); //us + +const unsigned char PIN_BUZZER = 3; +const unsigned char PIN_ROT = 2; + +const unsigned char PIN_SERVO_LANCE = 9; + +const unsigned char NUM_SENSORS = 2; +unsigned char PIN_SENSORS[NUM_SENSORS] = {13, 12}; // 左端...右端 + +const unsigned char PIN_MOTOR_LEFT = 6; +const unsigned char PIN_MOTOR_RIGHT = 5; + +/**************************************/ + +Servo lanceServo; +Motor motorL; +Motor motorR; + +QTRSensorsRC sensor(PIN_SENSORS, NUM_SENSORS); +unsigned int sensorValues[NUM_SENSORS]; + +volatile unsigned long distance = 0; +unsigned long modeChangedDistance = 0; + +int error_count = 0; + +/**************************************/ + +enum mode_t { + MODE_STOP, //停止 + MODE_STRAIGHT, //まっすぐ進む + MODE_TURN, //左に曲がる + MODE_ATTACK_PARALLEL_1, //平行標的1 + MODE_ATTACK_PARALLEL_2, //平行標的2 + MODE_ATTACK_VERTICAL_R, //右垂直標的 + MODE_ATTACK_VERTICAL_L, //左垂直標的 + MODE_ATTACK_CYLINDER //円筒標的 +} +mode; + +/**************************************/ + + +/* センサーを読んで、車体の動作を決定する */ +void trace(){ + error_count++; + for(int i=0; i 600) motorR.speed(SPEED_DEFAULT*0.6); + else motorR.speed(SPEED_DEFAULT); + if(sensorValues[1] > 600) motorL.speed(SPEED_DEFAULT*0.6); + else motorL.speed(SPEED_DEFAULT); +} + +/* ランスを中央からa[度]だけ動かす + <- - 0 + -> + / .\ + / | \ + []+--{}--+[] +*/ +void lance(int a){ + lanceServo.write(LANCE_DEFAULT - a); +} +void modeSet(){ + if(distance > LAP_DISTANCE || error_count > 100){ + mode = MODE_STOP; + return; + } +} + +void rot(){ + distance++; +} + +void startBeep(){ + tone(PIN_BUZZER,2000); + delay(100); + tone(PIN_BUZZER,1000); + delay(100); + noTone(PIN_BUZZER); +} +/**************************************/ + +void setup(){ + Serial.begin(9600); + //サーボ + lanceServo.attach(PIN_SERVO_LANCE, LANCE_MIN, LANCE_MAX); + lance(0); + //モーター + motorL.attach(PIN_MOTOR_LEFT); + motorR.attach(PIN_MOTOR_RIGHT); + motorL.mode(STOP); + motorR.mode(STOP); + //エンコーダー + pinMode(PIN_ROT, INPUT); + attachInterrupt(0, rot, RISING); + //ブザー + pinMode(PIN_BUZZER, OUTPUT); + startBeep(); + /* 3秒停止 */ + motorL.mode(STOP); + motorR.mode(STOP); + delay(3000); + /* スタート */ + mode = MODE_STRAIGHT; +} + +void loop(){ + Serial.println(distance); + sensor.read(sensorValues); + switch(mode){ + case MODE_STOP: //停止 + motorL.mode(STOP); + motorR.mode(STOP); + tone(PIN_BUZZER, 2000); + delay(2000); + noTone(PIN_BUZZER); + while(1){ + ; + }; + case MODE_STRAIGHT: //まっすぐ進む + trace(); + modeSet(); + break; + } +} diff --git a/arduino/gennai/pitches.h b/arduino/gennai/pitches.h new file mode 100644 index 0000000..9f16b4d --- /dev/null +++ b/arduino/gennai/pitches.h @@ -0,0 +1,93 @@ +/************************************************* + * Public Constants + *************************************************/ + +#define NOTE_B0 31 +#define NOTE_C1 33 +#define NOTE_CS1 35 +#define NOTE_D1 37 +#define NOTE_DS1 39 +#define NOTE_E1 41 +#define NOTE_F1 44 +#define NOTE_FS1 46 +#define NOTE_G1 49 +#define NOTE_GS1 52 +#define NOTE_A1 55 +#define NOTE_AS1 58 +#define NOTE_B1 62 +#define NOTE_C2 65 +#define NOTE_CS2 69 +#define NOTE_D2 73 +#define NOTE_DS2 78 +#define NOTE_E2 82 +#define NOTE_F2 87 +#define NOTE_FS2 93 +#define NOTE_G2 98 +#define NOTE_GS2 104 +#define NOTE_A2 110 +#define NOTE_AS2 117 +#define NOTE_B2 123 +#define NOTE_C3 131 +#define NOTE_CS3 139 +#define NOTE_D3 147 +#define NOTE_DS3 156 +#define NOTE_E3 165 +#define NOTE_F3 175 +#define NOTE_FS3 185 +#define NOTE_G3 196 +#define NOTE_GS3 208 +#define NOTE_A3 220 +#define NOTE_AS3 233 +#define NOTE_B3 247 +#define NOTE_C4 262 +#define NOTE_CS4 277 +#define NOTE_D4 294 +#define NOTE_DS4 311 +#define NOTE_E4 330 +#define NOTE_F4 349 +#define NOTE_FS4 370 +#define NOTE_G4 392 +#define NOTE_GS4 415 +#define NOTE_A4 440 +#define NOTE_AS4 466 +#define NOTE_B4 494 +#define NOTE_C5 523 +#define NOTE_CS5 554 +#define NOTE_D5 587 +#define NOTE_DS5 622 +#define NOTE_E5 659 +#define NOTE_F5 698 +#define NOTE_FS5 740 +#define NOTE_G5 784 +#define NOTE_GS5 831 +#define NOTE_A5 880 +#define NOTE_AS5 932 +#define NOTE_B5 988 +#define NOTE_C6 1047 +#define NOTE_CS6 1109 +#define NOTE_D6 1175 +#define NOTE_DS6 1245 +#define NOTE_E6 1319 +#define NOTE_F6 1397 +#define NOTE_FS6 1480 +#define NOTE_G6 1568 +#define NOTE_GS6 1661 +#define NOTE_A6 1760 +#define NOTE_AS6 1865 +#define NOTE_B6 1976 +#define NOTE_C7 2093 +#define NOTE_CS7 2217 +#define NOTE_D7 2349 +#define NOTE_DS7 2489 +#define NOTE_E7 2637 +#define NOTE_F7 2794 +#define NOTE_FS7 2960 +#define NOTE_G7 3136 +#define NOTE_GS7 3322 +#define NOTE_A7 3520 +#define NOTE_AS7 3729 +#define NOTE_B7 3951 +#define NOTE_C8 4186 +#define NOTE_CS8 4435 +#define NOTE_D8 4699 +#define NOTE_DS8 4978