Documentos de Académico
Documentos de Profesional
Documentos de Cultura
// Here are some parameters that you need to adjust for your setup
// Base speeds are the motor pwm values. Adjust them for desired motor speeds.
#define baseLeftSpeed 17
#define baseRightSpeed 17
// variables
uint8_t lspd, rspd; // motor speeds
uint16_t lsenseval, rsenseval, lwhiteval, rwhiteval; // sensor values
void setup(){
// setup pins
pinMode(lmotorpin, OUTPUT);
pinMode(rmotorpin, OUTPUT);
pinMode(2, INPUT); // these are the sensor pins, but the analog
pinMode(3, INPUT); // and digital functions use different numbers
ledoff;
pinMode(ledpin, OUTPUT);
analogWrite(lmotorpin, 0);
analogWrite(rmotorpin, 0);
lspd = baseLeftSpeed;
rspd = baseRightSpeed;
// give a few second pause to set the thing on a white surface
// the LED will flash during this time
lsenseval = 6;
while(lsenseval){
lsenseval--;
flashLED(1);
delay(989);
}
flashLED(4);
delay(500);
senseInit();
}
void loop(){
// followEdge() contains an infinite loop, so this loop really isn't necessary
followEdge();
}
void followEdge(){
// now look for an edge
uint8_t lastMove = 1; //0=straight, 1=left, 2=right
unsigned long moveEndTime = 0; // the millis at which to stop
unsigned long randomBits = micros(); // for a random walk
while(true){
// only read sensors about once every 20ms
// it can be done faster, but this makes motion smoother
delay(18);
// read the value 4 times and average
ledon;
delay(1);
lsenseval = 0;
rsenseval = 0;
for(i=0; i<4; i++){
lsenseval += analogRead(lsensepin);
rsenseval += analogRead(rsensepin);
}
// don't divide by 4 because it is used below
ledoff;
}else{
// both black - if prior turn toward white, else random walk
if(lastSense == 1 || millis() < prior){
// turn toward last white or left
if(priorDir == 0){
moveEndTime = millis()+smallturn;
move(lspd, 0); // turn right
lastMove = 2;
}else if(priorDir == 1){
moveEndTime = millis()+smallturn;
move(0, rspd); // turn left
lastMove = 1;
}else{
moveEndTime = millis()+bigturn;
move(lspd, 0); // turn right a lot
lastMove = 2;
}
}else{
// random walk
if(millis() < moveEndTime){
// just continue moving
}else{
if(lastMove){
moveEndTime = millis()+steplength;
move(lspd, rspd); // go straight
lastMove = 0;
}else{
if(randomBits & 1){
moveEndTime = millis()+smallturn;
move(0, rspd); // turn left
lastMove = 1;
}else{
moveEndTime = millis()+smallturn;
move(lspd, 0); // turn right
lastMove = 2;
}
randomBits >>= 1;
}
}
}
lastSense = 2;
}
}
}
void stop(){
analogWrite(lmotorpin, 0);
analogWrite(rmotorpin, 0);
}