Sumobotsclass
Jump to navigation
Jump to search
Motor control code
#include <PololuQTRSensors.h> //we're using the pololuQTR sensor library so we must attach it.
#define motor1dir 8 //direction motor 2
#define motor1speed 9 //pwm control motor 1
#define motor2dir 11 //direction motor 2
#define motor2speed 10 //pwm control motor 2
#define ledpin 13 //led pin
PololuQTRSensorsRC qtr((unsigned char[]) {19,18}, 2, 2000, 255); //declares two line sensors on pins 18 and 19 this corresponds to analog pins 4 and 5
unsigned int sensors[2];
const int linethreshold = 300;
void setup() { // put your setup code here, to run once:
//motor control outputs
pinMode(motor1dir, OUTPUT);
pinMode(motor1speed, OUTPUT);
pinMode(motor2dir, OUTPUT);
pinMode(motor2speed, OUTPUT);
// AV outputs
pinMode(ledPin, OUTPUT);
pinMode(buzzerPin, OUTPUT);
pinMode(irSensorPin, INPUT);
//Serial.begin(9600); // set up Serial library at 9600 bps for debugging
delay (3000); //wait for everything and for the match to start
blink(ledPin, 3, 100);
// blink the LED 3 times. This should happen only once.
// if you see the LED blink three times, it means that
// the module reset itself,. probably because the motor
// caused a brownout or a short.
} //end setup
void loop() { // put your main code here, to run repeatedly:
qtr.read(sensors); //reads the line sensors //set speed max analogWrite(motor2speed,255); analogWrite(motor1speed,255); //go forward digitalWrite(motor1dir, HIGH); digitalWrite(motor2dir, HIGH); delay(3000); //go backwards digitalWrite(motor1dir, LOW); digitalWrite(motor2dir, LOW); delay(3000); //go right digitalWrite(motor1dir, HIGH); digitalWrite(motor2dir, LOW); delay(3000); //go left digitalWrite(motor1dir, LOW); digitalWrite(motor2dir, HIGH); delay(3000); //stop analogWrite(motor1speed,0); analogWrite(motor2speed,0); delay(3000);
//DEBUG Don't forget to enable the serial port in setup
Serial.print("d= ");
Serial.print(analogRead(irSensorPin));
Serial.print(" r= ");
Serial.print(sensors[0]); //right
Serial.print(" l= ");
Serial.println(sensors[1]); //left
//do the robot dance
if (sensors[0] < linethreshold && sensors[1] < linethreshold)
{
// Serial.println("the edge backup and spin");
//backup
delay(500); //do this for half of a second
//rotate left
delay(500); //do this for half of a second
} //end found edge front
else if (sensors[1] < linethreshold)
{
// Serial.println("right sensor, turn left");
//rotate left
delay(1000); //do this for half of a second
} //end found edge right
else if (sensors[0] < linethreshold)
{
// Serial.println("left sensor, turn right");
//rotate right
delay(500); //do this for half of a second
} //end found edge Left
//WITHIN BORDERS
else if (irdistance(irSensorPin) < 450/*max distance */ )
{ //charge
// Serial.println("CHARGE");
//go forward
delay(100); //a longer delay for the charge
}//end charge
else
{ //search
Serial.println("scanning, where are you?");
//rotate right
delay(100);
}//end search
}
These are functions that make life easy, someone else did the math now we don't have to.
This function blinks an LED
void blink(int whatPin, int howManyTimes, int milliSecs) {
int i = 0;
for ( i = 0; i < howManyTimes; i++) {
digitalWrite(whatPin, HIGH);
delay(milliSecs/2);
digitalWrite(whatPin, LOW);
delay(milliSecs/2);
}
}// end blink
This is the function that allows us to use the sharp ir sensor for detecting distance
float irdistance(int pin){
//http://luckylarry.co.uk/arduino-projects/arduino-using-a-sharp-ir-sensor-for-distance-calculation/
float volts, distance;
for (int i=0; i< 5; i++){
volts = analogRead(pin)*0.0048828125;// (5/1024)
distance += 65* pow(volts, -1.10); //65 = theretical distance / (1/Volts)S
}
return (distance);
}//end irdistance
This function makes a buzzer make noise or make an ir led pulse at a certain frequency
void freqout(int freq, int t, int freqoutpin){ // freq in hz, t in ms, pin to use
int hperiod; //calculate 1/2 period in us
long cycles, i;
hperiod = (500000 / freq) - 7; // subtract 7 us to make up for digitalWrite overhead
cycles = ((long)freq * (long)t) / 1000; // calculate cycles
for (i=0; i<= cycles; i++){ // play note for t ms
digitalWrite(freqoutpin, HIGH);
delayMicroseconds(hperiod);
digitalWrite(freqoutpin, LOW);
delayMicroseconds(hperiod);
}//end loop
}// end freqout
Need a Mechapad or some Protoboard and Patience
