【サンプルコード集】

使用機体：3WD48mmオムニホイールロボット (10014)

超音波センサーによる衝突防止プログラム

#include SONAR.h>
#include Servo.h>

unsigned long currMillis=0;
SONAR sonar11(0x11),sonar12(0x12),sonar13(0x13);

#define MOTOR1_E 9
#define MOTOR2_E 10
#define MOTOR3_E 11
#define FORWORD 0
#define STOP 1
#define BACK 2
//**********************************************//
unsigned char motor1_Ctrl[3] = {60,45,30};
unsigned char motor2_Ctrl[3] = {60,45,30};
unsigned char motor3_Ctrl[3] = {60,45,30};
unsigned short distBuf[3];

Servo wheels;

int sonarUpdate() {
static unsigned char sonarCurr=1;
if(sonarCurr==3) sonarCurr=1;
else sonarCurr=0;
if(sonarCurr==1) {
distBuf[1]=sonar12.getDist();
sonar12.trigger();
sonar12.showDat();
} else if(sonarCurr==2) {
distBuf[2]=sonar13.getDist();
sonar13.trigger();
sonar13.showDat();
} else {
distBuf[0]=sonar11.getDist();
sonar11.trigger();
sonar11.showDat();
}
return sonarCurr;
}

//*************************************************//
void goAhead(){
analogWrite(MOTOR1_E, motor1_Ctrl[STOP]);
analogWrite(MOTOR2_E, motor2_Ctrl[BACK]);
analogWrite(MOTOR3_E, motor3_Ctrl[FORWORD]); // Revese
}
//*************************************************//
void getBack(){
analogWrite(MOTOR1_E,motor1_Ctrl[STOP]);
analogWrite(MOTOR2_E,motor2_Ctrl[FORWORD]);
analogWrite(MOTOR3_E,motor3_Ctrl[BACK]);
}
//************************************************//
void turnLeft(){
analogWrite(MOTOR1_E,motor1_Ctrl[FORWORD]); //Revese
analogWrite(MOTOR2_E,motor2_Ctrl[BACK]); //forward
analogWrite(MOTOR3_E,motor3_Ctrl[STOP]); //forward
}
void turnRight(){
analogWrite(MOTOR1_E,motor1_Ctrl[BACK]); //forward
analogWrite(MOTOR2_E,motor2_Ctrl[STOP]); //Revese
analogWrite(MOTOR3_E,motor3_Ctrl[FORWORD]); //Revese
}
//************************************************//
void RotateRight(){
analogWrite(MOTOR1_E,motor1_Ctrl[BACK]);
analogWrite(MOTOR2_E,motor2_Ctrl[BACK]);
analogWrite(MOTOR3_E,motor3_Ctrl[BACK]);
}
//*************************************************//
void RotateLeft(){
analogWrite(MOTOR1_E,motor1_Ctrl[FORWORD]);
analogWrite(MOTOR2_E,motor2_Ctrl[FORWORD]);
analogWrite(MOTOR3_E,motor3_Ctrl[FORWORD]);
}
//**************************************************//
void judge(){
if(distBuf[0]>=30){
if(distBuf[1]<=10 && distBuf[2]>10) turnRight();
else if(distBuf[2]<=10 && distBuf[1]>10) turnLeft();
else if(distBuf[1]<=10 && distBuf[2]<=10) RotateLeft();
else goAhead();
}else RotateLeft();
}
//**************************************************//
void allStop(){
analogWrite(MOTOR1_E, motor1_Ctrl[STOP]);
analogWrite(MOTOR2_E, motor2_Ctrl[STOP]);
analogWrite(MOTOR3_E, motor3_Ctrl[STOP]);
}
//*************************************************//
//void (*motion[8])()={ goAhead,RotateLeft,turnRight,RotateLeft,turnLeft,RotateLeft,judge,allStop}; //change the
void (*motion[8])()={ goAhead,RotateLeft,turnRight,RotateLeft,turnLeft,RotateLeft,goAhead,allStop}; //change the

void demowithSosars(){
unsigned char sonarcurrent=0;
if(millis()-currMillis>50/*SONAR::duration*/){ //judge if the time more than SONAR::duration;
currMillis=millis();
//sonarcurrent= sonarUpdate(); //if the requirement was ture call the function;
//Serial.println(distBuf[sonarcurrent]) ;
}
sonarUpdate();
//if(sonarcurrent==3){
if(distBuf[0] > 10){
analogWrite(MOTOR2_E, motor2_Ctrl[BACK]);
analogWrite(MOTOR3_E, motor3_Ctrl[FORWORD]);
//wheels.write(83);
}
else{
analogWrite(MOTOR2_E, motor2_Ctrl[STOP]);
analogWrite(MOTOR3_E, motor3_Ctrl[STOP]);
//wheels.write(83);
}
//delay(200);
/*unsigned char bitmap = (distBuf[0] < 20);//front
bitmap |= (distBuf[1]<20) <<1; //left
bitmap |= (distBuf[2]<20) <<2; //right:
Serial.print("bitmap=");
Serial.println(bitmap,DEC);
(*motion[bitmap])();*/
//}
}

void demoaction(){
for(int i=0;i<7;i++){
(*motion[i])();
Serial.println(i);
delay(10000);

}
}
//*************************************************//

void setup() {

TCCR1B=TCCR1B&0xf8|0x04;
TCCR2B=TCCR2B&0xf8|0x06;

//pinMode(MOTOR1_E, OUTPUT);
pinMode(MOTOR2_E, OUTPUT);
pinMode(MOTOR3_E, OUTPUT);
//wheels.attach(MOTOR1_E);

//wheels.write(83);p
delay(2000);
SONAR::init(); //call the init() from SONAR.h;

//Serial.begin(9600) ;
}
void loop(){
demowithSosars();
//demoaction();
}

本サンプルコードの解説：【その7】Nexus Robot 3WDオムニホイールロボットをぶつからない車にしてみた

三角関数を用いたオムニホイールの制御

#include SONAR.h>
#include Servo.h>

unsigned long currMillis=0;
SONAR sonar11(0x11),sonar12(0x12),sonar13(0x13);

#define MOTOR1_E 9
#define MOTOR2_E 10
#define MOTOR3_E 11
#define FORWORD 0
#define STOP 1
#define BACK 2
//**********************************************//
unsigned char motor1_Ctrl[3] = {60,45,30};
unsigned char motor2_Ctrl[3] = {60,45,30};
unsigned char motor3_Ctrl[3] = {60,45,30};
unsigned short distBuf[3];

Servo wheels;

int sonarUpdate() {
static unsigned char sonarCurr=1;
if(sonarCurr==3) sonarCurr=1;
else sonarCurr=0;
if(sonarCurr==1) {
distBuf[1]=sonar12.getDist();
sonar12.trigger();
sonar12.showDat();
} else if(sonarCurr==2) {
distBuf[2]=sonar13.getDist();
sonar13.trigger();
sonar13.showDat();
} else {
distBuf[0]=sonar11.getDist();
sonar11.trigger();
sonar11.showDat();
}
return sonarCurr;
}

//*************************************************//
void goAhead(){
analogWrite(MOTOR1_E, motor1_Ctrl[STOP]);
analogWrite(MOTOR2_E, motor2_Ctrl[BACK]);
analogWrite(MOTOR3_E, motor3_Ctrl[FORWORD]); // Revese
}
//*************************************************//
void getBack(){
analogWrite(MOTOR1_E,motor1_Ctrl[STOP]);
analogWrite(MOTOR2_E,motor2_Ctrl[FORWORD]);
analogWrite(MOTOR3_E,motor3_Ctrl[BACK]);
}
//************************************************//
void turnLeft(){
analogWrite(MOTOR1_E,motor1_Ctrl[FORWORD]); //Revese
analogWrite(MOTOR2_E,motor2_Ctrl[BACK]); //forward
analogWrite(MOTOR3_E,motor3_Ctrl[STOP]); //forward
}
void turnRight(){
analogWrite(MOTOR1_E,motor1_Ctrl[BACK]); //forward
analogWrite(MOTOR2_E,motor2_Ctrl[STOP]); //Revese
analogWrite(MOTOR3_E,motor3_Ctrl[FORWORD]); //Revese
}
//************************************************//
void RotateRight(){
analogWrite(MOTOR1_E,motor1_Ctrl[BACK]);
analogWrite(MOTOR2_E,motor2_Ctrl[BACK]);
analogWrite(MOTOR3_E,motor3_Ctrl[BACK]);
}
//*************************************************//
void RotateLeft(){
analogWrite(MOTOR1_E,motor1_Ctrl[FORWORD]);
analogWrite(MOTOR2_E,motor2_Ctrl[FORWORD]);
analogWrite(MOTOR3_E,motor3_Ctrl[FORWORD]);
}
//**************************************************//
void judge(){
if(distBuf[0]>=30){
if(distBuf[1]<=10 && distBuf[2]>10) turnRight();
else if(distBuf[2]<=10 && distBuf[1]>10) turnLeft();
else if(distBuf[1]<=10 && distBuf[2]<=10) RotateLeft();
else goAhead();
}else RotateLeft();
}
//**************************************************//
void allStop(){
analogWrite(MOTOR1_E, motor1_Ctrl[STOP]);
analogWrite(MOTOR2_E, motor2_Ctrl[STOP]);
analogWrite(MOTOR3_E, motor3_Ctrl[STOP]);
}
//*************************************************//
//void (*motion[8])()={ goAhead,RotateLeft,turnRight,RotateLeft,turnLeft,RotateLeft,judge,allStop}; //change the
void (*motion[8])()={ goAhead,RotateLeft,turnRight,RotateLeft,turnLeft,RotateLeft,goAhead,allStop}; //change the

void demowithSosars(){
unsigned char sonarcurrent=0;
if(millis()-currMillis>50/*SONAR::duration*/){ //judge if the time more than SONAR::duration;
currMillis=millis();
//sonarcurrent= sonarUpdate(); //if the requirement was ture call the function;
//Serial.println(distBuf[sonarcurrent]) ;
}
sonarUpdate();
//if(sonarcurrent==3){
if(distBuf[0] > 10){
analogWrite(MOTOR2_E, motor2_Ctrl[BACK]);
analogWrite(MOTOR3_E, motor3_Ctrl[FORWORD]);
//wheels.write(83);
}
else{
analogWrite(MOTOR2_E, motor2_Ctrl[STOP]);
analogWrite(MOTOR3_E, motor3_Ctrl[STOP]);
//wheels.write(83);
}
//delay(200);
/*unsigned char bitmap = (distBuf[0] < 20);//front
bitmap |= (distBuf[1]<20) <<1; //left
bitmap |= (distBuf[2]<20) <<2; //right:
Serial.print("bitmap=");
Serial.println(bitmap,DEC);
(*motion[bitmap])();*/
//}
}

void demoaction(){
for(int i=0;i<7;i++){
(*motion[i])();
Serial.println(i);
delay(10000);

}
}
//*************************************************//

void setup() {

TCCR1B=TCCR1B&0xf8|0x04;
TCCR2B=TCCR2B&0xf8|0x06;

//pinMode(MOTOR1_E, OUTPUT);
pinMode(MOTOR2_E, OUTPUT);
pinMode(MOTOR3_E, OUTPUT);
//wheels.attach(MOTOR1_E);

//wheels.write(83);p
delay(2000);
SONAR::init(); //call the init() from SONAR.h;

//Serial.begin(9600) ;
}
void loop(){
demowithSosars();
//demoaction();
}

本サンプルコードの解説：【その8】見よ！　これがオムニホイールロボットの真骨頂。Nexus robot 3WDを自由自在にコントロールしてみた

シリアルモニターでの無限回転サーボモーター回転方向制御

#include Servo.h>
#include stdlib.h>

#define MOTOR1 9
#define MOTOR2 10
#define MOTOR3 11

#define FOWORD1 0
#define REVERSE1 164
#define STOP1 83

#define FOWORD2 7
#define REVERSE2 171
#define STOP2 89

#define FOWORD3 6
#define REVERSE3 170
#define STOP3 88

char input[1];
int pos = 0; // サーボのポジション（変数）
int target = 0; // サーボのポジション（変数）
String inString = ""; // 受信文字列用のバッファ

Servo wheels[3];

void setup(){
wheels[0].attach(MOTOR1);
wheels[1].attach(MOTOR2);
wheels[2].attach(MOTOR3);

wheels[0].write(STOP1);
wheels[1].write(STOP2);
wheels[2].write(STOP3);

Serial.begin(9600);

}

// シリアル通信で受信したデータを数値に変換
void serialNumVal(){
while (Serial.available() > 0) { // 受信データがあったら…
int inChar = Serial.read(); // 1バイト読み込む
if (isDigit(inChar)) { // 数値だったら…
inString += (char)inChar; // 文字列を連結する
}
else if(inChar != 'n'){
if(inChar == 97){ //a
target = 0;
Serial.println("Target Change wheel0");
}
else if(inChar == 98){ //b
target = 1;
Serial.println("Target Change wheel1");
}
else if(inChar == 99){ //c
target = 2;
Serial.println("Target Change wheel2");
}
}
if (inChar == 'n') { // 改行コードLFが来たら…
pos = inString.toInt(); // 文字列を数値に変換する
Serial.println(pos);
wheels[target].write(pos);
//wheels[0].writeMicroseconds(pos);
inString = ""; // バッファクリア
}
}
}

void loop(){
serialNumVal();
/*
wheels[0].write(FOWORD1);
delay(2000);
wheels[0].write(STOP1);
delay(2000);
wheels[0].write(REVERSE1);
delay(2000);
wheels[0].write(STOP1);
delay(2000);


wheels[1].write(FOWORD2);
delay(2000);
wheels[1].write(STOP2);
delay(2000);
wheels[1].write(REVERSE2);
delay(2000);
wheels[1].write(STOP2);
delay(2000);


wheels[2].write(FOWORD3);
delay(2000);
wheels[2].write(STOP3);
delay(2000);
wheels[2].write(REVERSE3);
delay(2000);
wheels[2].write(STOP3);
delay(2000);
*/
}

本サンプルコードの解説：【その9】ロボット制御の舞台裏！？ シリアルモニターで各車輪の動きを調整してみた

キーボードによるロボットコントロール（ArduinoとPythonでのプログラムが必要）

Arduino
--------------------------------------------
#include Servo.h>

#define MOTOR1 9
#define MOTOR2 10
#define MOTOR3 11

#define FOWORD1 0
#define REVERSE1 164
#define STOP1 83

#define FOWORD2 7
#define REVERSE2 171
#define STOP2 89

#define FOWORD3 6
#define REVERSE3 170
#define STOP3 88

Servo wheels[3];

void setup(){
wheels[0].attach(MOTOR1);
wheels[1].attach(MOTOR2);
wheels[2].attach(MOTOR3);

wheels[0].write(STOP1);
wheels[1].write(STOP2);
wheels[2].write(STOP3);

Serial.begin(9600);
}

void loop(){
int inputchar; //入力状態の読み取りに使う

inputchar = Serial.read(); //シリアル通信で送信された値を読み取る

if(inputchar!=-1){
switch(inputchar){
case 'g':
wheels[0].detach();
wheels[1].attach(MOTOR2);
wheels[2].attach(MOTOR3);
wheels[1].write(FOWORD2);
wheels[2].write(REVERSE3);
break;

case 'b':
wheels[0].detach();
wheels[1].attach(MOTOR2);
wheels[2].attach(MOTOR3);
wheels[1].write(REVERSE2);
wheels[2].write(FOWORD3);
break;

case 'r':
wheels[0].attach(MOTOR1);
wheels[1].attach(MOTOR2);
wheels[2].detach();
wheels[0].write(REVERSE1);
wheels[1].write(FOWORD2);
break;

case 'l':
wheels[0].attach(MOTOR1);
wheels[1].attach(MOTOR2);
wheels[2].detach();
wheels[0].write(FOWORD1);
wheels[1].write(REVERSE2);
break;
default:
wheels[0].detach();
wheels[1].detach();
wheels[2].detach();
break;
}
}
}
--------------------------------------------

Python
--------------------------------------------
#coding:utf-8

import serial
from msvcrt import getch

def main():
#ポートとbaudレートの設定してシリアル通信を開始
with serial.Serial('COM3',9600,timeout=1) as sr:
while True:
#キーボードの入力判定
#入力に合わせて特定のコマンドを送信
key = ord(getch())
if key == 80:
flag=bytes('b','utf-8')
sr.write(flag)
elif key==72:
flag=bytes('g','utf-8')
sr.write(flag)
elif key==75:
flag=bytes('r','utf-8')
sr.write(flag)
elif key==77:
flag=bytes('l','utf-8')
sr.write(flag)
elif key==27:
break;
else:
flag=bytes('q','utf-8')
sr.write(flag)

#Escキー押されたら終了
sr.close()

if __name__ == "__main__":
main()
--------------------------------------------

本サンプルコードの解説：【その10】シリアル通信でNexus Robotをパソコンから操縦してみた

カメラユニット（Pixy）を用いた機能拡張例（物体追跡）

#include Servo.h>
#include SPI.h>
#include Pixy.h>

Pixy pixy;

#define MOTOR1 9
#define MOTOR2 10
#define MOTOR3 7

#define FOWORD1 0
#define REVERSE1 164
#define STOP1 83

#define FOWORD2 7
#define REVERSE2 171
#define STOP2 91

#define FOWORD3 6
#define REVERSE3 170
#define STOP3 89

Servo wheels[3];

void setup(){
pixy.init();

wheels[0].attach(MOTOR1);
wheels[1].attach(MOTOR2);
wheels[2].attach(MOTOR3);

wheels[0].write(STOP1);
wheels[1].write(STOP2);
wheels[2].write(STOP3);


}

void nomove(){
wheels[0].attach(MOTOR1);
wheels[1].attach(MOTOR2);
wheels[2].attach(MOTOR3);

wheels[0].write(STOP1);
wheels[1].write(STOP2);
wheels[2].write(STOP3);
}

void forward(){
//wheels[0].detach();
//wheels[1].attach(MOTOR2);
//wheels[2].attach(MOTOR3);
wheels[0].write(STOP1);
wheels[1].write(REVERSE2);
wheels[2].write(FOWORD3);
}

void backward(){
//wheels[0].detach();
//wheels[1].attach(MOTOR2);
//wheels[2].attach(MOTOR3);
wheels[0].write(STOP1);
wheels[1].write(FOWORD2);
wheels[2].write(REVERSE3);
}

void leftforward(){
//wheels[0].attach(MOTOR1);
//wheels[1].attach(MOTOR2);
//wheels[2].detach();
wheels[0].write(FOWORD1);
wheels[1].write(REVERSE2);
wheels[2].write(STOP3);
}

void leftbackward(){
//wheels[0].attach(MOTOR1);
//wheels[1].detach();
//wheels[2].attach(MOTOR3);
wheels[0].write(FOWORD1);
wheels[1].write(STOP2);
wheels[2].write(REVERSE3);
}

void rightforward(){
//wheels[0].attach(MOTOR1);
//wheels[1].detach();
//wheels[2].attach(MOTOR3);
wheels[0].write(REVERSE1);
wheels[1].write(STOP2);
wheels[2].write(FOWORD3);
}

void rightbackward(){
//wheels[0].attach(MOTOR1);
//wheels[1].attach(MOTOR2);
//wheels[2].detach();
wheels[0].write(REVERSE1);
wheels[1].write(FOWORD2);
wheels[2].write(STOP3);
}

void loop(){
static int i = 0;
static int j = 0;
static int fbFlag = 0;
uint16_t blocks;
blocks = pixy.getBlocks();



if(blocks){
i++;
j = 0;

if(i>=5){
i = 0;

if(pixy.blocks[0].width > 200 && pixy.blocks[0].height > 120){
backward();
}else if(pixy.blocks[0].x < 100){
if(pixy.blocks[0].height > 160){
fbFlag = 1;
}else if(pixy.blocks[0].height < 100){
fbFlag = 0;
}

if(fbFlag){
leftforward();
}else{
leftbackward();
}
}else if(pixy.blocks[0].x > 220){
if(pixy.blocks[0].height > 160){
fbFlag = 1;
}else if(pixy.blocks[0].height < 100){
fbFlag = 0;
}

if(fbFlag){
rightforward();
}else{
rightbackward();
}
}else if(pixy.blocks[0].width > 180){
nomove();
}else{
forward();
}
}
}else{
j++;

if(j >= 1000){
j = 0;
nomove();
}
}

}

本サンプルコードの解説：【その11】Nexusをパワーアップ！　外部デバイスを接続して機能拡張しよう！

3個の超音波センサーを活用した中心取り

#include Servo.h>
#include stdlib.h>

#define MOTOR1 9
#define MOTOR2 10
#define MOTOR3 11

#define FOWORD1 0
#define REVERSE1 164
#define STOP1 83

#define FOWORD2 7
#define REVERSE2 171
#define STOP2 89

#define FOWORD3 6
#define REVERSE3 170
#define STOP3 88

char input[1];
int pos = 0; // サーボのポジション（変数）
int target = 0; // サーボのポジション（変数）
String inString = ""; // 受信文字列用のバッファ

Servo wheels[3];

void setup(){
wheels[0].attach(MOTOR1);
wheels[1].attach(MOTOR2);
wheels[2].attach(MOTOR3);

wheels[0].write(STOP1);
wheels[1].write(STOP2);
wheels[2].write(STOP3);

Serial.begin(9600);

}

// シリアル通信で受信したデータを数値に変換
void serialNumVal(){
while (Serial.available() > 0) { // 受信データがあったら…
int inChar = Serial.read(); // 1バイト読み込む
if (isDigit(inChar)) { // 数値だったら…
inString += (char)inChar; // 文字列を連結する
}
else if(inChar != 'n'){
if(inChar == 97){ //a
target = 0;
Serial.println("Target Change wheel0");
}
else if(inChar == 98){ //b
target = 1;
Serial.println("Target Change wheel1");
}
else if(inChar == 99){ //c
target = 2;
Serial.println("Target Change wheel2");
}
}
if (inChar == 'n') { // 改行コードLFが来たら…
pos = inString.toInt(); // 文字列を数値に変換する
Serial.println(pos);
wheels[target].write(pos);
//wheels[0].writeMicroseconds(pos);
inString = ""; // バッファクリア
}
}
}

void loop(){
serialNumVal();
/*
wheels[0].write(FOWORD1);
delay(2000);
wheels[0].write(STOP1);
delay(2000);
wheels[0].write(REVERSE1);
delay(2000);
wheels[0].write(STOP1);
delay(2000);


wheels[1].write(FOWORD2);
delay(2000);
wheels[1].write(STOP2);
delay(2000);
wheels[1].write(REVERSE2);
delay(2000);
wheels[1].write(STOP2);
delay(2000);


wheels[2].write(FOWORD3);
delay(2000);
wheels[2].write(STOP3);
delay(2000);
wheels[2].write(REVERSE3);
delay(2000);
wheels[2].write(STOP3);
delay(2000);
*/
}

本サンプルコードの解説：【その12】超音波センサをフル活用？自動で中央へ向かってみた

※こちらの内容に関する質問等はお受けしておりません。
※本サンプルコードは、先頭で指定している#includeタグの後ろに「<」を付けなければ使用できません。
お手元でご利用の際は適切に「<」を入れてご使用ください。