- 超音波センサーによる衝突防止プログラム
#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】超音波センサーをフル活用?自動で中央へ向かってみた