NewPing sonar(trig_pin, ECHO_PIN, MAX_DISTANCE); // sets up sensor library to use the correct pins to measure distance.

Transcription

1 #include <Servo.h> #include <NewPing.h> #define TRIG_PIN A4 // Pin A4 on the Arduino Sensor Shield #define ECHO_PIN A5 // Pin A5 on the Arduino Sensor Shield #define MAX_DISTANCE 200 // sets maximum useable sensor measuring distance to 200cm #define MAX_SPEED 180 // sets speed of DC traction motors to 180/256 or about 70% of full speed - to keep power drain down. #define MAX_SPEED_OFFSET 20 // this sets offset to allow for differences between the DC traction motors #define COLL_DIST 10 // sets distance at which robot stops and reverses to 10cm #define TURN_DIST COLL_DIST+25 // sets distance at which robot veers away from object (not reverse) to 25cm (10+15) NewPing sonar(trig_pin, ECHO_PIN, MAX_DISTANCE); // sets up sensor library to use the correct pins to measure distance. //Assign digital pins used to drive the motors int ENA=5;//connected to Arduino's port 5(output pwm) int IN1=2;//connected to Arduino's port 2 int IN2=3;//connected to Arduino's port 3 int ENB=6;//connected to Arduino's port 6(output pwm) int IN3=4;//connected to Arduino's port 4 int IN4=7;//connected to Arduino's port 7 Servo myservo; // create servo object to control a servo int pos = 0; // this sets up variables for use in the sketch (code) int curdist = 0; //Current distance int maxdist = 0; //Maximum distance from a sweep of pings int maxpos = 0; //Angle at which maximum distance was detected. String motorset = ""; int speedset = 0; // SETUP LOOP void setup() //Serial.begin(115200); // Open serial monitor at baud to see ping results pinmode(ena,output);//output pinmode(enb,output); pinmode(in1,output); pinmode(in2,output); pinmode(in3,output); pinmode(in4,output); digitalwrite(ena,low); digitalwrite(enb,low);//stop driving digitalwrite(in2,low);//setting motora's direction digitalwrite(in4,high);//setting motorb's direction myservo.attach(9); // attaches the servo on pin 9 myservo.write(90); // tells the servo to position at 90-degrees ie. facing forward. delay(2000); // delay for two seconds lookaround(); //identify furthest object and direction setcourse(); //off we go. //

2 // MAIN LOOP void loop() checkforward(); // check robot is moving forward checkpath(); // set ultrasonic sensor to scan for any possible obstacles // void checkpath() myservo.write(144); // set servo to face left 54-degrees from forward delay(100); // wait 100ms for servo to reach position for(pos = 144; pos >= 36; pos-=18) // loop to sweep the servo (& sensor) from 144deg left to 18deg right at 18deg intervals. myservo.write(pos); // tell servo to go to position in variable 'pos' delay(60); // wait 60ms for servo to get to position curdist = readping(); // get the current distance to any object in front of sensor //Serial.println (curdist); if (curdist < COLL_DIST) // if the current distance to object is less than the collision distance checkcourse(); // run the checkcourse function break; // jump out of this loop if (curdist < TURN_DIST) // if current distance is less than the turn distance changepath(); // run the changepath function // void lookaround() //have a look around and identify at what angle is the furthest ping. Only run when staionary. maxdist = 0; maxpos= 144; myservo.write(144); // set servo to face left 54-degrees from forward delay(100); // wait 100ms for servo to reach position for(pos = 144; pos >= 36; pos-=18) // loop to sweep the servo (& sensor) from 144deg left to 18deg right at 18deg intervals. myservo.write(pos); // tell servo to go to position in variable 'pos' delay(60); // wait 60ms for servo to get to position curdist = readping(); // get the current distance to any object in front of sensor //Serial.println (curdist); if (curdist > maxdist) maxdist = curdist; maxpos=pos; void setcourse() // have a look around turn towards the furthest object and then move forward lookaround (); if (maxpos <= 54) turnright(); // turn right as furthest object was to the right if (maxpos >54 && maxpos < 90) veerright(); // veer right as furthest object is forward and slightly right if (maxpos >= 126) turnleft(); // turn left as furthest object was to the left if (maxpos >= 90 && maxpos < 126) veerleft(); //veer left as furthest object is ahead or slightly to the Left void checkcourse() // we're about to hit something so stop, move back a bit, stop and work out best course movestop(); delay(100); movebackward(); delay(600); //move backwards for 600ms movestop(); setcourse();

3 void changepath() if (pos < 90) veerleft(); // if current pos of sensor is less than 90deg, then object is on the right hand side so veer left if (pos >= 90) veerright(); // if current pos of sensor is >= than 90deg, then object is on the left hand side or ahead so veer right int readping() // read the ultrasonic sensor distance delay(70); unsigned int us = sonar.ping(); int cm = us/us_roundtrip_cm; return cm; // Motor direction functions. MotorA is on the right and MotorB on the left. void checkforward() // make sure both motors going forward. if (motorset=="forward") digitalwrite(in2,low);//setting motora's direction FWD digitalwrite(in4,high);//setting motorb's direction FWD void checkbackward() // make sure both motors going backward if (motorset=="backward") digitalwrite(in1,low); digitalwrite(in2,high);//setting motora's direction BACK digitalwrite(in3,high); digitalwrite(in4,low);//setting motorb's direction BACK void checkright() // Motor A BACK and Motor B FWD if (motorset=="right") digitalwrite(in1,low); digitalwrite(in2,high);//setting motora's direction BACK digitalwrite(in4,high);//setting motorb's direction FWD void checkleft() // Motor A FWD and Motor B Back if (motorset=="left") digitalwrite(in2,low);//setting motora's direction FWD digitalwrite(in3,high); digitalwrite(in4,low);//setting motorb's direction BACK

4 // // The above functions simply set the L298 IN1,2,3,&4 pins for the required Motor direction void movestop() // stop the motors digitalwrite(ena,low); digitalwrite(enb,low);//stop driving - void moveforward() motorset = "FORWARD"; checkforward(); for (speedset = 140; speedset < MAX_SPEED; speedset +=2) // slowly bring the speed up analogwrite(ena,speedset);//start driving motora analogwrite(enb,speedset+max_speed_offset);//start driving motorb delay(5); void movebackward() motorset = "BACKWARD"; checkbackward(); for (speedset = 140; speedset < MAX_SPEED; speedset +=2) // slowly bring the speed up analogwrite(ena,speedset);//start driving motora analogwrite(enb,speedset+max_speed_offset);//start driving motorb delay(5); void turnright() motorset = "RIGHT"; checkright(); delay(400); // run motors this way for 400ms moveforward (); - void turnleft() motorset = "LEFT"; checkleft(); delay(400); // run motors this way for 400ms moveforward(); - void veerright() // veering right motorset = "RIGHT"; checkright(); delay(300); // run motors this way for 300ms

5 moveforward (); void veerleft() // veering left motorset = "LEFT"; checkleft(); delay(300); // run motors this way for 300ms moveforward();