Nexus Automation Nexus Robot, Руководство пользователя, Страница: 75 / 128

www.nexusrobot.com Robot Kits manual
71
¾
2WD platform with 3 SONAR
Look the above figure of simple Wiring Diagram for RB004 2WD V2.0. this code is matched for it
RB004_2WD_PID_3SONAR_3IR code
#include <MotorWheel.h>
#include <Omni3WD.h>
#include <Omni4WD.h>
#include <R2WD.h>
#include <PID_Beta6.h>
#include <PinChangeInt.h>
#include <PinChangeIntConfig.h>
#include <SONAR.h> // Include the header files
/******************************************/
// SONAR
SONAR sonar11(0x11),sonar12(0x12),sonar13(0x13); // Software initialization
//SONAR is be defined in <SONAR.h> as a class
unsigned short distBuf[3]; // Used to save the data of the 3 sonars return;
void sonarsUpdate() {
static unsigned char sonarCurr=1; // A variable save data used to flag the sonar’s state
if(sonarCurr==3) sonarCurr=1;
else ++sonarCurr;
if(sonarCurr==1) { // The conditions is ture?
distBuf[1]=sonar12.getDist(); // Get the value of distance from sonar12
sonar12.trigger(); // Trigger sonar12
} else if(sonarCurr==2) {
distBuf[2]=sonar13.getDist(); // Get the value of distance from sonar13
sonar13.trigger(); // Trigger sonar13
} else {
distBuf[0]=sonar11.getDist(); // Ger the value of distance from sonar11
sonar11.trigger(); // Trigger sonar11
}
}
/*********************************************/
// Infrared Sensor