nebel/2014-git-work
1
0
Fork 0
Code Issues Pull requests Projects Releases Wiki Activity

update

Browse source
This commit is contained in:
Nebel 2013-09-16 17:37:33 +09:00
parent 4fd7458d7a
commit 4d0f0f6f6c
26 changed files with 2105 additions and 2 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

35
arduino/Hanzo3ServoSetMicroseconds/Hanzo3ServoSetMicroseconds.ino Normal file
View file

@ -0,0 +1,35 @@
#include <Servo.h>
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;
}
}
}

61
arduino/MotorSet/Motor.cpp Normal file
View 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;
}

75
arduino/MotorSet/Motor.h Normal file
View file

@ -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 <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

31
arduino/MotorSet/MotorSet.ino Normal file
View file

@ -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;
}
}
}

34
arduino/dev/DigitalReadSerialX8.cpp Normal file
View file

@ -0,0 +1,34 @@
/*
DigitalReadSerialX8.cpp
(C)2013 kou029w - MIT License
*/
#if defined(ARDUINO) && ARDUINO >= 100
#include <Arduino.h>
#else
#include <WProgram.h>
#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<typename T>
void DigitalReadSerialX8(T& serial, uint8_t minPin=12){
if(!serial) return;
for(int i=minPin; i<minPin+8; i++){
pinMode(i, INPUT);
}
serial.println("Press any key to end.");
delay(1000);
while(serial.available() == 0){
for(int i=minPin; i<minPin+8; i++){
serial.print('\t');
serial.print(digitalRead(i));
}
serial.print('\n');
}
}

31
arduino/dev/DigitalReadSerialX8.h Normal file
View file

@ -0,0 +1,31 @@
/*
DigitalReadSerialX8.h - センサーアレイ(8つ)の動作を確認するためのライブラリ
## 概要 ##
このライブラリは、シリアルモニター上から8つのセンサーアレイの動作を確認するためのシンプルなライブラリです。
## 使い方 ##
例:
#include "DigitalReadSerialX8.h"
void setup(){
Serial.begin(9600);
DigitalReadSerial_x8(Serial, 12); //この場合、pin12-pin19をみる
}
void loop(){
}
## ライセンス ##
(C)2013 kou029w - MIT License
*/
#ifndef DigitalReadSerialX8_h
#define DigitalReadSerialX8_h
#if defined(ARDUINO) && ARDUINO >= 100
#include <Arduino.h>
#else
#include <WProgram.h>
#endif
template<typename T> void DigitalReadSerialX8(T& serial, uint8_t minPin);
#endif

55
arduino/dev/PDControl.cpp Normal file
View file

@ -0,0 +1,55 @@
/*
PDControl.cpp
(C)2013 kou029w - MIT License
*/
#include <Arduino.h>
#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;
}

38
arduino/dev/PDControl.h Normal file
View file

@ -0,0 +1,38 @@
/*
PDControl.h
(C)2013 kou029w - MIT License
*/
#ifndef PDControl_h
#define PDControl_h
#include <Arduino.h>
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

49
arduino/dev/SerialUtil.cpp Normal file
View file

@ -0,0 +1,49 @@
/*
SerialUtil.cpp
(C)2013 kou029w - MIT License
*/
#include <Arduino.h>
#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<typename T>
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<typename T>
String getStr(size_t size, T& serial){
if(!serial) return String("");
String str = "";
for(size_t i = 0; i<size;){
if(serial.available()){
char c = serial.read();
str.concat(String(c));
i++;
}
}
return str;
}

18
arduino/dev/SerialUtil.h Normal file
View file

@ -0,0 +1,18 @@
/*
SerialUtil.h - Serialから文字列を受け取るためのライブラリ
## ライセンス ##
(C)2013 kou029w - MIT License
*/
#ifndef SerialUtil_h
#define SerialUtil_h
#include <Arduino.h>
template<typename T>
String inputLine(String str, T& serial);
template<typename T>
String getStr(size_t size, T& serial);
#endif

122
arduino/dev/ServoTest.cpp Normal file
View file

@ -0,0 +1,122 @@
/*
ServoTest.cpp
(C)2013 kou029w - MIT License
*/
#include <Arduino.h>
#include <Servo.h>
#include "ServoTest.h"
#include "SerialUtil.h"
//#ifdef Servo_h
template<>
class ServoTestClass<Servo,HardwareSerial>;
#ifdef __AVR_ATmega32U4__
template<>
class ServoTestClass<Servo,Serial_>;
#endif
//#endif
template<typename SERVO, typename S>
void ServoTestClass<SERVO,S>::_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<typename SERVO, typename S>
void ServoTestClass<SERVO,S>::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;
}
}
}
}

41
arduino/dev/ServoTest.h Normal file
View file

@ -0,0 +1,41 @@
/*
ServoTest.h - サーボの動作を確認するためのライブラリ
## 概要 ##
このライブラリは、シリアルモニター上からサーボの動作を確認するためのシンプルなライブラリです。
## 使い方 ##
例:
#include <Servo.h>
#include "ServoTest.h"
void setup(){
Serial.begin(9600);
ServoTest(Serial);
}
void loop(){
}
## ライセンス ##
(C)2013 kou029w - MIT License
*/
#ifndef ServoTest_h
#define ServoTest_h
#include <Arduino.h>
#include <Servo.h>
#include "SerialUtil.h"
template<typename SERVO, typename S>
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

3
arduino/dev/dev.ino Normal file
View file

@ -0,0 +1,3 @@
int analogRead(uint8_t pin, uint8_t max){
return ((long)analogRead(pin)*max)>>10;
}

47
arduino/hanzo2_1pr/Motor.cpp Normal file
View file

@ -0,0 +1,47 @@
/*
Motor.cpp
(c)2012 kou029w - MIT License [http://kou029w.appspot.com/mit-license.txt]
*/
#if defined(ARDUINO) && ARDUINO >= 100
#include <Arduino.h>
#else
#include <WProgram.h>
#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);
}

43
arduino/hanzo2_1pr/Motor.h Normal file
View file

@ -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 <Arduino.h>
#else
#include <WProgram.h>
#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

412
arduino/hanzo2_1pr/hanzo2_1pr.ino Normal file
View file

@ -0,0 +1,412 @@
/*
競技用ランサーロボット
半蔵 2.1 - PR用
[7 654321 0]
| |
| |
[]---{}---[]
/ .\
/ | \
[]+--{}--+[]
(C)2013 kou029w - MIT License [http://kou029w.appspot.com/mit-license.txt]
*/
#include <Servo.h>
#include "Motor.h"
#include "pitches.h"
void disk0(){
/*
$pu1
l12　o5
c4f4b-4agfg8.g16
<c4e-4dc>b-<c4>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;
}
}

93
arduino/hanzo2_1pr/pitches.h Normal file
View file

@ -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

35
arduino/hanzo3ServoSet/hanzo3ServoSet.ino Normal file
View file

@ -0,0 +1,35 @@
#include <Servo.h>
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;
}
}
}

2
arduino/softmodemsample/softmodemsample.ino
View file

@ -9,8 +9,6 @@ Arduinoとは次のように接続します（右側がArduinoの端子）。
ボリューム調整
Arduinoの電源電圧に合わせて調整します。AIN1の電圧をテスターで計り、(VCC / 2) + 300mVになるよう調節します。
* 5V電源の場合、2.7V
* 3.3V電源の場合、1.95V

61
arduino/test/Motor.cpp Normal file
View 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;
}

75
arduino/test/Motor.h Normal file
View file

@ -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 <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

495
arduino/test/test.ino Normal file
View file

@ -0,0 +1,495 @@
/*
競技用ランサーロボット
半蔵 2.1
[7 654321 0]
| |
| |
[]---{}---[]
/ .\
/ | \
[]+--{}--+[]
(c)2012 kou029w - MIT License [http://kou029w.appspot.com/mit-license.txt]
*/
#include <Servo.h>
#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;
}
}

95
arduino/toneMelodyDisk0/pitches.h Normal file
View file

@ -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

44
arduino/toneMelodyDisk0/toneMelodyDisk0.ino Normal file
View file

@ -0,0 +1,44 @@
#include "pitches.h"
void disk0(){
/*
$pu1
l12　o5
c4f4b-4agfg8.g16
<c4e-4dc>b-<c4>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() {
}

95
arduino/toneMelodyDisk1/pitches.h Normal file
View file

@ -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

17
arduino/toneMelodyDisk1/toneMelodyDisk1.ino Normal file
View file

@ -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.
}