Nexus Automation Nexus Robot User Manual page 95

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