|
|
| Line 1: |
Line 1: |
| − | == Motor control code ==
| + | [Sumobotclasscode code snippets] |
| − | These are the imports, variable decelerations and pin assignments.
| |
| − | <nowiki>#include <PololuQTRSensors.h> //we're using the pololuQTR sensor library so we must attach it.</nowiki>
| |
| − | <nowiki>#define motor1dir 8 //direction motor 2</nowiki>
| |
| − | <nowiki>#define motor1speed 9 //pwm control motor 1</nowiki>
| |
| − | <nowiki>#define motor2dir 11 //direction motor 2</nowiki>
| |
| − | <nowiki>#define motor2speed 10 //pwm control motor 2</nowiki>
| |
| − | <nowiki>#define ledpin 13 //led pin </nowiki>
| |
| − | <nowiki>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</nowiki>
| |
| − | <nowiki>unsigned int sensors[2];</nowiki>
| |
| − | <nowiki>const int linethreshold = 300; </nowiki>
| |
| − | | |
| − | 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:
| |
| − | //motortests
| |
| − | //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);
| |
| − | | |
| − | //sensor tests
| |
| − | qtr.read(sensors); //reads the line sensors
| |
| − | //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 roboboogie
| |
| − | <nowiki>if (sensors[0] < linethreshold && sensors[1] < linethreshold) </nowiki>
| |
| − | {
| |
| − | // 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
| |
| − | <nowiki>else if (sensors[1] < linethreshold) </nowiki>
| |
| − | {
| |
| − | // Serial.println("right sensor, turn left");
| |
| − | //rotate left
| |
| − | delay(1000); //do this for half of a second
| |
| − | } //end found edge right
| |
| − | <nowiki>else if (sensors[0] < linethreshold) </nowiki>
| |
| − | {
| |
| − | // Serial.println("left sensor, turn right");
| |
| − | //rotate right
| |
| − | delay(500); //do this for half of a second
| |
| − | } //end found edge Left
| |
| − | //WITHIN BORDERS
| |
| − | <nowiki>else if (irdistance(irSensorPin) < 450/*max distance */ )</nowiki>
| |
| − | { //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
| |
| − | }//end the main loop
| |
| − | | |
| − | 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;
| |
| − | <nowiki>for ( i = 0; i < howManyTimes; i++) {</nowiki>
| |
| − | 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;
| |
| − | <nowiki>for (int i=0; i< 5; i++){ </nowiki>
| |
| − | 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
| |
| − | <nowiki>for (i=0; i<= cycles; i++){ </nowiki>// play note for t ms
| |
| − | digitalWrite(freqoutpin, HIGH);
| |
| − | delayMicroseconds(hperiod);
| |
| − | digitalWrite(freqoutpin, LOW);
| |
| − | delayMicroseconds(hperiod);
| |
| − | }//end loop
| |
| − | }// end freqout
| |
| | | | |
| | ---- | | ---- |
