update
This commit is contained in:
parent
4fd7458d7a
commit
4d0f0f6f6c
26 changed files with 2105 additions and 2 deletions
|
|
@ -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
61
arduino/MotorSet/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;
|
||||||
|
}
|
||||||
75
arduino/MotorSet/Motor.h
Normal file
75
arduino/MotorSet/Motor.h
Normal 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
31
arduino/MotorSet/MotorSet.ino
Normal 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
34
arduino/dev/DigitalReadSerialX8.cpp
Normal 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
31
arduino/dev/DigitalReadSerialX8.h
Normal 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
55
arduino/dev/PDControl.cpp
Normal 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
38
arduino/dev/PDControl.h
Normal 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
49
arduino/dev/SerialUtil.cpp
Normal 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
18
arduino/dev/SerialUtil.h
Normal 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
122
arduino/dev/ServoTest.cpp
Normal 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
41
arduino/dev/ServoTest.h
Normal 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
3
arduino/dev/dev.ino
Normal 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
47
arduino/hanzo2_1pr/Motor.cpp
Normal 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
43
arduino/hanzo2_1pr/Motor.h
Normal 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
412
arduino/hanzo2_1pr/hanzo2_1pr.ino
Normal 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
93
arduino/hanzo2_1pr/pitches.h
Normal 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
35
arduino/hanzo3ServoSet/hanzo3ServoSet.ino
Normal 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;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
@ -9,8 +9,6 @@ Arduinoとは次のように接続します（右側がArduinoの端子）。
|
||||||
|
|
||||||
ボリューム調整
|
ボリューム調整
|
||||||
|
|
||||||
Arduinoの電源電圧に合わせて調整します。AIN1の電圧をテスターで計り、(VCC / 2) + 300mVになるよう調節します。
|
|
||||||
|
|
||||||
* 5V電源の場合、2.7V
|
* 5V電源の場合、2.7V
|
||||||
* 3.3V電源の場合、1.95V
|
* 3.3V電源の場合、1.95V
|
||||||
|
|
||||||
|
|
|
||||||
61
arduino/test/Motor.cpp
Normal file
61
arduino/test/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;
|
||||||
|
}
|
||||||
75
arduino/test/Motor.h
Normal file
75
arduino/test/Motor.h
Normal 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
495
arduino/test/test.ino
Normal 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
95
arduino/toneMelodyDisk0/pitches.h
Normal 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
44
arduino/toneMelodyDisk0/toneMelodyDisk0.ino
Normal 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
95
arduino/toneMelodyDisk1/pitches.h
Normal 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
17
arduino/toneMelodyDisk1/toneMelodyDisk1.ino
Normal 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.
|
||||||
|
}
|
||||||
Loading…
Add table
Reference in a new issue