Manual

www.nexusrobot.com Robot Kits manual
122
¾ Servo_3WD platform with 3 SONAR
Look the above figure of simple Wiring Diagram for 3WD omni wheel mobile robot. Tis code is matched
for it
Servo_3WD omni wheel mobile robot code
#include <SONAR.h> // Include the header files
SONAR sonar11(0x11),sonar12(0x12),sonar13(0x13); // Software initialization
//SONAR is be defined in <SONAR.h> as a class
#define MOTOR1_E 9 //define the pin 9 as the motor1’s pwm signal control pin
#define MOTOR2_E 10 //define the pin 10 as the motor2’s pwm signal control pin
#define MOTOR3_E 11 //define the pin 11 as the motor3’s pwm signal control pin
//**********************************************//
unsigned short distBuf[3];
unsigned char sonarUpdate(){
static unsigned int sonarCurr=1;
if(sonarCurr==3) sonarCurr=1;
else ++sonarCurr;
Serial.println(sonarCurr);
if(sonarCurr==1) {
distBuf[0]=sonar11.getDist(); //Save the data of the distance get from Sonar11.
sonar12.trigger(); // Tregger sonar12
}else if(sonarCurr==2){
distBuf[1]=sonar12.getDist();
sonar13.trigger(); // trigger the sonar13
}else if(sonarCurr==3){
distBuf[2]=sonar13.getDist(); //according to the address to read the serial.
sonar11.trigger(); // trigger the sonar11
}
return sonarCurr;
}
//*************************************************//
void goAhead(){
analogWrite(MOTOR1_E, 48); //stop motor1
analogWrite(MOTOR2_E, 62); //forward motor2
analogWrite(MOTOR3_E, 34); // Revese motor3
}
//*************************************************//
void getBack(){