Manual

www.nexusrobot.com Robot Kits manual
116
irqISR(irq4,isr4);
MotorWheel wheel4(10,7,18,19,&irq4);
Omni4WD Omni(&wheel1,&wheel2,&wheel3,&wheel4);
// This will create a Omni4WD object called Omni. then You can use any of its methods;
// for instance, to control a Omni4WD attached to pins, you could write
SONAR sonar11(0x11),sonar12(0x12),sonar13(0x13),sonar14(0x14); // Software initialization
//SONAR is be defined in <SONAR.h> as a class
unsigned short distBuf[4]; // Used to save the data of the 3 sonars return
unsigned char sonarsUpdate() {
static unsigned char sonarCurr = 1; // A variable save a data used to flag the state of sonar
if(sonarCurr==4) 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();
sonar13.trigger();
} else if(sonarCurr==3){
distBuf[3]=sonar14.getDist();
sonar14.trigger();
} else {
distBuf[0]=sonar11.getDist();
sonar11.trigger();
}
return sonarCurr; // Return the value
}
void goAhead(unsigned int speedMMPS){ // Car moves advance
if(Omni.getCarStat()!=Omni4WD::STAT_ADVANCE) Omni.setCarSlow2Stop(300);
Omni.setCarAdvance(0); // If the car’s state is not advance.stop it
// else moves advance continue
Omni.setCarSpeedMMPS(speedMMPS, 300); // Set the car speed at 300
}
void turnLeft(unsigned int speedMMPS){