Stap 3: STROOMDIAGRAM en CODE
We beginnen met de ruitvorm, allereerst vertegenwoordigt de ruitvormige shape een selectievakje. Bij deze controle zien we als er een object dichterbij dan 30 cm tot onze robot. De controle heeft twee mogelijke uitkomsten, ja of Nee. Ja, wat betekent dat dat er inderdaad een aantal object dichter dan 30 cm. Nee, wat betekent dat dat er geen objecten die aangetroffen binnen 30 cm.
Als er niets binnen 30 cm kan de robot eenvoudig verplaatsen naar voren als het pad duidelijk is. Als er iets dichter dan 30 cm moet de robot obstakel vermijden manoeuvres uitvoeren.
De eerste fase van de belemmering te vermijden is om te stoppen met de robot! Als u niet de robot stopt onmiddellijk het zal verpletteren!
Oef! Dat was dramatisch.
Nadat de robot is gestopt met het moet om te zien hoe het zou moeten gaan. Het doet dit door beide richtingen, veel op zoek zoals u hoort wanneer u de weg oversteekt. Eerst de robot draait links neemt een lezen, draait rechts, neemt een lezen.
Een ander selectievakje treedt op als u wilt zien welke richting is de beste manier om te gaan. Als links is de weg te gaan heeft weer naar links en ga vooruit. Als rechts is de weg naar de robot simpelweg wordt verplaatst naar voren als het kampt al in de goede richting. Dus dat was vrij gemakkelijk was het niet? Alles wat wij moeten doen is nu dat diagram converteren naar Arduino code en thats it, obstakel vermijden code gedaan!
#include
#include
////////////////////////////////////////////////servo motor and sonar sensor ////////////////////////////////////////////
Const int dangerdistance = 30; veiligheidsafstand voor obstakels (in cm) Servo panMotor; een servo-object #define TRIGGER_PIN 3 maken / / Arduino pin gebonden aan pin op de ultrasone sensor te activeren. #define ECHO_PIN 2 / / Arduino pin gebonden aan echo pin op de ultrasone sensor. #define MAX_distance 200 / / maximale afstand die wij wilt pingen voor (in centimeters). Maximale sensor afstand wordt geschat op 400-500 cm. NewPing sonar (TRIGGER_PIN, ECHO_PIN, MAX_distance); NewPing installatie van pennen en maximale afstand. ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////// MOTOR //////////////////////////////////////////////////////////////////
*
Deze code voert enkele eenvoudige manoeuvres om te illustreren de functies: - motor (motorNumber, motorDirection, motorSpeed) - motorBrake(motorNumber) - motorStop(motorNumber) - motorsStandby
Verbindingen: - Pin 5---> PWMA - Pin 7---> AIN2 - Pin 8---> AIN1 - Pin 11---> STBY - Pin 12---> BIN1 - Pin 13---> BIN2 - Pin 6---> PWMB
-Motor 1: A01 en A02 - Motor 2: B01 en B02
*
/ Definiëren de pinnen
Motor 1 int pinAIN1 = 8; Richting int pinAIN2 = 7; Richting int pinPWMA = 5; Snelheid
Motor 2 int pinBIN1 = 12; Richting int pinBIN2 = 13; Richting int pinPWMB = 6; Snelheid
Standby-int pinSTBY = 11;
Constanten te helpen herinneren de parameters statische Boole turnCW = 0; voor motor functie statische Boole turnCCW = 1; voor motor functie statische Boole motor1 = 0; Motor, motorStop, motorrem functioneert statische Booleaanse motor2 = 1; voor motor, motorStop, motorrem functies
////////////////////////////////////////////////////////////////////////////////////////////////////////////////// unsigned long time; timer voor boucle doe... terwijl de unsigned long timeplus; / / timer voor boucle doen... terwijl
int afstand; int ons; void setup {//Set de PIN modi motor pinMode (pinPWMA, OUTPUT); pinMode (pinAIN1, OUTPUT); pinMode (pinAIN2, OUTPUT);
pinMode (pinPWMB, OUTPUT); pinMode (pinBIN1, OUTPUT); pinMode (pinBIN2, OUTPUT); pinMode (pinSTBY, OUTPUT); Serial.begin(9600); panMotor.write(90); delay(15000); / / wait totdat ik klaar panMotor.attach(4); hecht de servo op pin 9 op het servo-object}
//////////////////////////////////////////////////////////LOOP//////////////////////////////////////////////////
void loop {panMotor.write(90); / / een sonar sensor naar 90 graden draaien
delay(500); ping sonar sensor om afstand int ons = sonar.ping(); int centerdistance = uS / US_ROUNDTRIP_CM; if(centerdistance==0) {centerdistance = 200;} newping bibliotheek vergis repareren
Als (centerdistance > dangerdistance) //if pad is duidelijk {/ / *** terwijl geen belemmering IN uw FRONT MOVING en CHECK FOR obstakel houden elke 100 MS ***
doen / / (terwijl sensor geven minder dan 30cm houden robot vooruit) {int ons = sonar.ping(); int centerdistance = uS / US_ROUNDTRIP_CM; / / check de sensoren delay(100); / / wait voor sensoren te stabiliseren motor (motor1, turnCW, 150), motor (motor2, turnCW, 150);} terwijl (centerdistance < dangerdistance);
} / /---pathe is geblokkeerd---anders //if pad is geblokkeerd {motorBrake(motor1); motorBrake(motor2); als (centerdistance
panMotor.write(175); / / draai links delay(900); int. leftdistance = (sonar.ping) / US_ROUNDTRIP_CM; scannen naar de linker delay(500); if(leftdistance==0) {leftdistance = 200;} Fix newping library vergis / / / vergelijken afstand rechts en links / / / als (leftdistance > rightdistance & & leftdistance > dangerdistance) //if links is minder gehinderd en super dan dangerdistance {//Stop Motor1, Motor2 motorStop(motor1); motorStop(motor2); panMotor.write (175); //turn sonar naar links "Maak je klaar om de linkerzijde van de robot controleren wanneer het weer" delay(400); time=millis(); doen / / (zet robot naar links terwijl sensor geven minder dan 30cm en tijd niet meer dan 800 ms) {int ons = sonar.ping(); afstand = uS / US_ ROUNDTRIP_CM; Controleer de sensoren delay(100); wachten voor sensoren te stabiliseren van //Turn naar motor1: Stop Motor1, langzame Motor2 motor (motor1, turnCCW, 180); Motor (motor2, turnCW, 180); timeplus=Millis(); } terwijl (afstand > dangerdistance & timeplus-time < 800);
motorBrake(motor1); motorBrake(motor2); panMotor.write(90); / / schakelen sonar naar oorsprong positie delay(2000);
} anders als (rightdistance > leftdistance & & rightdistance > dangerdistance) //if recht is minder gehinderd en super dan dangerdistance {//Stop Motor1, Motor2 motorStop(motor1); motorStop(motor2); panMotor.write(5); delay(400); time=millis(); //(do turn robot to right while sensor give less than 30cm and time not more then 800 ms) doen {int ons = sonar.ping(); afstand = uS / US_ROUNDTRIP_CM; / / check de sensoren delay(100); / / wait voor sensoren te stabiliseren van de motor (motor2 turnCCW, 180); Motor (motor1, turnCW, 180); timeplus=Millis(); } terwijl (afstand > dangerdistance & timeplus-time < 800); motorBrake(motor1); motorBrake(motor2);
panMotor.write(90); delay(2000); } else / / indien geen oplossing {/ / stop alles motorBrake(motor1); motorBrake(motor2); motorsStandby(); panMotor.write(90); delay(9000);}} }
///////////////////////////// FUNCTIONS /////////////////////////////////////////
VOID motor (boolean motorNumber, boolean motorDirection, int motorSpeed) {/ * dit een opgegeven motor, rijdt in een bepaalde richting, met een opgegeven snelheid:-motorNumber: motor1 of motor2---> Motor 1 of Motor 2 - motorDirection: turnCW of turnCCW---> met de klok mee of linksdraaiend - motorSpeed: 0 tot 255---> 0 = stop / 255 = fast * /
Booleaanse pinIn1; Betrekking heeft op AIN1 of BIN1 (afhankelijk van de motor nummer opgegeven)
De richting om te schakelen van de motor / / Clockwise opgeven: AIN1/BIN1 = HIGH en AIN2/BIN2 = lage / / capaciteitsopbouwende-rechtsom: AIN1/BIN1 = laag en AIN2/BIN2 = hoog als (motorDirection == turnCW) pinIn1 = hoog; anders pinIn1 = laag;
Selecteer de motor te schakelen, en stel de richting en de snelheid if(motorNumber == motor1) {digitalWrite (pinAIN1, pinIn1); digitalWrite (pinAIN2,! pinIn1); //This is het tegenovergestelde van de AIN1 analogWrite (pinPWMA, motorSpeed);} else {digitalWrite (pinBIN1, pinIn1); digitalWrite (pinBIN2,! pinIn1); //This is het tegenovergestelde van de BIN1 analogWrite (pinPWMB, motorSpeed);}
Ten slotte ervoor dat STBY is uitgeschakeld - trek het hoge digitalWrite (pinSTBY, hoge);
}
VOID motorrem (boolean motorNumber) {/ * deze "Korte rem" s de opgegeven motor, door de snelheid instellen op nul * /
Als (motorNumber == motor1) analogWrite (pinPWMA, 0); anders analogWrite (pinPWMB, 0); }
VOID motorStop (boolean motorNumber) {/ * dit stopt de opgegeven motor door beide pins IN te stellen op laag * / if (motorNumber == motor1) {digitalWrite (pinAIN1, laag); digitalWrite (pinAIN2, laag);} else {digitalWrite (pinBIN1, laag); digitalWrite (pinBIN2, laag);}}
VOID motorsStandby() {/ * dit zet de motoren in de standby-modus * / digitalWrite (pinSTBY, laag);}
