Nexus Automation Nexus Robot, User Manual

Page: 94 / 128

www.nexusrobot.com Robot Kits manual
90
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() { //the function to
static unsigned char sonarCurr=1; // A variable save a data used to flag the current of sonar
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(); // Ger 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
}
}
/*********************************************/
/*******************************************/
// Motors
irqISR(irq1,isr1);
MotorWheel wheel1(9,8,6,7,&irq1); // Pin9:PWM, Pin8:DIR, Pin6:PhaseA, Pin7:PhaseB
irqISR(irq2,isr2);
MotorWheel wheel2(10,11,14,15,&irq2); // Pin10:PWM, Pin11:DIR, Pin14:PhaseA, Pin15:PhaseB
irqISR(irq3,isr3);
MotorWheel wheel3(3,2,4,5,&irq3); // Pin3:PWM, Pin2:DIR, Pin4:PhaseA, Pin5:PhaseB
Omni3WD Omni(&wheel1,&wheel2,&wheel3);
// This will create a Omni3WD object called Omni. then You
// can use any of its methods; for instance, to
// control a Omni3WD attached to pins, you could write
/******************************************/