Manual

www.nexusrobot.com Robot Kits manual
91
/******************************************/
// demo
unsigned long currMillis=0;
void demoWithSensors(unsigned int speedMMPS,unsigned int distance) {
if(millis()-currMillis>SONAR::duration) { // every 60ms call sonarUpdate once
currMillis=millis();
sonarsUpdate();
}
if(distBuf[1]<distance) { // If the left side have something
if(Omni.getCarStat()!=Omni3WD::STAT_RIGHT) Omni.setCarSlow2Stop(500);
Omni.setCarRight(speedMMPS); // Set car turn right
} else if(distBuf[2]<distance) { // If the right have something
if(Omni.getCarStat()!=Omni3WD::STAT_LEFT) Omni.setCarSlow2Stop(500);
Omni.setCarLeft(speedMMPS); // Set car turn left
} else if(distBuf[0]<distance) { // If the front have something
if(Omni.getCarStat()!=Omni3WD::STAT_ROTATERIGHT) Omni.setCarSlow2Stop(500);
Omni.setCarRotateRight(speedMMPS); // Set car rotateright
} else { // There is nothing around the car
if(Omni.getCarStat()!=Omni3WD::STAT_ADVANCE) Omni.setCarSlow2Stop(500);
Omni.setCarAdvance(speedMMPS); // Set car moves advance
}
Omni.PIDRegulate(); //PID regulate
}
/*****************************************/
// setup()
void setup() {
TCCR1B=TCCR1B&0xf8|0x01; // Pin9,Pin10 PWM 31250Hz
TCCR2B=TCCR2B&0xf8|0x01; // Pin3,Pin11 PWM 31250Hz
SONAR::init(13); // Initial sonars
Omni.PIDEnable(0.26,0.02,0,10); // Enable PID
}
/****************************************/
// loop()
void loop() {
demoWithSensors(80,30); // call the demo actions
}