จากการทดสอบ เมื่อเลือกใช้ รุ่น MG996R เซอร์โวมอเตอร์จะเป็น เฟืองโลหะ ต้องเพิ่ม Jack 5.5 X 2.1mm สำหรับ เพาเวอร์ซัพพลาย ของ Arduino ตัวผู้ ต่อจากรางถ่าน ขั้วบวก + สีแดง และ ขั้วลบ- สีดำ ของ แบตเตอรี่ 18650 เข้าไปยัง Jack ซัพพลายตัวเมีย ของ Arduino UNO R3 ดังรูปด้วย
จากนั้นทดสอบ หุ่นยนต์หลบสิ่งกีดขวาง ได้โดย Upload โค้ดนี้ ไปยัง บอร์ด Arduino UNO R3
#include "IRremote.h"
#include
//Pins for motor A
const int MotorA1 = 6;
const int MotorA2 = 7;
//Pins for motor B
const int MotorB1 = 8;
const int MotorB2 = 9;
//Pins for ultrasonic sensor
const int trigger=10;
const int echo=11;
int leftscanval, centerscanval, rightscanval, ldiagonalscanval, rdiagonalscanval;
char choice;
//Pin for IR control
int receiver = 12; // pin 1 of IR receiver to Arduino digital pin 12
IRrecv irrecv(receiver); // create instance of 'irrecv'
decode_results results;
char contcommand;
int modecontrol=0;
int power=0;
const int distancelimit = 27; //Distance limit for obstacles in front
const int sidedistancelimit = 12; //Minimum distance in cm to obstacles at both sides (the robot will allow a shorter distance sideways)
int distance;
int numcycles = 0;
char turndirection; //Gets 'l', 'r' or 'f' depending on which direction is obstacle free
const int turntime = 900; //Time the robot spends turning (miliseconds)
int thereis;
Servo head;
void setup(){
head.attach(5);
head.write(80);
irrecv.enableIRIn(); // Start the IR receiver
pinMode(MotorA1, OUTPUT);
pinMode(MotorA2, OUTPUT);
pinMode(MotorB1, OUTPUT);
pinMode(MotorB2, OUTPUT);
pinMode(trigger,OUTPUT);
pinMode(echo,INPUT);
//Variable inicialization
digitalWrite(MotorA1,LOW);
digitalWrite(MotorA2,LOW);
digitalWrite(MotorB1,LOW);
digitalWrite(MotorB2,LOW);
digitalWrite(trigger,LOW);
}
void go(){
digitalWrite (MotorA1, HIGH);
digitalWrite (MotorA2, LOW);
digitalWrite (MotorB1, HIGH);
digitalWrite (MotorB2, LOW);
}
void backwards(){
digitalWrite (MotorA1 , LOW);
digitalWrite (MotorA2, HIGH);
digitalWrite (MotorB1, LOW);
digitalWrite (MotorB2, HIGH);
}
int watch(){
long howfar;
digitalWrite(trigger,LOW);
delayMicroseconds(5);
digitalWrite(trigger,HIGH);
delayMicroseconds(15);
digitalWrite(trigger,LOW);
howfar=pulseIn(echo,HIGH);
howfar=howfar*0.01657; //how far away is the object in cm
return round(howfar);
}
void turnleft(int t){
digitalWrite (MotorA1, LOW);
digitalWrite (MotorA2, HIGH);
digitalWrite (MotorB1, HIGH);
digitalWrite (MotorB2, LOW);
delay(t);
}
void turnright(int t){
digitalWrite (MotorA1, HIGH);
digitalWrite (MotorA2, LOW);
digitalWrite (MotorB1, LOW);
digitalWrite (MotorB2, HIGH);
delay(t);
}
void stopmove(){
digitalWrite (MotorA1 ,LOW);
digitalWrite (MotorA2, LOW);
digitalWrite (MotorB1, LOW);
digitalWrite (MotorB2, LOW);
}
void watchsurrounding(){ //Meassures distances to the right, left, front, left diagonal, right diagonal and asign them in cm to the variables rightscanval,
//leftscanval, centerscanval, ldiagonalscanval and rdiagonalscanval (there are 5 points for distance testing)
centerscanval = watch();
if(centerscanval
head.write(120);
delay(100);
ldiagonalscanval = watch();
if(ldiagonalscanval
head.write(160); //Didn't use 180 degrees because my servo is not able to take this angle
delay(300);
leftscanval = watch();
if(leftscanval
head.write(120);
delay(100);
ldiagonalscanval = watch();
if(ldiagonalscanval
head.write(80); //I used 80 degrees because its the central angle of my 160 degrees span (use 90 degrees if you are moving your servo through the whole 180 degrees)
delay(100);
centerscanval = watch();
if(centerscanval
head.write(40);
delay(100);
rdiagonalscanval = watch();
if(rdiagonalscanval
head.write(0);
delay(100);
rightscanval = watch();
if(rightscanval
head.write(80); //Finish looking around (look forward again)
delay(300);
}
char decide(){
watchsurrounding();
if (leftscanval>rightscanval && leftscanval>centerscanval){
choice = 'l';
}
else if (rightscanval>leftscanval && rightscanval>centerscanval){
choice = 'r';
}
else{
choice = 'f';
}
return choice;
}
void translateIR() { //Used when robot is switched to operate in remote control mode
switch(results.value)
{
case 0xFF629D: //Case 'FORWARD'
go();
break;
case 0xFF22DD: //Case 'LEFT'
turnleft(turntime);
stopmove();
break;
case 0xFF02FD: //Case 'OK'
stopmove();
break;
case 0xFFC23D: //Case 'RIGHT'
turnright(turntime);
stopmove();
break;
case 0xFFA857: //Case 'REVERSE'
backwards();
break;
case 0xFF42BD: //Case '*'
modecontrol=0; stopmove(); // If an '*' is received, switch to automatic robot operating mode
break;
default:
;
}// End Case
delay(500); // Do not get immediate repeat
}
void loop(){
if (irrecv.decode(&results)){ //Check if the remote control is sending a signal
if(results.value==0xFF6897){ //If an '1' is received, turn on robot
power=1; }
if(results.value==0xFF4AB5){ //If a '0' is received, turn off robot
stopmove();
power=0; }
if(results.value==0xFF42BD){ //If an '*' is received, switch operating mode from automatic robot to remote control (press also "*" to return to automatic robot mode)
modecontrol=1; // Activate remote control operating mode
stopmove(); //The robot stops and starts responding to the user's directions
}
irrecv.resume(); // receive the next value
}
while(modecontrol==1){ //The system gets into this loop during the remote control mode until modecontrol=0 (with '*')
if (irrecv.decode(&results)){ //If something is being received
translateIR();//Do something depending on the signal received
irrecv.resume(); // receive the next value
}
}
if(power==1){
go(); // if nothing is wrong go forward using go() function above.
++numcycles;
if(numcycles>130){ //Watch if something is around every 130 loops while moving forward
watchsurrounding();
if(leftscanval
turnright(turntime);
}
if(rightscanval
turnleft(turntime);
}
numcycles=0; //Restart count of cycles
}
distance = watch(); // use the watch() function to see if anything is ahead (when the robot is just moving forward and not looking around it will test the distance in front)
if (distance
++thereis;}
if (distance>distancelimit){
thereis=0;} //Count is restarted
if (thereis > 25){
stopmove(); // Since something is ahead, stop moving.
turndirection = decide(); //Decide which direction to turn.
switch (turndirection){
case 'l':
turnleft(turntime);
break;
case 'r':
turnright(turntime);
break;
case 'f':
; //Do not turn if there was actually nothing ahead
