From 9c7651bf9a843618fb9d5a453098a74f728dd5a0 Mon Sep 17 00:00:00 2001 From: kou029w Date: Fri, 22 Mar 2013 11:50:07 +0900 Subject: [PATCH] =?UTF-8?q?=E5=AE=9F=E6=A9=9F=E3=81=A7=E3=81=AE=E5=8B=95?= =?UTF-8?q?=E4=BD=9C=E6=9C=AA=E7=A2=BA=E8=AA=8D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- DigitalReadSerialX8.cpp | 34 ---- DigitalReadSerialX8.h | 31 ---- SerialUtil.cpp | 49 ------ SerialUtil.h | 18 --- ServoTest.cpp | 122 -------------- ServoTest.h | 41 ----- hanzo3.ino | 342 ++++++++++++++++++++++++++++++++++++++-- 7 files changed, 331 insertions(+), 306 deletions(-) delete mode 100644 DigitalReadSerialX8.cpp delete mode 100644 DigitalReadSerialX8.h delete mode 100644 SerialUtil.cpp delete mode 100644 SerialUtil.h delete mode 100644 ServoTest.cpp delete mode 100644 ServoTest.h diff --git a/DigitalReadSerialX8.cpp b/DigitalReadSerialX8.cpp deleted file mode 100644 index c5c0624..0000000 --- a/DigitalReadSerialX8.cpp +++ /dev/null @@ -1,34 +0,0 @@ -/* -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/SerialUtil.cpp b/SerialUtil.cpp deleted file mode 100644 index 9d1e650..0000000 --- a/SerialUtil.cpp +++ /dev/null @@ -1,49 +0,0 @@ -/* -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/ServoTest.cpp b/ServoTest.cpp deleted file mode 100644 index dd3d370..0000000 --- a/ServoTest.cpp +++ /dev/null @@ -1,122 +0,0 @@ -/* -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/ServoTest.h b/ServoTest.h deleted file mode 100644 index e875aed..0000000 --- a/ServoTest.h +++ /dev/null @@ -1,41 +0,0 @@ -/* -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/hanzo3.ino b/hanzo3.ino index 9ea3301..2154561 100644 --- a/hanzo3.ino +++ b/hanzo3.ino @@ -3,20 +3,340 @@ 半蔵 3.0 (C)2013 kou029w - MIT License */ -#include -#include "ServoTest.h" -//#include "SerialUtil.h" -#ifdef __AVR_ATmega32U4__ -ServoTestClass ServoTest; -#else -ServoTestClass ServoTest; -#endif +#include +#include +#include "Motor.h" + +const unsigned long LAP_DISTANCE = 10000; //ロータリーエンコーダーのカウント数/周 +const unsigned long TURN_DISTANCE = LAP_DISTANCE*90/848; //カーブ中でのカウント数 + +/*ここまだ*/ +const int LANCE_ANGLE_PARALLEL_1 = 90 - 53; //度 +const int LANCE_ANGLE_PARALLEL_2 = 90 - 43; //度 +const int LANCE_ANGLE_VERTICAL_L = 90 - 120; //度 +const int LANCE_ANGLE_VERTICAL_R = 90 - 57; //度 + +const byte SPEED_DEFAULT = 128; + +// Servo : DHCM (2013-03-13) +// 60deg : 1250 us +// 90deg : 1440 us +//120deg : 1630 us +const int STEERING_DEFAULT = 90; //度 +const unsigned int STEERING_MIN = (1440-570); //us +const unsigned int STEERING_MAX = (1440+570); //us + +const float STEERING_KP = 4.0; + +//Servo : SC-0352 +// 60deg : 1265 us +// 90deg : 1535 us +//120deg : 1805 us +const int LANCE_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_STEERING = 9; +const byte PIN_SERVO_LANCE = 10; + +const byte PIN_MOTOR_LEFT = 6; +const byte PIN_MOTOR_RIGHT = 5; + +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_ATTACK //標的を狙う +} +mode; + +enum target_t { + TARGET_NONE, + TARGET_PARALLEL_1, //平行標的1 + TARGET_PARALLEL_2, //平行標的2 + TARGET_VERTICAL_R, //右垂直標的 + TARGET_VERTICAL_L, //左垂直標的 + TARGET_CYLINDER //円筒標的 +} +target; + +/**************************************/ + +/* センサーを読んで、車体の動作を決定する */ +void trace(){ + switch(sensor&MASK_LINE){ + case 0b01000000: + steering(-5*STEERING_KP); + motorL.speed(0); + motorR.speed(SPEED_DEFAULT*0.7); + break; + case 0b01100000: + steering(-4*STEERING_KP); + motorL.speed(0); + motorR.speed(SPEED_DEFAULT*0.7); + break; + case 0b00100000: + steering(-3*STEERING_KP); + motorL.speed(0); + motorR.speed(SPEED_DEFAULT); + break; + case 0b00110000: + steering(-2*STEERING_KP); + motorL.speed(0); + motorR.speed(SPEED_DEFAULT); + break; + case 0b00010000: + steering(-1); + 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(1); + motorL.speed(SPEED_DEFAULT); + motorR.speed(SPEED_DEFAULT); + break; + case 0b00001100: + steering(2*STEERING_KP); + motorL.speed(SPEED_DEFAULT); + motorR.speed(SPEED_DEFAULT); + break; + case 0b00000100: + steering(3*STEERING_KP); + motorL.speed(0); + motorR.speed(0); + break; + case 0b00000110: + steering(4*STEERING_KP); + motorL.speed(0); + motorR.speed(0); + break; + case 0b00000010: + steering(5*STEERING_KP); + motorL.speed(0); + motorR.speed(0); + 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; //黒が1、白が0 + if(digitalRead(PIN_SENSOR_0)) b += 0b00000001; //右端 + if(digitalRead(PIN_SENSOR_1)) b += 0b00000010; + if(digitalRead(PIN_SENSOR_2)) b += 0b00000100; + if(digitalRead(PIN_SENSOR_3)) b += 0b00001000; + if(digitalRead(PIN_SENSOR_4)) b += 0b00010000; + if(digitalRead(PIN_SENSOR_5)) b += 0b00100000; + if(digitalRead(PIN_SENSOR_6)) b += 0b01000000; + if(digitalRead(PIN_SENSOR_7)) b += 0b10000000; //左端 + //今回ラインが白なので、反転して、ラインを1とする + b ^= 0xff; + return sensor = b; +} + +void modeSet(){ + if(distance > LAP_DISTANCE*5){ //5周したら停止 + mode = MODE_STOP; + return; + } + switch(sensor&MASK_MARKER){ + default: + case 0b00000000: + mode = MODE_STRAIGHT; + target = TARGET_NONE; + case 0b10000001: + modeChangedDistance = distance; + mode = MODE_TURN; + target = TARGET_NONE; + break; + case 0b00000001: + modeChangedDistance = distance; + mode = MODE_ATTACK; + if(distance%LAP_DISTANCE < LAP_DISTANCE/7){ //まずは、E的をねらいたい + target = TARGET_PARALLEL_1; + }else if(distance%LAP_DISTANCE < LAP_DISTANCE/3){ //次に、F的をねらいたい + target = TARGET_PARALLEL_2; + }else{ //後半は垂直標的をねらいたい + target = TARGET_VERTICAL_R; + } + break; + case 0b10000000: + modeChangedDistance = distance; + mode = MODE_ATTACK; + target = TARGET_VERTICAL_L; + break; + } +} + +void stop(){ + motorL.mode(STOP); + motorR.mode(STOP); +} + +void straight(){ + trace(); + modeSet(); +} + +void turn(){ + trace(); + lance(0); + if(distance - modeChangedDistance > TURN_DISTANCE){ + modeSet(); + } +} + +void attack(){ + switch(target){ + default: + case TARGET_NONE: + trace(); + modeSet(); + break; + case TARGET_PARALLEL_1: //平行標的1 + trace(); + lance(LANCE_ANGLE_PARALLEL_1); + if(distance - modeChangedDistance > 24){ //マーカーから少し進んで、叩く + lance(LANCE_ANGLE_PARALLEL_1+8); + modeSet(); + } + break; + case TARGET_PARALLEL_2: //平行標的2 + trace(); + lance(LANCE_ANGLE_PARALLEL_2); + if(distance - modeChangedDistance > 26){ //マーカーから少し進んで、叩く + lance(LANCE_ANGLE_PARALLEL_2+8); + modeSet(); + } + break; + case TARGET_VERTICAL_R: //右垂直標 + trace(); + lance(LANCE_ANGLE_VERTICAL_R); + modeSet(); + break; + case TARGET_VERTICAL_L: //左垂直標 + trace(); + lance(LANCE_ANGLE_VERTICAL_L); + modeSet(); + break; + case TARGET_CYLINDER: //円筒標的 + trace(); + modeSet(); + break; + } +} + +void rot(){ + distance++; +} void setup(){ - Serial.begin(9600); - ServoTest(Serial); + //サーボ + steeringServo.attach(PIN_SERVO_STEERING, STEERING_MIN, STEERGING_MAX); + lanceServo.attach(PIN_SERVO_LANCE, LANCE_MIN, LANCE_MAX); + steering(0); + lance(0); + //モーター + motorL.attach(PIN_MOTOR_LEFT); + motorR.attach(PIN_MOTOR_RIGHT); + motorL.mode(STOP); + motorR.mode(STOP); + //センサー + sensorInit(); + //エンコーダー + pinMode(PIN_ROT, INPUT); + attachInterrupt(0, rot, FALLING); + /* 3秒停止 */ + stop(); + delay(3000); + /* スタート */ + modeSet(); } void loop(){ -} + sensorRead(); + switch(mode){ + default: + case MODE_STOP: //停止 + stop(); + break; + case MODE_STRAIGHT: //まっすぐ進む + straight(); + break; + case MODE_TURN: //左に曲がる + turn(); + break; + case MODE_ATTACK: //標的を狙う + attack(); + break; + } +}