diff --git a/arduino/Hanzo3ServoSetMicroseconds/Hanzo3ServoSetMicroseconds.ino b/arduino/Hanzo3ServoSetMicroseconds/Hanzo3ServoSetMicroseconds.ino new file mode 100644 index 0000000..9ecc36b --- /dev/null +++ b/arduino/Hanzo3ServoSetMicroseconds/Hanzo3ServoSetMicroseconds.ino @@ -0,0 +1,35 @@ +#include + +Servo servo1; +Servo servo2; + +void setup() { + servo1.attach(9,1450-570,1450+570); + servo2.attach(10,1535-780,1535+780); + servo1.write(90); + servo2.write(90); + Serial.begin(9600); + Serial.print("format : [0-9]+[ab]"); +} + +void loop() { + static int v = 0; + + if ( Serial.available()) { + char ch = Serial.read(); + + switch(ch) { + case '0'...'9': + v = v * 10 + ch - '0'; + break; + case 'a': + servo1.writeMicroseconds(v); + v = 0; + break; + case 'b': + servo2.writeMicroseconds(v); + v = 0; + break; + } + } +} diff --git a/arduino/MotorSet/Motor.cpp b/arduino/MotorSet/Motor.cpp new file mode 100644 index 0000000..c7e7958 --- /dev/null +++ b/arduino/MotorSet/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/MotorSet/Motor.h b/arduino/MotorSet/Motor.h new file mode 100644 index 0000000..6783d68 --- /dev/null +++ b/arduino/MotorSet/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/MotorSet/MotorSet.ino b/arduino/MotorSet/MotorSet.ino new file mode 100644 index 0000000..f6a7caf --- /dev/null +++ b/arduino/MotorSet/MotorSet.ino @@ -0,0 +1,31 @@ +#include "Motor.h" + +Motor motor; + +void setup() { + motor.attach(5,6); + Serial.begin(9600); + Serial.print("format : [0-255][ab]"); +} + +void loop() { + static int v = 0; + + if ( Serial.available() ) { + char ch = Serial.read(); + + switch(ch) { + case '0'...'9': + v = v * 10 + ch - '0'; + break; + case 'a': + motor.speed(v); + v = 0; + break; + case 'b': + motor.speed(-v); + v = 0; + break; + } + } +} diff --git a/arduino/dev/DigitalReadSerialX8.cpp b/arduino/dev/DigitalReadSerialX8.cpp new file mode 100644 index 0000000..c5c0624 --- /dev/null +++ b/arduino/dev/DigitalReadSerialX8.cpp @@ -0,0 +1,34 @@ +/* +DigitalReadSerialX8.cpp +(C)2013 kou029w - MIT License +*/ +#if defined(ARDUINO) && ARDUINO >= 100 +#include +#else +#include +#endif +#include "DigitalReadSerialX8.h" + +template<> +void DigitalReadSerialX8(HardwareSerial& serial, uint8_t minPin); +#ifdef __AVR_ATmega32U4__ +template<> +void DigitalReadSerialX8(Serial_& serial, uint8_t minPin); +#endif + +template +void DigitalReadSerialX8(T& serial, uint8_t minPin=12){ + if(!serial) return; + for(int i=minPin; i= 100 +#include +#else +#include +#endif + +template void DigitalReadSerialX8(T& serial, uint8_t minPin); + +#endif diff --git a/arduino/dev/PDControl.cpp b/arduino/dev/PDControl.cpp new file mode 100644 index 0000000..8b7ba4c --- /dev/null +++ b/arduino/dev/PDControl.cpp @@ -0,0 +1,55 @@ +/* +PDControl.cpp +(C)2013 kou029w - MIT License +*/ + +#include +#include "PDControl.h" + +PDControl_c::PDControl_c(int p=1, int d=0, float decR=0.0){ + lastInput = 0; + diff = 0; + Kp = p; + Kd = d; + diffDecayRate = decR; +} + +int PDControl_c::operator()(int input){ + int output; + + diff = diffDecayRate*diff; + + diff = (input - lastInput) + diff; + output = Kp*input + Kd*diff; + + lastInput = input; + + return output; +} + +FastPDControl_c::FastPDControl_c(int p=1, int d=0, unsigned char hl=0){ + lastInput = 0; + diff = 0; + Kp = p; + Kd = d; + diffHalfLife = hl; + _count = 0; +} + +int FastPDControl_c::operator()(int input){ + int output; + + if(_count < diffHalfLife){ + _count++; + }else{ + _count = 0; + diff>>1; + } + + diff = (input - lastInput) + diff; + output = Kp*input + Kd*diff; + + lastInput = input; + + return output; +} diff --git a/arduino/dev/PDControl.h b/arduino/dev/PDControl.h new file mode 100644 index 0000000..88053a4 --- /dev/null +++ b/arduino/dev/PDControl.h @@ -0,0 +1,38 @@ +/* +PDControl.h +(C)2013 kou029w - MIT License +*/ +#ifndef PDControl_h +#define PDControl_h + +#include + +class PDControl_c{ + public: + PDControl_c(); + PDControl_c(int p, int d); + PDControl_c(int p, int d, float decR); + int operator()(int input); + int lastInput; + int diff; + int Kp; + int Kd; + float diffDecayRate; +}; + +class FastPDControl_c{ + public: + FastPDControl_c(); + FastPDControl_c(int p, int d); + FastPDControl_c(int p, int d, uint8_t hl); + int operator()(int input); + int lastInput; + int diff; + int Kp; + int Kd; + uint8_t diffHalfLife; + private: + uint8_t _count; +}; + +#endif diff --git a/arduino/dev/SerialUtil.cpp b/arduino/dev/SerialUtil.cpp new file mode 100644 index 0000000..9d1e650 --- /dev/null +++ b/arduino/dev/SerialUtil.cpp @@ -0,0 +1,49 @@ +/* +SerialUtil.cpp +(C)2013 kou029w - MIT License +*/ + +#include +#include "SerialUtil.h" + +template<> +String inputLine(String str, HardwareSerial& serial); +template<> +String getStr(size_t size, HardwareSerial& serial); +#ifdef __AVR_ATmega32U4__ +template<> +String inputLine(String str, Serial_& serial); +template<> +String getStr(size_t size, Serial_& serial); +#endif + +// serialから文字列を一行('\r'まで)入力 (str:入力がないとき返す文字列) +template +String inputLine(String str, T& serial){ + if(!serial) return str; + String result = ""; + for(;;){ + if(serial.available()){ + char c = serial.read(); + if(c == '\r') break; + result += String(c); + } + } + if(result != "") return result; + return str; +} + +// serialから文字列を入力 +template +String getStr(size_t size, T& serial){ + if(!serial) return String(""); + String str = ""; + for(size_t i = 0; i + +template +String inputLine(String str, T& serial); + +template +String getStr(size_t size, T& serial); + +#endif diff --git a/arduino/dev/ServoTest.cpp b/arduino/dev/ServoTest.cpp new file mode 100644 index 0000000..dd3d370 --- /dev/null +++ b/arduino/dev/ServoTest.cpp @@ -0,0 +1,122 @@ +/* +ServoTest.cpp +(C)2013 kou029w - MIT License +*/ + +#include +#include +#include "ServoTest.h" +#include "SerialUtil.h" + +//#ifdef Servo_h +template<> +class ServoTestClass; +#ifdef __AVR_ATmega32U4__ +template<> +class ServoTestClass; +#endif +//#endif + +template +void ServoTestClass::_setAndPrint(SERVO& servo, const char* servoName, S& serial){ + static unsigned long lastTime; + lastTime = _time; + _time = millis(); + serial.print("delay("); + serial.print(_time - lastTime); + serial.println(");"); + + serial.print(servoName); + switch(_mode){ + case ATTACH_MODE: + serial.print(".attach("); + serial.print(_val); + servo.attach(_val); + break; + case DETACH_MODE: + serial.println(".detach("); + servo.detach(); + break; + case WRITE_MODE: + serial.print(".write("); + serial.print(_val); + servo.write(_val); + break; + case WRITE_MICROSEC_MODE: + serial.print(".writeMicroseconds("); + serial.print(_val); + servo.writeMicroseconds(_val); + break; + } + serial.println(");"); +} + +template +void ServoTestClass::operator()(S& serial){ + // using namespace servotest; + char _helpdoc[] = "" + "/*\n" + "## help ##\n" + "default\n" + " Format : \"angle + 'a/b'\"\n" + " Example: \"90a\" => servoA.write(90)\n" + "\n" + "[t] attach\n" + " Format : \"'t' + pin + 'a/b'\"\n" + " Example: \"t8b\" => servoB.attach(8)\n" + "\n" + "[d] detach\n" + " Format : \"'d' + 'a/b'\"\n" + " Example: \"db\" => servoB.detach()\n" + "\n" + "[u] write-microseconds\n" + " Format : \"'u' + us + 'a/b'\"\n" + " Example: \"u1500a\" => servoA.writeMicroseconds(1500)\n" + "*/\n"; + _time = millis(); + SERVO _servoA; + SERVO _servoB; + _servoA.attach(9); + _servoB.attach(10); + serial.println("servoA.attach(9);"); + serial.println("servoB.attach(10);"); + + /* Main loop */ + for(;;){ + _mode = NONE; + _val = 0; + serial.println("/*-- [q] quit [h] help --*/"); + while(_mode == NONE){ + char c = getStr(1, serial)[0]; + switch(c){ + case 'h': + serial.print(_helpdoc); + break; + case 'q': + _servoA.detach(); + _servoB.detach(); + return; + case 't': + _mode = ATTACH_MODE; + break; + case 'd': + _mode = DETACH_MODE; + break; + case 'u': + _mode = WRITE_MICROSEC_MODE; + break; + case '0'...'9': + _val = _val * 10 + c - '0'; + break; + case 'a': + if(_mode == NONE) _mode = WRITE_MODE; + _setAndPrint(_servoA, "servoA", serial); + break; + case 'b': + if(_mode == NONE) _mode = WRITE_MODE; + _setAndPrint(_servoB, "servoB", serial); + break; + } + } + } +} diff --git a/arduino/dev/ServoTest.h b/arduino/dev/ServoTest.h new file mode 100644 index 0000000..e875aed --- /dev/null +++ b/arduino/dev/ServoTest.h @@ -0,0 +1,41 @@ +/* +ServoTest.h - サーボの動作を確認するためのライブラリ + +## 概要 ## +このライブラリは、シリアルモニター上からサーボの動作を確認するためのシンプルなライブラリです。 + +## 使い方 ## +例: + #include + #include "ServoTest.h" + void setup(){ + Serial.begin(9600); + ServoTest(Serial); + } + void loop(){ + } + +## ライセンス ## +(C)2013 kou029w - MIT License +*/ + +#ifndef ServoTest_h +#define ServoTest_h + +#include +#include +#include "SerialUtil.h" + +template +class ServoTestClass{ + public: + void operator()(S& serial); + private: + void _setAndPrint(SERVO& servo, const char* servoName, S& serial); + enum mode_e {ATTACH_MODE, DETACH_MODE, WRITE_MODE, WRITE_MICROSEC_MODE, NONE} _mode; + unsigned long _time; + int _val; + char* _helpdoc; +}; + +#endif diff --git a/arduino/dev/dev.ino b/arduino/dev/dev.ino new file mode 100644 index 0000000..155947d --- /dev/null +++ b/arduino/dev/dev.ino @@ -0,0 +1,3 @@ +int analogRead(uint8_t pin, uint8_t max){ + return ((long)analogRead(pin)*max)>>10; +} diff --git a/arduino/hanzo2_1pr/Motor.cpp b/arduino/hanzo2_1pr/Motor.cpp new file mode 100644 index 0000000..205038a --- /dev/null +++ b/arduino/hanzo2_1pr/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_1pr/Motor.h b/arduino/hanzo2_1pr/Motor.h new file mode 100644 index 0000000..f41bb06 --- /dev/null +++ b/arduino/hanzo2_1pr/Motor.h @@ -0,0 +1,43 @@ +/* +Motor.h +モータードライバIC(TA7291Pなど)のためのシンプルなライブラリ +### 使い方 ### +#include "Motor.h" +Motor motor; +void setup(){ + motor.attach(5, 6); // pin1は、PWM対応であることが望ましい(5はPWM対応pin) +} +void loop(){ +// motor.mode(GO); //正転 + motor.mode(GO, 100); //0-255(ここでは、100)のスピードで正転 + delay(1000); + motor.mode(STOP); //停止 + delay(1000); +} +### ライセンス ### +(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_1pr/hanzo2_1pr.ino b/arduino/hanzo2_1pr/hanzo2_1pr.ino new file mode 100644 index 0000000..88b5026 --- /dev/null +++ b/arduino/hanzo2_1pr/hanzo2_1pr.ino @@ -0,0 +1,412 @@ +/* +競技用ランサーロボット +半蔵 2.1 - PR用 + +[7 654321 0] + | | + | | +[]---{}---[] + / .\ + / | \ +[]+--{}--+[] + +(C)2013 kou029w - MIT License [http://kou029w.appspot.com/mit-license.txt] +*/ + +#include +#include "Motor.h" +#include "pitches.h" + +void disk0(){ + /* + $pu1 + l12　o5 + c4f4b-4agfg8.g16 + b-a4b-4 + f8.d16 + $pu2 + f2. + */ + + int melodyDisk0[] = { + NOTE_C4, NOTE_F4, NOTE_AS4, NOTE_A4, NOTE_G4, NOTE_F4, NOTE_G4, + NOTE_C5, NOTE_DS5,NOTE_D5, NOTE_C5,NOTE_AS4, + NOTE_C5,NOTE_A4,NOTE_AS4, + NOTE_F4,NOTE_D4,NOTE_F4 + }; + + // note durations: 4 = quarter note, 8 = eighth note, etc.: + int noteDurationsDisk0[] = { + 4,4,4,11,11,11,4, + 4,4,11,11,11, + 4,4,4, + 6,11,2 + }; + + for (int thisNote = 0; thisNote < 18; thisNote++) { + int noteDuration = 1300/noteDurationsDisk0[thisNote]; + tone(3, melodyDisk0[thisNote],noteDuration); + int pauseBetweenNotes = noteDuration * 1.30; + delay(pauseBetweenNotes); + noTone(3); + } +} + +void disk1(){ + int melodyDisk1[] = {NOTE_B5,NOTE_D6,NOTE_G6}; + for (int thisNote = 0; thisNote < 3; thisNote++) { + tone(3, melodyDisk1[thisNote]); + delay(100); + noTone(3); + } +} + + + +const unsigned long LAP_DISTANCE = 500; //ロータリーエンコーダーのカウント数/周 +const unsigned long TURN_DISTANCE = 50; //カーブ中でのカウント数 + +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 byte SPEED_DEFAULT = 0xff; + +//Servo : SC-0352 +// 60deg : 1265 us +// 90deg : 1535 us +//120deg : 1805 us +const int STEERING_DEFAULT = 90; //度 +const unsigned int STEERING_MIN = ( 800+30); //us +const unsigned int STEERING_MAX = (2300+30); //us + +const int LANCE_DEFAULT = 90; //度 +const unsigned int LANCE_MIN = ( 544-62); //us +const unsigned int LANCE_MAX = (2400-62); //us + +const byte PIN_LED = 4; +const byte PIN_BUZZER = 3; +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_STEERING = 10; +const byte PIN_SERVO_LANCE = 9; + +const byte MASK_LINE = 0b01111110; +const byte MASK_MARKER = 0b10000001; + +/**************************************/ + +Servo steeringServo; +Servo lanceServo; +Motor motorR; +Motor motorL; + +//ラインが1、地面が0、LSBが左端、MSBが右端 +byte sensor = 0x00; + +volatile unsigned long distance = 0; +unsigned long modeChangedDistance = 0; + +/**************************************/ + +enum mode_t { + MODE_STOP, //停止 + MODE_STRAIGHT, //まっすぐ進む + MODE_TURN, //曲がる + MODE_TURN_LEFT, //左に曲がる + MODE_TURN_RIGHT,//右に曲がる + MODE_ATTACK_PARALLEL_1, //平行標的1 + MODE_ATTACK_PARALLEL_2, //平行標的2 + MODE_ATTACK_VERTICAL_R, //右垂直標的 + MODE_ATTACK_VERTICAL_L, //左垂直標的 + MODE_ATTACK_CYLINDER //円筒標的 +} +mode; + +/**************************************/ + +/* センサーを読んで、車体の動作を決定する */ +void trace(){ + switch(sensor&MASK_LINE){ + case 0b01000000: + steering(-20); + motorL.speed(SPEED_DEFAULT/2); + motorR.speed(SPEED_DEFAULT); + break; + case 0b01100000: + steering(-15); + motorL.speed(SPEED_DEFAULT/2); + motorR.speed(SPEED_DEFAULT); + break; + case 0b00100000: + steering(-11); + motorL.speed(SPEED_DEFAULT); + motorR.speed(SPEED_DEFAULT); + break; + case 0b00110000: + steering(-8); + motorL.speed(SPEED_DEFAULT); + motorR.speed(SPEED_DEFAULT); + break; + case 0b00010000: + steering(-4); + motorL.speed(SPEED_DEFAULT); + motorR.speed(SPEED_DEFAULT); + break; + case 0b00011000: + steering(0); + motorL.speed(SPEED_DEFAULT); + motorR.speed(SPEED_DEFAULT); + break; + case 0b00001000: + steering(4); + motorL.speed(SPEED_DEFAULT); + motorR.speed(SPEED_DEFAULT); + break; + case 0b00001100: + steering(8); + motorL.speed(SPEED_DEFAULT); + motorR.speed(SPEED_DEFAULT); + break; + case 0b00000100: + steering(11); + motorL.speed(SPEED_DEFAULT); + motorR.speed(SPEED_DEFAULT); + break; + case 0b00000110: + steering(15); + motorL.speed(SPEED_DEFAULT); + motorR.speed(SPEED_DEFAULT); + break; + case 0b00000010: + steering(20); + motorL.speed(SPEED_DEFAULT); + motorR.speed(SPEED_DEFAULT); + break; + } +} + +/* ステアリングを中央からa[度]だけ動かす + <- - 0 + -> + | | + | | + []---{}---[] + / .\ +*/ +void steering(int a){ + steeringServo.write(STEERING_DEFAULT + a); +} + +/* ランスを中央からa[度]だけ動かす + <- - 0 + -> + / .\ + / | \ + []+--{}--+[] +*/ +void lance(int a){ + lanceServo.write(LANCE_DEFAULT - a); +} + +/* センサーのアレイの初期化 */ +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); //左端 +} + +/* センサーアレイの状態を見る*/ +byte sensorRead(){ + byte b=0; //黒が1、白が0 +// if(analogRead(PIN_SENSOR_0)>511) b += 0b00000001; //右端 +// if(analogRead(PIN_SENSOR_1)>511) b += 0b00000010; + if(analogRead(PIN_SENSOR_2)>511) b += 0b00000100; + if(analogRead(PIN_SENSOR_3)>511) b += 0b00001000; + if(analogRead(PIN_SENSOR_4)>511) b += 0b00010000; + if(analogRead(PIN_SENSOR_5)>511) b += 0b00100000; + if(analogRead(PIN_SENSOR_6)>511) b += 0b01000000; + if(analogRead(PIN_SENSOR_7)>511) b += 0b10000000; //左端 + return sensor = b; +} + +void modeSet(){ + static int i = 0; + if(sensor == 0){ + i++; + }else i=0; + if(i>0){ + mode = MODE_TURN_LEFT; + return; + } + + switch(sensor&MASK_MARKER){ +// default: +// case 0b00000000: +// mode = MODE_STRAIGHT; +// break; +// case 0b10000001: +// modeChangedDistance = distance; +// mode = MODE_TURN; +// break; + case 0b00000001: + modeChangedDistance = distance; + mode = MODE_ATTACK_PARALLEL_1; + break; + case 0b10000000: + modeChangedDistance = distance; + mode = MODE_ATTACK_VERTICAL_L; + break; + } +} + +/* ロータリーエンコーダーの変化を見る */ +void count(){ + static byte rot[2] = {0}; //rot[0]:今の状態, rot[1]:前回の状態 + rot[0] = digitalRead(PIN_ROT); + if(rot[0] != rot[1]){ + rot[1] = rot[0]; + distance++; + } +} + +void startBeep(){ + tone(PIN_BUZZER,2000); + delay(100); + tone(PIN_BUZZER,1000); + delay(100); + noTone(PIN_BUZZER); +} + +void setup(){ + //サーボ + steeringServo.attach(PIN_SERVO_STEERING, STEERING_MIN, STEERING_MAX); + lanceServo.attach(PIN_SERVO_LANCE, LANCE_MIN, LANCE_MAX); + steering(0); + lance(0); + //モーター + motorL.attach(5,7); + motorR.attach(6,8); + motorL.mode(STOP); + motorR.mode(STOP); + //センサー + sensorInit(); + //エンコーダー + pinMode(PIN_ROT, INPUT); + //ブザー + pinMode(PIN_BUZZER, OUTPUT); + //LED + pinMode(PIN_LED, OUTPUT); + digitalWrite(PIN_LED, HIGH); + /* 停止 */ + motorL.mode(STOP); + motorR.mode(STOP); + delay(1000); + disk1(); + delay(1000); + /* スタート */ + digitalWrite(PIN_LED, LOW); + mode = MODE_STRAIGHT; +// Serial.begin(9600); +// while(1){ +// count(); +// Serial.println(distance); +// } +} + +void loop(){ + count(); + if(distance > 500){ + mode = MODE_STOP; + } + sensorRead(); + switch(mode){ + default: + case MODE_STOP: //停止 + motorL.mode(STOP); + motorR.mode(STOP); + digitalWrite(PIN_LED, HIGH); + delay(1000); + lance(0); + steering(0); + delay(500); + disk0(); +// delay(1000); +// tone(PIN_BUZZER,2000); +// delay(500); +// noTone(PIN_BUZZER); + while(1){ + ; + }; + break; + case MODE_STRAIGHT: //まっすぐ進む + trace(); + modeSet(); + break; + case MODE_TURN_LEFT: //左に曲がる + lance(-70); + if(distance - modeChangedDistance < TURN_DISTANCE){ + steering(-30); + motorL.speed(SPEED_DEFAULT/2); + motorR.speed(SPEED_DEFAULT); + }else mode = MODE_STOP; + break; + case MODE_TURN_RIGHT://右に曲がる + lance(0); + if(distance - modeChangedDistance < TURN_DISTANCE){ + steering(30); + motorL.speed(SPEED_DEFAULT); + motorR.speed(SPEED_DEFAULT/2); + }else mode = MODE_STOP; + break; + case MODE_ATTACK_PARALLEL_1: //平行標的1 + trace(); + lance(LANCE_ANGLE_PARALLEL_1); + if(distance - modeChangedDistance > 255){ //マーカーから少し進んで、叩く + lance(LANCE_ANGLE_PARALLEL_1 + 10); + mode = MODE_STRAIGHT; + } + break; + case MODE_ATTACK_PARALLEL_2: //平行標的2 + trace(); + lance(LANCE_ANGLE_PARALLEL_2); + if(distance - modeChangedDistance > 283){ //マーカーから少し進んで、叩く + lance(LANCE_ANGLE_PARALLEL_2 + 10); + mode = MODE_STRAIGHT; + } + break; + case MODE_ATTACK_VERTICAL_R: //右垂直標 + trace(); + lance(LANCE_ANGLE_VERTICAL_R); + if(distance - modeChangedDistance > 300){ //マーカーから少し進んで、まっすぐに戻す + lance(0); + mode = MODE_STRAIGHT; + } + break; + case MODE_ATTACK_VERTICAL_L: //左垂直標 + trace(); + lance(LANCE_ANGLE_VERTICAL_L); + if(distance - modeChangedDistance > 5){ //マーカーから少し進んで、まっすぐに戻す + lance(0); + mode = MODE_STRAIGHT; + } + break; + case MODE_ATTACK_CYLINDER: //円筒標的 + trace(); + mode = MODE_STRAIGHT; + break; + } +} diff --git a/arduino/hanzo2_1pr/pitches.h b/arduino/hanzo2_1pr/pitches.h new file mode 100644 index 0000000..9f16b4d --- /dev/null +++ b/arduino/hanzo2_1pr/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 diff --git a/arduino/hanzo3ServoSet/hanzo3ServoSet.ino b/arduino/hanzo3ServoSet/hanzo3ServoSet.ino new file mode 100644 index 0000000..25f3fe3 --- /dev/null +++ b/arduino/hanzo3ServoSet/hanzo3ServoSet.ino @@ -0,0 +1,35 @@ +#include + +Servo servo1; +Servo servo2; + +void setup() { + servo1.attach(9,1440-570,1440+570); + servo2.attach(10,1535-780,1535+780); + servo1.write(90); + servo2.write(90); + Serial.begin(9600); + Serial.print("format : [0-180][ab]"); +} + +void loop() { + static int v = 0; + + if ( Serial.available() ) { + char ch = Serial.read(); + + switch(ch) { + case '0'...'9': + v = v * 10 + ch - '0'; + break; + case 'a': + servo1.write(v); + v = 0; + break; + case 'b': + servo2.write(v); + v = 0; + break; + } + } +} diff --git a/arduino/softmodemsample/softmodemsample.ino b/arduino/softmodemsample/softmodemsample.ino index 8f3196d..d71a878 100644 --- a/arduino/softmodemsample/softmodemsample.ino +++ b/arduino/softmodemsample/softmodemsample.ino @@ -9,8 +9,6 @@ Arduinoとは次のように接続します（右側がArduinoの端子）。 ボリューム調整 -Arduinoの電源電圧に合わせて調整します。AIN1の電圧をテスターで計り、(VCC / 2) + 300mVになるよう調節します。 - * 5V電源の場合、2.7V * 3.3V電源の場合、1.95V diff --git a/arduino/test/Motor.cpp b/arduino/test/Motor.cpp new file mode 100644 index 0000000..c7e7958 --- /dev/null +++ b/arduino/test/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/test/Motor.h b/arduino/test/Motor.h new file mode 100644 index 0000000..6783d68 --- /dev/null +++ b/arduino/test/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/test/test.ino b/arduino/test/test.ino new file mode 100644 index 0000000..45da340 --- /dev/null +++ b/arduino/test/test.ino @@ -0,0 +1,495 @@ +/* +競技用ランサーロボット +半蔵 2.1 + +[7 654321 0] + | | + | | +[]---{}---[] + / .\ + / | \ +[]+--{}--+[] + +(c)2012 kou029w - MIT License [http://kou029w.appspot.com/mit-license.txt] +*/ + +#include +#include "Motor.h" + +const unsigned long LAP_COUNT = 848; //カウント/周 +const unsigned long TARGET_NONE_COUNT = LAP_COUNT*90/848; //カーブ中でのカウント数 + +const int LANCE_ANGLE_PARALLEL1 = 90 - 53; //度 +const int LANCE_ANGLE_PARALLEL2 = 90 - 43; //度 +const int LANCE_ANGLE_LEFT_VERTICAL = 90 - 120; //度 +const int LANCE_ANGLE_RIGHT_VERTICAL = 90 - 57; //度 + +const byte SPEED_DEFAULT = 128; + +// Servo : DHCM (2013-03-13) +// 60deg : 1250 us +// 90deg : 1440 us +//120deg : 1630 us +const int HANDLE_DEGREE_DEFAULT = 90; //度 +const unsigned int HANDLE_MIN = (1440-570); //us +const unsigned int HANDLE_MAX = (1440+570); //us + +const float HANDLE_KP = 4; + +//Servo : SC-0352 +// 60deg : 1265 us +// 90deg : 1535 us +//120deg : 1805 us +const int LANCE_DEGREE_DEFAULT = 90; //度 +const unsigned int LANCE_MIN = (1535-780); //us +const unsigned int LANCE_MAX = (1535+780); //us + +//const byte PIN_BUZZER = 3; +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 = 9; +const byte PIN_SERVO_LANCE = 10; + +const byte PIN_MOTOR_LEFT = 6; +const byte PIN_MOTOR_RIGHT = 5; + +enum { + MODE_STOP, //停止 + MODE_TRACE, //標準 + MODE_LEFT, //左カーブ + MODE_TARGET_PARALLEL1, //平行標的1 + MODE_TARGET_PARALLEL2, //平行標的2 + MODE_TARGET_RIGHT_VERTICAL, //右垂直標的 + MODE_TARGET_LEFT_VERTICAL, //左垂直標的 + MODE_TARGET_CYLINDER //円筒標的 +}; + +const byte MASK_MODE_TRACE = 0b01111110; +const byte MASK_CHECK_MARKER = 0b10000001; + +/**************************************/ + +Servo servoHandle; +Servo servoLance; +Motor motorR; +Motor motorL; + +byte mode; +volatile unsigned long counter = 0; +unsigned long modeChangeCounter; + +/**************************************/ + +/* センサーを読んで、車体の動作を決定する */ +void trace(byte sensor){ + switch(sensor&MASK_MODE_TRACE){ + case 0b01000000: + handle(-5*HANDLE_KP); + motorMode(GO, GO, 0, SPEED_DEFAULT*0.7); + break; + case 0b01100000: + handle(-4*HANDLE_KP); + motorMode(GO, GO, 0, SPEED_DEFAULT*0.7); + break; + case 0b00100000: + handle(-3*HANDLE_KP); + motorMode(GO, GO, 0, SPEED_DEFAULT); + break; + case 0b00110000: + handle(-4); + motorMode(GO, GO, 0, SPEED_DEFAULT); + break; + case 0b00010000: + handle(-2); + motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT); + break; + case 0b00011000: + handle(0); + motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT); + break; + case 0b00001000: + handle(2); + motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT); + break; + case 0b00001100: + handle(4); + motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT); + break; + case 0b00000100: + handle(3*HANDLE_KP); + motorMode(GO, GO, 0, 0); + break; + case 0b00000110: + handle(4*HANDLE_KP); + motorMode(GO, GO, 0, 0); + break; + case 0b00000010: + handle(5*HANDLE_KP); + motorMode(GO, GO, 0, 0); + break; + } +} + +/* MODE_LEFT時の再加速用trace() */ +void traceLeft(byte sensor){ + switch(sensor&MASK_MODE_TRACE){ + case 0b01000000: + handle(-5*HANDLE_KP); + motorMode(GO, GO, 0, SPEED_DEFAULT); + break; + case 0b01100000: + handle(-4*HANDLE_KP); + motorMode(GO, GO, 0, SPEED_DEFAULT); + break; + case 0b00100000: + handle(-3*HANDLE_KP); + motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT); + break; + case 0b00110000: + handle(-2*HANDLE_KP); + motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT); + break; + case 0b00010000: + handle(-2); + motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT); + break; + case 0b00011000: + handle(0); + motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT); + break; + case 0b00001000: + handle(2); + motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT); + break; + case 0b00001100: + handle(2*HANDLE_KP); + motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT); + break; + case 0b00000100: + handle(3*HANDLE_KP); + motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT); + break; + case 0b00000110: + handle(4*HANDLE_KP); + motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT); + break; + case 0b00000010: + handle(5*HANDLE_KP); + motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT); + break; + } +} + +/* モーターの動作を決定する */ +void motorMode(byte leftMode, byte rightMode, byte leftSpeed, byte rightSpeed){ + motorL.mode( leftMode, leftSpeed); + motorR.mode(rightMode, rightSpeed); +} + +/* ステアリングを中央からdegree[度]だけ動かす + <- - 0 + -> + | | + | | + []---{}---[] + / .\ +*/ +void handle(int degree){ + servoHandle.write(HANDLE_DEGREE_DEFAULT - degree); +} + +/* ランスを中央からdegree[度]だけ動かす + <- - 0 + -> + / .\ + / | \ + []+--{}--+[] +*/ +void lance(int degree){ + servoLance.write(LANCE_DEGREE_DEFAULT - degree); +} + +/* ロータリーエンコーダーの変化を見る + todo:外部割り込み使ってみたい、、、 +*/ +void count(){ + static byte rot[2] = {0}; //rot[0]:今の状態, rot[1]:前回の状態 + rot[0] = digitalRead(PIN_ROT); + if(rot[0] != rot[1]){ + rot[1] = rot[0]; + counter++; + } +} + +void rotRotEnc(){ + counter++; +} + +/* マーカーを読んで、モードを返す */ +int checkMarker(byte sensor){ + int mode = MODE_TRACE; + switch(sensor & MASK_CHECK_MARKER){ + case 0b10000001: + modeChangeCounter = counter; + mode = MODE_LEFT; + break; + case 0b00000001: + modeChangeCounter = counter; + if(counter%LAP_COUNT > LAP_COUNT/2){ //後半は垂直標的をねらいたい + mode = MODE_TARGET_RIGHT_VERTICAL; + }else if(counter%LAP_COUNT < LAP_COUNT/7){ //まずは、E的をねらいたい + mode = MODE_TARGET_PARALLEL1; + }else if(counter%LAP_COUNT < LAP_COUNT/3){ //次に、F的をねらいたい + mode = MODE_TARGET_PARALLEL2; + }else{ + mode = MODE_TARGET_RIGHT_VERTICAL; + } + break; + case 0b10000000: + modeChangeCounter = counter; + mode = MODE_TARGET_LEFT_VERTICAL; + break; + } + return mode; +} + +/* 前方センサーの初期化 */ +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); +} + +/* センサーの状態を返す */ +byte sensorRead(){ + byte sensor = 0; //黒が1、白が0 + if(digitalRead(PIN_SENSOR_0)) sensor += 0b00000001; + if(digitalRead(PIN_SENSOR_1)) sensor += 0b00000010; + if(digitalRead(PIN_SENSOR_2)) sensor += 0b00000100; + if(digitalRead(PIN_SENSOR_3)) sensor += 0b00001000; + if(digitalRead(PIN_SENSOR_4)) sensor += 0b00010000; + if(digitalRead(PIN_SENSOR_5)) sensor += 0b00100000; + if(digitalRead(PIN_SENSOR_6)) sensor += 0b01000000; + if(digitalRead(PIN_SENSOR_7)) sensor += 0b10000000; + return sensor; +} +/* センサーの状態を返す */ +//byte sensorRead(){ +// static byte sensor = 0; //黒が0、白が1 +// int pos = 0; +// int i = 0; +// if(!digitalRead(PIN_SENSOR_0))i++, pos += -7; +// if(!digitalRead(PIN_SENSOR_1))i++, pos += -5; +// if(!digitalRead(PIN_SENSOR_2))i++, pos += -3; +// if(!digitalRead(PIN_SENSOR_3))i++, pos += -1; +// if(!digitalRead(PIN_SENSOR_4))i++, pos += 1; +// if(!digitalRead(PIN_SENSOR_5))i++, pos += 3; +// if(!digitalRead(PIN_SENSOR_6))i++, pos += 5; +// if(!digitalRead(PIN_SENSOR_7))i++, pos += 7; +// if(i!=0) pos = pos/i; +// else return sensor; +// switch(pos){ +// case 7: +// sensor = 0b00000001; +// break; +// case 6: +// sensor = 0b00000011; +// break; +// case 5: +// sensor = 0b00000010; +// break; +// case 4: +// sensor = 0b00000110; +// break; +// case 3: +// sensor = 0b00000100; +// break; +// case 2: +// sensor = 0b00001100; +// break; +// case 1: +// sensor = 0b00001000; +// break; +// case 0: +// sensor = 0b00011000; +// break; +// case -1: +// sensor = 0b00010000; +// break; +// case -2: +// sensor = 0b00110000; +// break; +// case -3: +// sensor = 0b00100000; +// break; +// case -4: +// sensor = 0b01100000; +// break; +// case -5: +// sensor = 0b01000000; +// break; +// case -6: +// sensor = 0b11000000; +// break; +// case -7: +// sensor = 0b10000000; +// break; +// } +// return sensor; +//} + +void init_sen() { + pinMode(12, INPUT); + pinMode(13, INPUT); + pinMode(14, INPUT); + pinMode(15, INPUT); + pinMode(16, INPUT); + pinMode(17, INPUT); + pinMode(18, INPUT); + pinMode(19, INPUT); + Serial.begin(9600); +} + +void printLine() { + Serial.print(digitalRead(12)); + Serial.print('\t'); + Serial.print(digitalRead(13)); + Serial.print('\t'); + Serial.print(digitalRead(14)); + Serial.print('\t'); + Serial.print(digitalRead(15)); + Serial.print('\t'); + Serial.print(digitalRead(16)); + Serial.print('\t'); + Serial.print(digitalRead(17)); + Serial.print('\t'); + Serial.print(digitalRead(18)); + Serial.print('\t'); + Serial.print(digitalRead(19)); + Serial.print('\n'); +} +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); + motorR.attach(PIN_MOTOR_RIGHT); + motorL.mode(STOP); + motorR.mode(STOP); + //センサー + sensorInit(); + //エンコーダー + pinMode(PIN_ROT, INPUT); +// digitalWrite(PIN_ROT, HIGH); + attachInterrupt(0, rotRotEnc, FALLING); + //LED +// pinMode(PIN_LED, OUTPUT); +// digitalWrite(PIN_LED, HIGH); + //モード + mode = MODE_TRACE; + delay(3000); + /* スタート */ +// digitalWrite(PIN_LED, LOW); + handle(0); + lance(0); + motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT); +} + +void loop(){ +// printLine(); + /* センサーを見る */ + byte sensor = 0; + sensor = ~sensorRead(); //ライン(白)が1、地面(黒)が0 + + for(;;){ + sensor = ~sensorRead(); //ライン(白)が1、地面(黒)が0 + trace(sensor); + if(millis()>123000){ + motorL.mode(STOP); + motorR.mode(STOP); + for(;;){}; + } + if(counter>10000){ + motorL.mode(STOP); + motorR.mode(STOP); + for(;;){}; + } + } + count(); //ロータリーエンコーダーの変化を見る + /* 全体の動作を決定する */ + switch(mode){ + case MODE_TARGET_RIGHT_VERTICAL: //右垂直標的 + lance(LANCE_ANGLE_RIGHT_VERTICAL); + mode = MODE_TRACE; + break; + case MODE_TARGET_LEFT_VERTICAL: //左垂直標的 + lance(LANCE_ANGLE_LEFT_VERTICAL); + mode = MODE_TRACE; + break; + case MODE_TARGET_PARALLEL1: //平行標的1 + trace(sensor); + lance(LANCE_ANGLE_PARALLEL1); + if(counter - modeChangeCounter > 24){ //マーカーから少し進んで、叩く + lance(LANCE_ANGLE_PARALLEL1+8); + mode = MODE_TRACE; + } + break; + case MODE_TARGET_PARALLEL2: //平行標的2 + trace(sensor); + lance(LANCE_ANGLE_PARALLEL2); + if(counter - modeChangeCounter > 26){ //マーカーから少し進んで、叩く + lance(LANCE_ANGLE_PARALLEL2+8); + mode = MODE_TRACE; + } + break; + case MODE_TRACE: + trace(sensor); + mode = checkMarker(sensor); + if(counter > LAP_COUNT*10){ //10周したら停止 + mode = MODE_STOP; + } + break; + case MODE_LEFT: //左カーブ +// trace(sensor); + if(counter - modeChangeCounter < TARGET_NONE_COUNT/2) trace(sensor); + else traceLeft(sensor); //カーブの後半は加速したい + if(counter < LAP_COUNT) lance(30); //1周目は円筒標的をねらいたい + else lance(-30); + if(counter - modeChangeCounter > TARGET_NONE_COUNT){ + lance(0); + mode = MODE_TRACE; + } + break; +// case MODE_TARGET_CYLINDER: //円筒標的 +// digitalWrite(PIN_LED, HIGH); +// handle(0); +// lance(-90); +// motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT); +// if(sensor&MASK_CHECK_MARKER == 0b10000001){ +// modeChangeCounter = counter; +// }else if( counter - modeChangeCounter > LAP_COUNT/21 ){ +// mode = MODE_STOP; +// } +// break; + case MODE_STOP: //停止 +// digitalWrite(PIN_LED, HIGH); + motorL.mode(STOP); + motorR.mode(STOP); + break; + } +} diff --git a/arduino/toneMelodyDisk0/pitches.h b/arduino/toneMelodyDisk0/pitches.h new file mode 100644 index 0000000..55c7d54 --- /dev/null +++ b/arduino/toneMelodyDisk0/pitches.h @@ -0,0 +1,95 @@ +/************************************************* + * 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 + + diff --git a/arduino/toneMelodyDisk0/toneMelodyDisk0.ino b/arduino/toneMelodyDisk0/toneMelodyDisk0.ino new file mode 100644 index 0000000..7ed2089 --- /dev/null +++ b/arduino/toneMelodyDisk0/toneMelodyDisk0.ino @@ -0,0 +1,44 @@ +#include "pitches.h" + +void disk0(){ + /* + $pu1 + l12　o5 + c4f4b-4agfg8.g16 + b-a4b-4 + f8.d16 + $pu2 + f2. + */ + + int melodyDisk0[] = { + NOTE_C4, NOTE_F4, NOTE_AS4, NOTE_A4, NOTE_G4, NOTE_F4, NOTE_G4, + NOTE_C5, NOTE_DS5,NOTE_D5, NOTE_C5,NOTE_AS4, + NOTE_C5,NOTE_A4,NOTE_AS4, + NOTE_F4,NOTE_D4,NOTE_F4 + }; + + // note durations: 4 = quarter note, 8 = eighth note, etc.: + int noteDurationsDisk0[] = { + 4,4,4,11,11,11,4, + 4,4,11,11,11, + 4,4,4, + 6,11,2 + }; + + for (int thisNote = 0; thisNote < 19; thisNote++) { + int noteDuration = 1000/noteDurationsDisk0[thisNote]; + tone(3, melodyDisk0[thisNote],noteDuration); + int pauseBetweenNotes = noteDuration * 1.30; + delay(pauseBetweenNotes); + noTone(3); + } +} + +void setup(){ + disk0(); +} +void loop() { +} + + diff --git a/arduino/toneMelodyDisk1/pitches.h b/arduino/toneMelodyDisk1/pitches.h new file mode 100644 index 0000000..55c7d54 --- /dev/null +++ b/arduino/toneMelodyDisk1/pitches.h @@ -0,0 +1,95 @@ +/************************************************* + * 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 + + diff --git a/arduino/toneMelodyDisk1/toneMelodyDisk1.ino b/arduino/toneMelodyDisk1/toneMelodyDisk1.ino new file mode 100644 index 0000000..3265746 --- /dev/null +++ b/arduino/toneMelodyDisk1/toneMelodyDisk1.ino @@ -0,0 +1,17 @@ +#include "pitches.h" + +void disk1(){ + int melodyDisk1[] = {NOTE_B5,NOTE_D6,NOTE_G6}; + for (int thisNote = 0; thisNote < 3; thisNote++) { + tone(3, melodyDisk1[thisNote]); + delay(100); + noTone(3); + } +} +void setup() { + disk1(); +} + +void loop() { + // no need to repeat the melody. +}