ArduinoでStartStopModuleを作る
全日本ロボット相撲大会の自立型の部(autonomous)で使用する StartStopModule を Arduino nanoで作る方法をシェアします。
以降、StartStopModule を[スタートストップモジュール]、もしくは[モジュール]と記載します。
スタートストップモジュールについて
(画像はJSUMOオンラインショップページより転載)
スタートストップモジュールはJSUMOオンラインショップから購入できるので、通常は自作する必要はありません。しかし、選手が作成したモジュールも大会で使用できるので、どうしても自作したい人は、挑戦してみるのも面白いかもしれません。
注意事項と免責事項
注意事項
ロボットの誤動作は重大な事故に繋がる恐れがある為、自作したモジュールの動作確認、安全確認を必ず行ってください。
Arduino nanoを電源ノイズや電圧降下から保護する為に、電源ラインにコンデンサをつけることを推奨します。
Arduino nanoに他の仕事をさせると思わぬ動作不良を引き起こす可能性がある為、他の機能は実装せず、単体で使用することを推奨します。
免責事項
このプログラムコードを使用して製作されたモジュールで引き起こされるすべての事象について、その責任はモジュールの製作者本人が負うものとし、プログラムコードの作成者は一切の責任を負いません。すべて自己責任でお願いします。
製作したモジュールの動作
以下の手順で動作させています。
ボタン長押し(記録モードの起動)
READY信号を記録
START信号を記録
STOP信号を記録
他の信号で動作しない確認
READY信号を複数回送る
START信号を送る
STOP信号を送る
モジュールの構成
構成部品
Arduino nano :1個
赤外線リモコン受信モジュール(40kHz):1個
積層セラミックコンデンサ10μF:1個(受信モジュールにより異なる)
カーボン抵抗47Ω(1/6w):1個(受信モジュールにより異なる)
押しボタンスイッチ:1個
LED赤:1個
LED青:1個
LED紫:1個(Signalピンの動作確認用:実動作には不要)
カーボン抵抗1kΩ(1/6w):3個
Arduino nanoと各部品の接続
D2:押しボタンスイッチ ピン1(マイコン側でプルアップ)
D4:赤外線リモコン受信モジュール OUT(マイコン側でプルアップ)
A2:LED紫アノード(Signalピン:START=5V/STOP=0V)
A3:LED青アノード
A5:LED赤アノード
その他の接続
赤外線リモコン受信モジュール VCC:5V(カーボン抵抗47Ωにて接続)
赤外線リモコン受信モジュール GND:GND
押しボタンスイッチ ピン2:GND
LED紫カソード:GND(カーボン抵抗1kΩにて接続)
LED青カソード:GND(カーボン抵抗1kΩにて接続)
LED赤カソード:GND(カーボン抵抗1kΩにて接続)
積層セラミックコンデンサ10μF ピン1:赤外線リモコン受信モジュール VCC
積層セラミックコンデンサ10μF ピン2:赤外線リモコン受信モジュール GND
プログラムコード
ページの一番下に記載したArduinoスケッチをArduino IDEのスケッチにコピー&ペーストしてArduino nanoに転送すればOKです。(Arduino IDEのバージョン2.1.1で動作を確認しています)
スケッチを転送したら
まず、ボタンを長押しして記録モードを起動してください。
記録モードを起動したら
READY/START/STOP信号の記録を行ってください。
記録できる信号はソニーフォーマットのみです。
信号の記録が終わったら
記録していない信号をリモコンから送信して、誤動作しないことを確認してください。誤動作しないことを確認したら
記録した信号をリモコンから送信して、モジュールが正しく動作することを確認してください。(モジュールの動作については動画を参照)
コードの説明
Specify the pins to use
ピンの割り当て設定
ご都合に合わせてピン割り当てを変更してください。
注意:D0,D1,A6,A7は使用できません。(シリアル通信とAnalog入力専用ピンの為)
Debug mode flag
DebugMode = 1 に変更するとデバッグ用のシリアル出力がONになります。赤外線リモコンから受信したコードや動作状態を示すテキストがシリアル出力されます。
Sets the threshold for receiving infrared remote control codes
赤外線リモコンの受信処理で使用する閾値(範囲外だと受信しません)
・受信の開始からタイムアウトまでの時間
・リーダーコードの長さの上限
・リーダーコードの長さの下限
Button long press time threshold
ボタンの長押しを判定する閾値
1,000,000マイクロ秒(1秒)を設定しています。
EEPROM address to store the signal
READY/START/STOPの信号を記録するEEPROMアドレス
variables will change
プログラムで使用するグローバル変数一覧
Detect_long_button_presses()
ボタンの長押しを検出する関数
Signal_recording_mode()
リモコンの信号を記録する関数
IR_reception_data_processing()
赤外線リモコンの受信コードを読み取る関数
ソニーフォーマットのデータコード7bitを読み取ります。
その後に続くアドレスコードは無視されます。
SONYフォーマット
キャリア: 赤外線(λp = 950nm)
サブキャリア: fsc = 40kHz, 1/3duty
T = 600μs(受信の基準となる時間)
コードの流れ [リーダー(2400μs) → データ(7bit) → アドレス(5/8/13bit)]
Serial data output for debugging
コメントアウトされたシリアル出力コード
赤外線リモコンの受信処理のデバッグ用に各変数の値を出力します。
注意:すべてのコードを有効化するとシリアル出力に時間がかかりすぎる為、赤外線信号を正しく受信できなくなります。
Arduinoスケッチ
//This is a program for creating a start-stop module for the All Japan Robot Sumo Tournament with an Arduino board.
//If you use this program to create a start-stop module, be sure to check the operation.
//And please understand that you are responsible for all the events that occur as a result.
//This program accepts Sony format infrared remote control signals.
//Connect the OUT pin of the 40kHz subcarrier receiver module to IRinPin.
//Connect a push button switch connected to GND to buttonPin.
//Connect the red and blue LEDs to LED_Red and LED_Blue.
//SignalPin connects to the robot's signal input pin.
#include <EEPROM.h>
//Debug mode flag
const int DebugMode = 0; // Setting this variable to 1 turns on serial communication for debugging.
//Specify the pins to use
const int buttonPin = 2; // the number of the pushbutton pin (ON=LOW/OFF=HIGH)
const int IRinPin = 4; // the number of the IR Read pin (ON=LOW/OFF=HIGH)
const int LED_Red = 19; // the number of the LED pin (RED)
const int LED_Blue = 17; // the number of the LED pin (BLUE)
const int SignalPin = 16; // the number of the Signal Pin (START=5V/STOP=0V)
//Sets the threshold for receiving infrared remote control codes
const unsigned long Limit_Time_L_Frame = 40000; //1 Frame max 38400 micro sec (1 frame limit)
const unsigned long Limit_Time_L_upper = 3000; //Leader code is 2400 micro sec (upper limit)
const unsigned long Limit_Time_L_lower = 1800; //Leader code is 2400 micro sec (lower limit)
//Button long press time threshold
const unsigned long Button_on_Limit = 1000000; //1 micro sec (1 sec)
//EEPROM address to store the signal
const int EEPROM_Addr_READY = 1; // EEPROM Address READY
const int EEPROM_Addr_START = 2; // EEPROM Address START
const int EEPROM_Addr_STOP = 3; // EEPROM Address STOP
// variables will change
unsigned long button_now1 = 0; // Button on Time Count onry
unsigned long button_now2 = 0; // Button on Time Count onry
unsigned long Button_on_Time = 0; // Button on Time Count onry
int Run_Mode = 0; // Run Mode Status (0=disable 1=ready 2=start 3=stop)
int IR_Recording_Mode = 0; // IR Recording Mode Status (0=disable 1=ready 2=start 3=stop)
int IR_Recording_Data = 255; // IR Recording data value
int Data_READY = 255; // IR Recording data
int Data_START = 255; // IR Recording data
int Data_STOP = 255; // IR Recording data
unsigned long Limit_Time_L_Frame_now1 = 0; // Limit Time Count onry
unsigned long Limit_Time_L_Frame_now2 = 0; // Limit Time Count onry
unsigned long Base_1T = 600; //Base time 600us
unsigned long Limit_T_D0_upper = 750; //micro sec Data "0" code 600us
unsigned long Limit_T_D1_upper = 1500; //micro sec Data "1" code 1200us
unsigned long time_IR_T_off = 0; // IR Time Count onry
unsigned long time_IR_T_on = 0; // IR Time Count onry
unsigned long time_IR_Frame = 0; // Count value when IR pin turns off
unsigned long time_IR_Frame_Now1 = 0; // Limit Time Count onry
unsigned long time_IR_Frame_Now2 = 0; // Limit Time Count onry
unsigned long time_IR_Leader = 0; // Count value when IR pin turns off
unsigned long time_IR_Leader_0 = 0; // Count value when IR pin turns on
unsigned long time_IR_Data_Now = 0; // IR Time Count onry
unsigned long time_Now1 = 0; // Limit Time Count onry
unsigned long time_Now2 = 0; // Limit Time Count onry
int time_IR_Data0 = 0; // IR Received data 0
int time_IR_Data1 = 0; // IR Received data 1
int time_IR_Data2 = 0; // IR Received data 2
int time_IR_Data3 = 0; // IR Received data 3
int time_IR_Data4 = 0; // IR Received data 4
int time_IR_Data5 = 0; // IR Received data 5
int time_IR_Data6 = 0; // IR Received data 6
int IR_Data_Val = 255; // IR Received data value
int IR_Data_Read_bit = 0; // IR Received data bit Status
//Detect long button presses
void Detect_long_button_presses() {
if (digitalRead(buttonPin) == LOW && Run_Mode == 0) {
// Save counter value when buttonPin becomes LOW
button_now1 = micros();
// Wait for IR pin to go HIGH
while(digitalRead(buttonPin) == LOW){
//Measure button LOW time
button_now2 = micros();
Button_on_Time = button_now2 - button_now1;
if(Button_on_Time > Button_on_Limit){
//If a long button press is detected, a flag is set to switch to signal recording mode.
IR_Recording_Mode = 1;
IR_Recording_Data = 255;
if (DebugMode == 1) {
Serial.println("Button long press");
}
digitalWrite(LED_Red, HIGH);
digitalWrite(LED_Blue, HIGH);
delay(250);
digitalWrite(LED_Red, LOW);
digitalWrite(LED_Blue, LOW);
delay(250);
digitalWrite(LED_Red, HIGH);
digitalWrite(LED_Blue, HIGH);
delay(250);
digitalWrite(LED_Red, LOW);
digitalWrite(LED_Blue, LOW);
delay(250);
digitalWrite(LED_Red, HIGH);
digitalWrite(LED_Blue, HIGH);
delay(250);
digitalWrite(LED_Red, LOW);
digitalWrite(LED_Blue, LOW);
delay(250);
break;
}
}
}
}
//Signal recording mode
void Signal_recording_mode() {
if (IR_Recording_Mode != 0 && IR_Recording_Data != 255) {
// Stores the received value depending on the recording mode
switch (IR_Recording_Mode) {
case 1:
Data_READY = IR_Recording_Data;
IR_Recording_Mode = 2;
IR_Recording_Data = 255;
if (DebugMode == 1) {
Serial.print(Data_READY);
Serial.println(" -> READY");
}
//LED flashing
digitalWrite(LED_Red, HIGH);
digitalWrite(LED_Blue, LOW);
delay(75);
digitalWrite(LED_Red, LOW);
digitalWrite(LED_Blue, HIGH);
delay(75);
digitalWrite(LED_Red, HIGH);
digitalWrite(LED_Blue, LOW);
delay(75);
digitalWrite(LED_Red, LOW);
digitalWrite(LED_Blue, HIGH);
delay(75);
digitalWrite(LED_Red, HIGH);
digitalWrite(LED_Blue, LOW);
delay(500);
break;
case 2:
if (IR_Recording_Data != Data_READY) {
Data_START = IR_Recording_Data;
IR_Recording_Mode = 3;
IR_Recording_Data = 255;
if (DebugMode == 1) {
Serial.print(Data_START);
Serial.println(" -> START");
}
//LED flashing
digitalWrite(LED_Red, LOW);
digitalWrite(LED_Blue, HIGH);
delay(75);
digitalWrite(LED_Red, HIGH);
digitalWrite(LED_Blue, LOW);
delay(75);
digitalWrite(LED_Red, LOW);
digitalWrite(LED_Blue, HIGH);
delay(500);
}
break;
case 3:
if (IR_Recording_Data != Data_READY && IR_Recording_Data != Data_START) {
Data_STOP = IR_Recording_Data;
IR_Recording_Mode = 0;
IR_Recording_Data = 255;
//Store data in EEPROM
EEPROM.write(EEPROM_Addr_READY, Data_READY);
EEPROM.write(EEPROM_Addr_START, Data_START);
EEPROM.write(EEPROM_Addr_STOP, Data_STOP);
if (DebugMode == 1) {
Serial.print(Data_STOP);
Serial.println(" -> STOP");
}
//LED flashing
digitalWrite(LED_Red, HIGH);
digitalWrite(LED_Blue, HIGH);
delay(200);
digitalWrite(LED_Red, LOW);
digitalWrite(LED_Blue, LOW);
delay(200);
digitalWrite(LED_Red, HIGH);
digitalWrite(LED_Blue, HIGH);
delay(200);
digitalWrite(LED_Red, LOW);
digitalWrite(LED_Blue, LOW);
delay(200);
digitalWrite(LED_Red, HIGH);
digitalWrite(LED_Blue, HIGH);
delay(200);
digitalWrite(LED_Red, LOW);
digitalWrite(LED_Blue, LOW);
delay(500);
}
break;
default:
IR_Recording_Mode = 0;
}
}
}
// Infrared remote control reception data processing
void IR_reception_data_processing() {
if (IR_Data_Read_bit == 7) {
//Save data for record
IR_Recording_Data = IR_Data_Val;
// Change operation mode according to received data
if (IR_Recording_Mode == 0) {
if (IR_Data_Val == Data_READY) {
if (Run_Mode == 0 || Run_Mode == 1) {
Run_Mode = 1;
//Red light after blinking LED
digitalWrite(LED_Red, HIGH);
digitalWrite(LED_Blue, HIGH);
delay(75);
digitalWrite(LED_Red, LOW);
digitalWrite(LED_Blue, LOW);
delay(75);
digitalWrite(LED_Red, HIGH);
digitalWrite(LED_Blue, HIGH);
delay(75);
digitalWrite(LED_Red, LOW);
digitalWrite(LED_Blue, LOW);
delay(75);
digitalWrite(LED_Red, HIGH);
digitalWrite(LED_Blue, HIGH);
delay(75);
digitalWrite(LED_Red, LOW);
digitalWrite(LED_Blue, LOW);
delay(75);
digitalWrite(LED_Red, HIGH);
digitalWrite(LED_Blue, HIGH);
delay(75);
digitalWrite(LED_Red, LOW);
digitalWrite(LED_Blue, LOW);
delay(75);
digitalWrite(LED_Red, HIGH);
digitalWrite(LED_Blue, LOW);
if (DebugMode == 1) {
Serial.println("READY");
}
}
}
if (IR_Data_Val == Data_START && Run_Mode == 1) {
Run_Mode = 2;
//Switch to LED blue & turn Signal pin ON
digitalWrite(SignalPin, HIGH);
digitalWrite(LED_Red, LOW);
digitalWrite(LED_Blue, HIGH);
if (DebugMode == 1) {
Serial.println("START");
}
}
if (IR_Data_Val == Data_STOP) {
if (Run_Mode == 1 || Run_Mode == 2) {
//Turn off the Signal pin & blink the LED in an infinite loop
digitalWrite(SignalPin, LOW);
if (DebugMode == 1) {
Serial.println("STOP");
}
while(1){
digitalWrite(LED_Red, HIGH);
digitalWrite(LED_Blue, HIGH);
delay(250);
digitalWrite(LED_Red, LOW);
digitalWrite(LED_Blue, LOW);
delay(250);
}
}
}
}
if (DebugMode == 1) {
Serial.print("Code:");
Serial.println(IR_Data_Val, DEC);
}
IR_Data_Val = 255;
IR_Data_Read_bit = 0;
} else {
//When there is no received data
//Receive reader code
// check if the IR pin is turn OFF.
if (digitalRead(IRinPin) == LOW) {
// Store the counter value when the IR pin goes LOW
time_IR_T_off = micros();
time_IR_Frame = time_IR_T_off;
// IR pinがHIGHになるのを待つ
while(digitalRead(IRinPin) == LOW){
//Stop processing if the time limit is exceeded
time_Now2 = micros();
time_Now1 = time_Now2 - time_IR_T_off;
if(time_Now1 > Limit_Time_L_upper){
break;
}
}
// Stores the counter value when the IR pin goes HIGH
time_IR_T_on = micros();
// Calculate the time when the IR pin is LOW
time_IR_Leader = time_IR_T_on - time_IR_T_off;
//If the time of the leader code is within the range, proceed to data reception
if(time_IR_Leader < Limit_Time_L_upper && time_IR_Leader > Limit_Time_L_lower) {
//Calculate base time from leader code time
Base_1T = time_IR_Leader / 4;
Limit_T_D0_upper = Base_1T * 1.25;
Limit_T_D1_upper = Base_1T * 2.50;
// Loop until 7bit data is received
IR_Data_Read_bit = 0;
while(IR_Data_Read_bit <= 6){
//Confirm IR pin (receive DATA0-6 in order by 1bit with this if statement)
if (digitalRead(IRinPin) == LOW) {
// Store the counter value when the IR pin goes LOW
time_IR_T_off = micros();
time_IR_Frame = time_IR_T_off;
time_IR_Leader_0 = time_IR_T_off - time_IR_T_on;
// Wait for IR pin to go HIGH
while(digitalRead(IRinPin) == LOW){
//If HIGH time after reader code is out of range, stop processing
if(time_IR_Leader_0 > Limit_T_D0_upper){
IR_Data_Read_bit = 256;
break;
}
//Stop processing if the time limit is exceeded
time_Now2 = micros();
time_Now1 = time_Now2 - time_IR_T_off;
if(time_Now1 > Limit_T_D1_upper){
IR_Data_Read_bit = 256;
break;
}
}
// Stores the counter value when the IR pin goes HIGH
time_IR_T_on = micros();
// Calculate the time when the IR pin is LOW
time_IR_Data_Now = time_IR_T_on - time_IR_T_off;
// Save the received data to DATA0-6
switch (IR_Data_Read_bit) {
case 0:
time_IR_Data0 = time_IR_Data_Now;
break;
case 1:
time_IR_Data1 = time_IR_Data_Now;
break;
case 2:
time_IR_Data2 = time_IR_Data_Now;
break;
case 3:
time_IR_Data3 = time_IR_Data_Now;
break;
case 4:
time_IR_Data4 = time_IR_Data_Now;
break;
case 5:
time_IR_Data5 = time_IR_Data_Now;
break;
case 6:
time_IR_Data6 = time_IR_Data_Now;
break;
}
//Increase DATA reception bit by 1 -> switch to next data reception (DATA0-6)
IR_Data_Read_bit ++;
}
//Calculate elapsed time from start of reception
time_IR_Frame_Now2 = micros();
time_IR_Frame_Now1 = time_IR_Frame_Now2 - time_IR_Frame;
//If the elapsed time from the start of reception exceeds the limit, exit the loop
if(time_IR_Frame_Now1 > Limit_Time_L_Frame){
IR_Data_Read_bit = 256;
break;
}
}
}
//If 7bit data can be received, save the value in IR_Data_Val
if(IR_Data_Read_bit == 7){
IR_Data_Val = 0;
//DATA0 -> IR_Data_Val
if(time_IR_Data0 > Limit_T_D0_upper){
bitWrite(IR_Data_Val,0,1);
} else{
bitWrite(IR_Data_Val,0,0);
}
//DATA1 -> IR_Data_Val
if(time_IR_Data1 > Limit_T_D0_upper){
bitWrite(IR_Data_Val,1,1);
} else{
bitWrite(IR_Data_Val,1,0);
}
//DATA2 -> IR_Data_Val
if(time_IR_Data2 > Limit_T_D0_upper){
bitWrite(IR_Data_Val,2,1);
} else{
bitWrite(IR_Data_Val,2,0);
}
//DATA3 -> IR_Data_Val
if(time_IR_Data3 > Limit_T_D0_upper){
bitWrite(IR_Data_Val,3,1);
} else{
bitWrite(IR_Data_Val,3,0);
}
//DATA4 -> IR_Data_Val
if(time_IR_Data4 > Limit_T_D0_upper){
bitWrite(IR_Data_Val,4,1);
} else{
bitWrite(IR_Data_Val,4,0);
}
//DATA5 -> IR_Data_Val
if(time_IR_Data5 > Limit_T_D0_upper){
bitWrite(IR_Data_Val,5,1);
} else{
bitWrite(IR_Data_Val,5,0);
}
//DATA6 -> IR_Data_Val
if(time_IR_Data6 > Limit_T_D0_upper){
bitWrite(IR_Data_Val,6,1);
} else{
bitWrite(IR_Data_Val,6,0);
}
}
/*
//Serial data output for debugging (Caution: If you output all of these serially, the IR receiver program will malfunction because the transmission time will be too long.)
if (DebugMode == 1) {
Serial.println("-----------------------------");
Serial.print("time_IR_Frame:");
Serial.println(time_IR_Frame);
Serial.print("time_IR_Frame_Now1:");
Serial.println(time_IR_Frame_Now1);
Serial.print("time_IR_Frame_Now2:");
Serial.println(time_IR_Frame_Now2);
Serial.print("time_IR_T_off:");
Serial.println(time_IR_T_off);
Serial.print("time_IR_T_on:");
Serial.println(time_IR_T_on);
Serial.print("Leader:");
Serial.println(time_IR_Leader);
Serial.print("Data0:");
Serial.println(time_IR_Data0);
Serial.print("Data1:");
Serial.println(time_IR_Data1);
Serial.print("Data2:");
Serial.println(time_IR_Data2);
Serial.print("Data3:");
Serial.println(time_IR_Data3);
Serial.print("Data4:");
Serial.println(time_IR_Data4);
Serial.print("Data5:");
Serial.println(time_IR_Data5);
Serial.print("Data6:");
Serial.println(time_IR_Data6);
Serial.print("time_Now1:");
Serial.println(time_Now1);
Serial.print("time_Now2:");
Serial.println(time_Now2);
Serial.print("IR_Data_Read_bit:");
Serial.println(IR_Data_Read_bit);
Serial.print("Base_1T:");
Serial.println(Base_1T);
Serial.print("Limit_T_D0_upper:");
Serial.println(Limit_T_D0_upper);
Serial.print("Limit_T_D1_upper:");
Serial.println(Limit_T_D1_upper);
Serial.print("Limit_Time_L_Frame:");
Serial.println(Limit_Time_L_Frame);
Serial.print("Limit_Time_L_Frame_now1:");
Serial.println(Limit_Time_L_Frame_now1);
Serial.print("Limit_Time_L_Frame_now2:");
Serial.println(Limit_Time_L_Frame_now2);
Limit_Time_L_Frame_now2 = micros();
Limit_Time_L_Frame_now1 = Limit_Time_L_Frame_now2 - time_IR_Frame;
Serial.print("CT:");
Serial.println(Limit_Time_L_Frame_now1);
}
*/
//clear data
time_IR_Frame = 0;
time_IR_Frame_Now1 = 0;
time_IR_Frame_Now2 = 0;
time_IR_T_off = 0;
time_IR_T_on = 0;
time_IR_Leader = 0;
time_IR_Data0 = 0;
time_IR_Data1 = 0;
time_IR_Data2 = 0;
time_IR_Data3 = 0;
time_IR_Data4 = 0;
time_IR_Data5 = 0;
time_IR_Data6 = 0;
time_Now1 = 0;
time_Now2 = 0;
}
}
}
void setup() {
// initialize the pin
digitalWrite(SignalPin, LOW);
pinMode(LED_Red, OUTPUT);
pinMode(LED_Blue, OUTPUT);
pinMode(SignalPin, OUTPUT);
pinMode(buttonPin, INPUT_PULLUP);
pinMode(IRinPin, INPUT_PULLUP);
//EEPROM DATA READ
Data_READY = EEPROM.read(EEPROM_Addr_READY);
Data_START = EEPROM.read(EEPROM_Addr_START);
Data_STOP = EEPROM.read(EEPROM_Addr_STOP);
//Clear value if data read from EEPROM is incorrect
if (Data_READY == Data_START || Data_READY == Data_STOP || Data_START == Data_STOP) {
Data_READY = 255;
Data_START = 255;
Data_STOP = 255;
}
if (DebugMode == 1) {
Serial.begin(115200);
}
}
void loop() {
//Detect long button presses
Detect_long_button_presses();
//Signal recording mode
Signal_recording_mode();
// Infrared remote control reception data processing
IR_reception_data_processing();
}