new
This commit is contained in:
commit
0a6cfd416c
5 changed files with 549 additions and 0 deletions
61
Motor.cpp
Normal file
61
Motor.cpp
Normal file
|
|
@ -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;
|
||||||
|
}
|
||||||
74
Motor.h
Normal file
74
Motor.h
Normal file
|
|
@ -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 <Arduino.h>
|
||||||
|
#else
|
||||||
|
#include <WProgram.h>
|
||||||
|
#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
|
||||||
72
README.md
Normal file
72
README.md
Normal file
|
|
@ -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]
|
||||||
|
|
|
||||||
|
+-><mode>
|
||||||
|
|
|
||||||
|
+->[lance]
|
||||||
|
|
|
||||||
|
| [line]
|
||||||
|
| |
|
||||||
|
+--+->[motor/steering]
|
||||||
|
|
||||||
|
mode - 全体の動作を決定
|
||||||
|
count - ロータリーエンコーダーから得られる情報
|
||||||
|
mark - コース上のマーカーから得られる情報
|
||||||
|
line - コース上の線から得られる情報
|
||||||
|
motor - 駆動用モーターの動作
|
||||||
|
steering - ステアリングの動作
|
||||||
|
lance - ランスの動作
|
||||||
|
|
||||||
|
### ソフトウェア仕様 ###
|
||||||
|
* 使用言語 : Arduino 1.0
|
||||||
|
|
||||||
|
## ライセンス ##
|
||||||
|
(C)2013 kou029w - MIT Licence
|
||||||
273
hanzo4.ino
Normal file
273
hanzo4.ino
Normal file
|
|
@ -0,0 +1,273 @@
|
||||||
|
/*
|
||||||
|
競技用ランサーロボット
|
||||||
|
半蔵 4.0
|
||||||
|
(C)2013 kou029w - MIT License
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <Tone.h>
|
||||||
|
#include <Servo.h>
|
||||||
|
#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;
|
||||||
|
}
|
||||||
|
}
|
||||||
69
hanzo4Utils.ino
Normal file
69
hanzo4Utils.ino
Normal file
|
|
@ -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);
|
||||||
|
}
|
||||||
Loading…
Add table
Reference in a new issue