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