commit 0a6cfd416cd3289669829503b6d35f256804ec86 Author: kou029w Date: Thu Sep 26 22:09:41 2013 +0900 new diff --git a/Motor.cpp b/Motor.cpp new file mode 100644 index 0000000..873239a --- /dev/null +++ b/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/Motor.h b/Motor.h new file mode 100644 index 0000000..9c49f4a --- /dev/null +++ b/Motor.h @@ -0,0 +1,74 @@ +/* +Motor.h - モータードライバ(L298P シールド)のためのライブラリ + +## 概要 ## +このライブラリは、モータードライバ(L298P シールド)のためのシンプルなライブラリです。 +PWM対応ピンが1つしか必要ないのがこのライブラリの特徴です。 + +## 使い方 ## +例: + #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 + STOP | L | H + BACK | 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/README.md b/README.md new file mode 100644 index 0000000..3103263 --- /dev/null +++ b/README.md @@ -0,0 +1,72 @@ +# 競技用ランサーロボット 半蔵4.0 制御プログラム # + +## これはなに ## +[ロボットグランプリ](http://www.robotgrandprix.com/)に向けて製作中のマシン、半蔵4.0のための制御プログラムです。 + +## システム構成 ## +以下に、半蔵4.0のシステム構成を示します。 + +### Hardware ### + [@ @@@@@@ @] - Sensor array + | | + | | | + []---{|---[] - Servomotor for steering + / |\ + / {} \ - Servomotor for lance + []+------+[] - Motor for driving + $ - Rotary encoder + + [Sensor array] + | [Rotary encoder] + | | + +--+->[MPU] + | + +->[Motor for driving] + +->[Servomotor for steering] + +->[Servomotor for lance] + + MPU - Atmel AVR + Rotary encoder - ロータリーエンコーダー + Sensor array - 反射型フォトインタラプタSG-2BC + Motor for driving - 駆動用モーター/ハイスピードギアボックスHE + Servomotor for steering - ステアリング用サーボモーター + Servomotor for lance - ランス用サーボモーター + +### 車体仕様 ### +* ホイールベース : 約150mm +* 車重 : 1kg以下 +* 車体材料 : タミヤ ユニバーサルプレートL +* 使用ギアボックス : タミヤ ハイスピードギアボックスHE +* ギア比 : 11.6:1 +* 使用モーター : 上記ギアボックスに付属するもの +* 使用サーボモーター : Savox SC-0352(ランス用) + +### 回路仕様 ### +* 動作電圧 : 7.2V以上 +* 使用マイコン : Atmel AVR (Arduino) +* 使用センサーアレイ : SG-2BC x 8 (ロボラボ講習会のもの) + +### Software ### + [count/mark] + | + +-> + | + +->[lance] + | + | [line] + | | + +--+->[motor/steering] + + mode - 全体の動作を決定 + count - ロータリーエンコーダーから得られる情報 + mark - コース上のマーカーから得られる情報 + line - コース上の線から得られる情報 + motor - 駆動用モーターの動作 + steering - ステアリングの動作 + lance - ランスの動作 + +### ソフトウェア仕様 ### +* 使用言語 : Arduino 1.0 + +## ライセンス ## +(C)2013 kou029w - MIT Licence \ No newline at end of file diff --git a/hanzo4.ino b/hanzo4.ino new file mode 100644 index 0000000..91444ec --- /dev/null +++ b/hanzo4.ino @@ -0,0 +1,273 @@ +/* +競技用ランサーロボット +半蔵 4.0 +(C)2013 kou029w - MIT License +*/ + +#include +#include +#include "Motor.h" + +const unsigned long LAP_DISTANCE = 28500; //ロータリーエンコーダーのカウント数/周 +const unsigned long TURN_DISTANCE = 2000; //カーブ中でのカウント数 + +const int LANCE_ANGLE_PARALLEL_1 = 55; //度 +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 = 127; + +//const float STEERING_KP = 3.8; + +// Servo : SC-1267SG +const unsigned int STEERING_CENTER = 1810; //us +const unsigned int STEERING_MIN = (STEERING_CENTER-780); //us +const unsigned int STEERING_MAX = (STEERING_CENTER+780); //us + +//Servo : SC-0352 +const unsigned int LANCE_CENTER = 1450; //us +const unsigned int LANCE_MIN = (LANCE_CENTER-780); //us +const unsigned int LANCE_MAX = (LANCE_CENTER+780); //us + +const unsigned char PIN_BUZZER = 3; +const unsigned char PIN_ROT = 2; + +const unsigned char PIN_SENSOR_0 = 19; //右端 +const unsigned char PIN_SENSOR_1 = 18; +const unsigned char PIN_SENSOR_2 = 17; +const unsigned char PIN_SENSOR_3 = 16; +const unsigned char PIN_SENSOR_4 = 15; +const unsigned char PIN_SENSOR_5 = 14; +const unsigned char PIN_SENSOR_6 = 13; +const unsigned char PIN_SENSOR_7 = 12; //左端 + +const unsigned char PIN_SERVO_STEERING = 9; +const unsigned char PIN_SERVO_LANCE = 10; + +const unsigned char PIN_MOTOR_LEFT_E = 5; +const unsigned char PIN_MOTOR_LEFT_M = 4; +const unsigned char PIN_MOTOR_RIGHT_E = 6; +const unsigned char PIN_MOTOR_RIGHT_M = 7; + +const unsigned char MASK_LINE = 0b01111110; +const unsigned char MASK_MARKER = 0b10000001; + +/**************************************/ + +Servo steeringServo; +Servo lanceServo; +Motor motorR; +Motor motorL; + +//ラインが1、地面が0、LSBが左端、MSBが右端 +unsigned char sensor = 0x00; +int errorCount = 0; + +int speed = SPEED_DEFAULT; + +volatile unsigned long distance = 0; +unsigned long modeChangedDistance = 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(){ + switch(sensor&MASK_LINE){ + case 0b01000000: + steering(-22); + motorL.speed(-speed/2); + motorR.speed(speed/2); + break; + case 0b01100000: + steering(-20); + motorL.speed(-speed/2); + motorR.speed(speed/2); + break; + case 0b00100000: + steering(-18); + motorL.speed(-speed/2); + motorR.speed(speed/2); + break; + case 0b00110000: + steering(-2); + motorL.speed(speed); + motorR.speed(speed); + break; + case 0b00010000: + steering(-1); + motorL.speed(speed); + motorR.speed(speed); + break; + case 0b00011000: + steering(0); + motorL.speed(speed); + motorR.speed(speed); + break; + case 0b00001000: + steering(1); + motorL.speed(speed); + motorR.speed(speed); + break; + case 0b00001100: + steering(2); + motorL.speed(speed); + motorR.speed(speed); + break; + case 0b00000100: + steering(3); + motorL.speed(0); + motorR.speed(0); + break; + case 0b00000110: + steering(4); + motorL.speed(0); + motorR.speed(0); + break; + case 0b00000010: + steering(5); + motorL.speed(0); + motorR.speed(0); + break; + } +} + +/* モードを決定する */ +void modeSet(){ + if(sensor == 0) errorCount++; + else errorCount = 0; + if(distance > LAP_DISTANCE*5 || errorCount > 10000){ + mode = MODE_STOP; + return; + } + switch(sensor&MASK_MARKER){ + default: + case 0b00000000: + mode = MODE_STRAIGHT; + break; + case 0b10000001: + modeChangedDistance = distance; + mode = MODE_TURN; + break; + case 0b00000001: + modeChangedDistance = distance; + if(distance%LAP_DISTANCE < 1429){ //まずは、E的をねらいたい + mode = MODE_ATTACK_PARALLEL_1; + }else if(distance%LAP_DISTANCE < 3333){ //次に、F的をねらいたい + mode = MODE_ATTACK_PARALLEL_2; + }else{ //後半は垂直標的をねらいたい + mode = MODE_ATTACK_VERTICAL_R; + } + break; + case 0b10000000: + modeChangedDistance = distance; + if(distance%LAP_DISTANCE > 5000) mode = MODE_ATTACK_VERTICAL_L; + break; + } +} + +void setup(){ +// Serial.begin(9600); + //サーボ + steeringServo.attach(PIN_SERVO_STEERING, STEERING_MIN, STEERING_MAX); + lanceServo.attach(PIN_SERVO_LANCE, LANCE_MIN, LANCE_MAX); + steering(0); + lance(0); + //モーター + motorL.attach(PIN_MOTOR_LEFT_E, PIN_MOTOR_LEFT_M); + motorR.attach(PIN_MOTOR_RIGHT_E, PIN_MOTOR_RIGHT_M); + motorL.mode(STOP); + motorR.mode(STOP); + //センサー + sensorInit(); + //エンコーダー + 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); + sensorRead(); + switch(mode){ + default: + case MODE_STOP: //停止 + steering(-20); + motorL.mode(STOP); + motorR.mode(STOP); + stopBeep(); + while(1){ + ; + }; + break; + case MODE_STRAIGHT: //まっすぐ進む + trace(); + modeSet(); + break; + case MODE_TURN: //左に曲がる + trace(); + lance(0); + if(distance - modeChangedDistance > TURN_DISTANCE){ + mode = MODE_STRAIGHT; + } + break; + case MODE_ATTACK_PARALLEL_1: //平行標的1 + trace(); + lance(LANCE_ANGLE_PARALLEL_1); + if(distance - modeChangedDistance > 1200){ //マーカーから少し進んで、叩く + 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 > 1200){ //マーカーから少し進んで、叩く + 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 > 300){ //マーカーから少し進んで、まっすぐに戻す + lance(0); + mode = MODE_STRAIGHT; + } + break; + case MODE_ATTACK_CYLINDER: //円筒標的 + trace(); + mode = MODE_STRAIGHT; + break; + } +} diff --git a/hanzo4Utils.ino b/hanzo4Utils.ino new file mode 100644 index 0000000..ed7a9d3 --- /dev/null +++ b/hanzo4Utils.ino @@ -0,0 +1,69 @@ +/* ステアリングを中央からa[度]だけ動かす + <- - 0 + -> + || + || + []---{}---[] + / .\ +*/ +void steering(int a){ + steeringServo.write(90 + a); +} + +/* ランスを中央からa[度]だけ動かす + <- - 0 + -> + / .\ + / | \ + []+--{}--+[] +*/ +void lance(int a){ + lanceServo.write(90 - 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); //左端 +} + +/* センサーアレイの状態を見る*/ +unsigned char sensorRead(){ + unsigned char 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; //左端 + b ^= 0xff; //今回ラインが白なので、反転して、ラインを1とする + return sensor = b; +} + +/* エンコーダーの割り込み処理 */ +void rot(){ + distance++; +} + +/* 起動音 */ +void startBeep(){ + tone(PIN_BUZZER,2000); + delay(100); + tone(PIN_BUZZER,1000); + delay(100); + noTone(PIN_BUZZER); +} + +/* 終了音 */ +void stopBeep(){ + tone(PIN_BUZZER, 2000); + delay(2000); + noTone(PIN_BUZZER); +}