Pathfinder

Code

• Programma

ProgrammaArduino

#include  // Add Adafruit Motor Shield for Arduino kit library.#include  // Add Servo Motor library.#define BuzzPIN A0 // Wijs PIN A0 toe als BuzzPIN (Verbind Arduino UNO "A0" PIN met Buzzer "+" PIN).#define TrigPIN A1 // Wijs PIN A1 toe als TrigPIN (Verbind Arduino UNO "A1" PIN met Ultrasone Sonar Sensor "Trig" PIN).#define EchoPIN A2 // Wijs PIN A2 toe als EchoPIN (Verbind Arduino UNO "A2" PIN met Ultrasone Sonar Sensor "Trig" PIN).#define LEDBPIN A3 // Wijs PIN A3 toe als LEDBPIN (Verbind Arduino UNO "A3" PIN met RGB Diffused Common Cathode "LEDB" PIN).#define LEDGPIN A4 // Wijs PIN A4 toe als LEDGPIN (Verbind Arduino UNO "A4" PIN met RGB Diffused Common Cathode "LEDG" PIN).#define LEDRPIN A5 // Wijs PIN toe A5 als LEDRPIN (verbind Arduino UNO "A5" PIN met RGB Diffused Common Cathode "LEDR" PIN). #define DCMROFF 25 // Dit stelt Offset in om verschillen tussen de twee DC-tractiemotoren mogelijk te maken.AF_DCMo tor M1 (1, MOTOR12_64KHZ); // Creëer DCMotor #1 met behulp van M1-uitgang, ingesteld op 64 kHz PWM-frequentie. AF_DCMotor M2 (2, MOTOR12_64KHZ); // Creëer DCMotor #2 met behulp van M2-uitgang, ingesteld op 64 kHz PWM-frequentie. Servo SER1; // Maak een Servo-object om Servo.int Search (void) te besturen { // Integer type variabele declaratie. float Duur =0,0; // Variabele declaratie van het type Float. vlotter CM =0,0; // Variabele declaratie van het type Float. digitalWrite (TrigPIN, LAAG); // TrigPIN-uitgang als 0V (Logisch laag niveau). vertragingMicroseconden (2); // Vertraging voor 2us, stuur 10 ons hoge puls naar ultrasone sonarsensor "TrigPIN". digitalWrite (TrigPIN, HOOG); // TrigPIN-uitgang als 5V (Logisch hoog niveau). vertraging Microseconden (10); // Vertraging voor 10us. digitalWrite (TrigPIN, LAAG); // TrigPIN-uitgang als 0V (Logisch laag niveau). Duur =pulseIn (EchoPIN, HOOG); // Begin met het tellen van de tijd, tot weer EchoPIN terug naar logisch "High Level" en zet de "Time" in de variabele genaamd "Duur". CM =(Duur/58,8); // Zet afstand om in CM. retour CM; // Keer terug naar CM.}int RightDistance, LeftDistance; // Afstanden aan weerszijden.float Distance =0.00; // Variabele declaratie van het type float.void setup () { // Setup-lus. pinMode (BuzzPIN, UITGANG); // Verklaar BuzzPIN als "Uitvoerpincode". pinMode (TrigPIN, UITGANG); // Declareer TrigPIN als "Uitvoerpincode". pinMode (EchoPIN, INPUT); // Declareer EchoPIN als "Uitvoerpincode". pinMode (LEDBPIN, UITGANG); // Verklaar LEDBPIN als "Uitvoerpincode". pinMode (LEDGPIN, UITGANG); // Verklaar LEDGPIN als "Uitvoerpincode". pinMode (LEDRPIN, UITGANG); // Declareer LEDRPIN als "Output PIN". SER1.bijvoegen (10); // Bevestigt de servo op pin 10 (SER1 op de Adafruit Motor Shield voor Arduino-kit aan het Servo-object).} ongeldige lus () {// Hoofdlus. SER1.schrijven (80); // Vertelt de servo om op 80 graden te positioneren (naar voren gericht). vertraging (100); // Vertraging voor 0.1s. Afstand =Zoeken (); // De afstand meten in CM. if (Afstand <30) { // Indien obstakel gevonden in 30cm. digitalWrite (BuzzPIN, HOOG); // BuzzPIN-uitgang als 5V (Logisch hoog niveau). digitalWrite (LEDBPIN, LAAG); // LEDBPIN-uitgang als 0V (Logisch laag niveau). digitalWrite (LEDGPIN, LAAG); // LEDGPIN-uitgang als 0V (Logisch laag niveau). digitalWrite (LEDRPIN, HOOG); // LEDRPIN-uitgang als 5V (Logisch hoog niveau). M1.setSpeed ​​(100); // Vertragen. M2.setSpeed ​​(100); // Vertragen. ChangePath (); // Als vooruit is geblokkeerd Verander van richting. } else if ((Afstand>=30) &&(Afstand <60)) { // Als een obstakel wordt gevonden tussen 30 cm en 60 cm. digitalWrite (BuzzPIN, LAAG); // BuzzPIN-uitgang als 0V (Logisch laag niveau). digitalWrite (LEDBPIN, HOOG); // LEDBPIN-uitgang als 5V (Logisch hoog niveau). digitalWrite (LEDGPIN, LAAG); // LEDGPIN-uitgang als 0V (Logisch laag niveau). digitalWrite (LEDRPIN, LAAG); // LEDRPIN-uitgang als 0V (Logisch laag niveau). M1.setSpeed ​​(150); // Snelheid iets verhoogd. M2.setSpeed ​​(150); // Snelheid iets verhoogd. Vooruit (); // Robot beweegt naar voorwaartse richting. } else if ((Afstand>=60) &&(Afstand <90)) { // Als een obstakel wordt gevonden tussen 60 cm en 90 cm. digitalWrite (BuzzPIN, LAAG); // BuzzPIN-uitgang als 0V (Logisch laag niveau). digitalWrite (LEDBPIN, LAAG); // LEDBPIN-uitgang als 0V (Logisch laag niveau). digitalWrite (LEDGPIN, HOOG); // LEDGPIN-uitgang als 5V (Logisch hoog niveau). digitalWrite (LEDRPIN, LAAG); // LEDRPIN-uitgang als 0V (Logisch laag niveau). M1.setSpeed ​​(200); // Versnellen. M2.setSpeed ​​(200); // Versnellen. Vooruit (); // Robot beweegt naar voorwaartse richting. } else { // Als het obstakel niet kan worden gevonden in 90 cm. digitalWrite (BuzzPIN, LAAG); // BuzzPIN-uitgang als 0V (Logisch laag niveau). digitalWrite (LEDBPIN, HOOG); // LEDBPIN-uitgang als 5V (Logisch hoog niveau). digitalWrite (LEDGPIN, HOOG); // LEDGPIN-uitgang als 5V (Logisch hoog niveau). digitalWrite (LEDRPIN, HOOG); // LEDRPIN-uitgang als 5V (Logisch hoog niveau). M1.setSnelheid (250); // Snelheidsverhoging volledig. M2.setSnelheid (250); // Snelheidsverhoging volledig. Vooruit (); // Robot beweegt naar voorwaartse richting. }}void ChangePath () { // Path Change-lus. Stoppen (); // Robotstop. Achteruit (); // Robot loopt achteruit. Stoppen (); // Robotstop. SER1.schrijven (12); // Controleer de afstand naar rechts. vertraging (500); // Vertraging voor 0,5 s. RightDistance =Zoeken (); // Stel de juiste afstand in. vertraging (500); // Vertraging voor 0,5 s. SER1.schrijven (160); // Controleer de afstand naar links. vertraging (1000); // Vertraging voor 1s. LeftDistance =Zoeken (); // Stel de linkerafstand in. vertraging (500); // Vertraging voor 0,5 s. SER1.schrijven (80); // Keer terug naar het midden. vertraging (500); // Vertraging voor 0,5 s. VergelijkAfstand (); // Vind de langste afstand.}void CompareDistance () { // Distance Compare loop. if (RightDistance> LeftDistance) { // Als rechts minder wordt belemmerd. Rechtsaf (); // Robot Draai in de juiste richting. } else if (LeftDistance> RightDistance) { // If Left is minder gehinderd. Sla linksaf (); // Robot Draai naar links. } else { // Als beide even belemmerd zijn. Keer om (); // Robot omdraaien. }}void Forward () { // Forward loop. M1.run (VOORUIT); // Draai DCMotor #1 naar Vooruit. M2.run (VOORUIT); // Draai DCMotor #1 naar Forward.}void Backward () { // Backward loop. M1.run (ACHTERUIT); // Draai DCMotor #1 naar Achteruit. M2.run (ACHTERUIT); // Draai DCMotor #2 naar Achteruit. vertraging (500); // Vertraging voor 1s.}void TurnRight () { // Right Turn-lus. M1.run (ACHTERUIT); // Draai DCMotor #1 naar Achteruit. M2.run (VOORUIT); // Draai DCMotor #2 naar Vooruit. M1.setSpeed ​​(100+DCMROFF); // Kalibreer de snelheid van DCMotor #1. vertraging (300); // Vertraging voor 0,7s.}void TurnLeft () { // Left Turn-lus. M1.run (VOORUIT); // Draai DCMotor #1 naar Vooruit. M2.run (ACHTERUIT); // Draai DCMotor #2 naar Achteruit. M2.setSpeed ​​(100+DCMROFF); // Kalibreer de snelheid van DCMotor #2. vertraging (300); // Vertraging voor 0,7s.}void TurnAround () { // Trun Around-lus. M1.run (VOORUIT); // Draai DCMotor #1 naar Vooruit. M2.run (ACHTERUIT); // Draai DCMotor #2 naar Achteruit. M2.setSpeed ​​(100+DCMROFF); // Kalibreer de snelheid van DCMotor #2. vertraging (700); // Vertraging voor 2.1s.}void Stop () {// Stop loop. M1.run (VRIJGAVE); // Laat DCMotor #1 los. M2.run (RELEASE); // Laat gelijkstroommotor #2 los. vertraging (100); // Vertraging voor 0.1s.}