Manual

www.nexusrobot.com Robot Kits manual
96
if(IRs[1] || IRs[2]) { // The Anti-drop return a High Value
Omni.setCarBackoff(speedMMPS); // get car back
Omni.delayMS(ms); // delay “ms” ,every 10 ms call the PIDregulate() once time.
if(IRs[1]) {
Omni.setCarRotateLeft(speedMMPS);
} else Omni.setCarRotateRight(speedMMPS);
Omni.delayMS(ms);
} else if(distBuf[1]<distance || distBuf[2]<distance) { // the right side have something
Omni.setCarLeft(speedMMPS);
} else if(distBuf[4]<distance || distBuf[5]<distance) { // Left side have something
Omni.setCarRight(speedMMPS);
} else if(distBuf[0]<distance || distBuf[3]<distance || IRs[0]) {
Omni.setCarRotateRight(speedMMPS);
} else {
Omni.setCarAdvance(speedMMPS);
}
Omni.PIDRegulate();
if(millis()%100==0) Omni.debugger();
}
/*****************************************/
// setup()
void setup() {
TCCR1B=TCCR1B&0xf8|0x01; // Pin9,Pin10 PWM 31250Hz
TCCR2B=TCCR2B&0xf8|0x01; // Pin3,Pin11 PWM 31250Hz
SONAR::init(13); // Pin13 as RW Control
initIRs();
Omni.PIDEnable(0.26,0.02,0,10); //Enable PID
}
/****************************************/
// loop()
void loop() {
demoWithSensors(80,20,300); //Call the demo function
}