Hur man gör ett hinder att undvika Arduino Robot! "Min Robot V1" (3 / 3 steg)
Steg 3: Flödesschema och kod
Först av allt vi börjar med rombformen, representerar rombformen en check. Vid denna kontroll ser vi om det finns ett objekt närmare än 30 cm till vår robot. Checken har två möjliga utfall, ja eller nej. Ja, vilket innebär att det finns faktiskt en del objekt närmare än 30 cm. Nej, vilket innebär att det finns inga objekt som upptäcks inom 30 cm.
Om det finns inget inom 30 cm roboten kan helt enkelt flytta fram som vägen är klar. Om det finns något närmare än 30 cm måste roboten utför hinder undvikande manövrer.
Den första etappen av hinder skatteundandragande är att stoppa roboten! Om du inte stoppa roboten omedelbart kommer det att krascha!
Phew! Det var dramatiska.
Roboten har upphört den behöver för att se hur det ska gå. Det gör detta genom att titta båda riktningarna, mycket som ska du när du korsar vägen. Först roboten vänder vänster tar en läsning, vänder rätt, tar en läsning.
En annan kontroll inträffar för att se vilken riktning är det bästa sättet att gå. Om vänster är vägen att gå den har att vända tillbaka till vänster och sedan gå framåt. Om just är vägen att gå roboten helt enkelt rör sig framåt som det som redan är vänd åt rätt håll. Så det var ganska lätt var det inte? Allt vi måste göra nu är att konvertera diagrammet till Arduino kod och det är det, hinder skatteundandragande kod gjort!
#include
#include
////////////////////////////////////////////////servo motor and sonar sensor ////////////////////////////////////////////
CONST int dangerdistance = 30. Säkerhetsavstånd för hinder (i cm) Servo panMotor; skapa ett servo objekt #define TRIGGER_PIN 3 / / Arduino PIN-kod knuten till utlösa pin på ultrasonic sensor. #define ECHO_PIN 2 / / Arduino PIN-kod knuten till echo pin på ultrasonic sensor. #define MAX_distance 200 / / maximalt avstånd vi vill skicka ping (i centimeter). Maximal sensorn avståndet klassas på 400-500 cm. NewPing sonar (TRIGGER_PIN, ECHO_PIN, MAX_distance); NewPing inställning av stift och maximalt avstånd. ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////// MOTOR //////////////////////////////////////////////////////////////////
*
Denna kod bedriver några enkla manövrar för att illustrera funktionerna: - motorDrive (motorNumber, motorDirection, motorSpeed) - motorBrake(motorNumber) - motorStop(motorNumber) - motorsStandby
Anslutningar: - stift 5---> PWMA - stift 7---> AIN2 - stift 8---> AIN1 - stift 11---> STBY - Pin 12---> BIN1 - stift 13---> BIN2 - stift 6---> PWMB
-Motor 1: A01 och A02 - Motor 2: B01 och B02
*
/ Ange stift
Motor 1 int pinAIN1 = 8; Riktning int pinAIN2 = 7. Riktning int pinPWMA = 5; Hastighet
Motor 2 int pinBIN1 = 12; Riktning int pinBIN2 = 13. Riktning int pinPWMB = 6; Hastighet
Standby int pinSTBY = 11;
Konstanter för att minnas de parametrar statisk boolean turnCW = 0; för motorDrive funktion statisk boolean turnCCW = 1; för motorDrive funktion statisk boolean motor1 = 0; för motorDrive, motorStop, motorBrake fungerar statisk boolean motor2 = 1; för motorDrive, motorStop, motorBrake funktioner
////////////////////////////////////////////////////////////////////////////////////////////////////////////////// unsigned long time; timer för boucle do... medan osignerade långa timeplus; / / timer för boucle göra... medan
int avstånd; int oss; void setup() {//Set PIN lägen motor pinMode (pinPWMA, produktionen), pinMode (pinAIN1, produktionen), pinMode (pinAIN2, produktionen);
pinMode (pinPWMB, produktionen); pinMode (pinBIN1, produktionen); pinMode (pinBIN2, produktionen); pinMode (pinSTBY, produktionen); Serial.BEGIN(9600); panMotor.write(90); Delay(15000); / / wait tills jag får redo panMotor.attach(4); tillmäter objektet servo servo på stift 9}
//////////////////////////////////////////////////////////LOOP//////////////////////////////////////////////////
void loop() {panMotor.write(90); / / vända en ekolod sensor till 90 graders
Delay(500); ping ekolod givare att få avståndet int oss = sonar.ping(); int centerdistance = oss / US_ROUNDTRIP_CM; IF(centerdistance==0) {centerdistance = 200.} fixa newping bibliotek misstas
om (centerdistance > dangerdistance) //if sökväg är klart {/ / *** medan inget hinder i din FRONT hålla MOVING och CHECK för hinder varje 100 MS ***
göra / / (medan sensorn ger mindre än 30cm hålla roboten framåt) {int oss = sonar.ping(); int centerdistance = oss / US_ROUNDTRIP_CM; / / kontrollera sensorer delay(100); / / wait för sensorer för att stabilisera motorDrive (motor1, turnCW, 150), motorDrive (motor2, turnCW, 150);} medan (centerdistance < dangerdistance);
} / /---pathe blockeras---annat //if väg är blockerad {motorBrake(motor1); motorBrake(motor2); om (centerdistance
panMotor.write(175); / / slå vänster delay(900); int leftdistance = sonar.ping () / US_ROUNDTRIP_CM; Skanna till den vänstra delay(500); IF(leftdistance==0) {leftdistance = 200.} fixa newping bibliotek fel / / / jämföra avstånd höger och vänster / / / om (leftdistance > rightdistance & & leftdistance > dangerdistance) //if vänster är mindre störd och super än dangerdistance {//Stop Motor1, Motor2 motorStop(motor1); motorStop(motor2); panMotor.write (175); //turn ekolod till vänster "redo att kontrollera vänster sida av robot när det vänder" delay(400); time=millis(); göra / / (stänger robot till vänster medan sensorn ger mindre än 30cm och inte mer tid då 800 ms) {int oss = sonar.ping(); avstånd = oss / US_ ROUNDTRIP_CM; kontrollera de sensorer delay(100); vänta för sensorer för att stabilisera //Turn mot motor1: stoppa Motor1, långsam Motor2 motorDrive (motor1, turnCCW, 180); motorDrive (motor2, turnCW, 180); timeplus=Millis(); } medan (avstånd > dangerdistance & timeplus-tid < 800);
motorBrake(motor1); motorBrake(motor2); panMotor.write(90); / / slå ekolod till ursprung position delay(2000);
} else om (rightdistance > leftdistance & & rightdistance > dangerdistance) //if rätten är mindre störd och super än dangerdistance {//Stop Motor1, Motor2 motorStop(motor1); motorStop(motor2); panMotor.write(5); delay(400); time=millis(); göra //(do turn robot to right while sensor give less than 30cm and time not more then 800 ms) {int oss = sonar.ping(); avstånd = oss / US_ROUNDTRIP_CM; / / kontrollera sensorer delay(100); / / wait för sensorer för att stabilisera motorDrive (motor2 turnCCW, 180); motorDrive (motor1, turnCW, 180); timeplus=Millis(); } medan (avstånd > dangerdistance & timeplus-tid < 800); motorBrake(motor1); motorBrake(motor2);
panMotor.write(90); Delay(2000); {} annat / / om ingen lösning {/ / stoppa allt motorBrake(motor1); motorBrake(motor2); motorsStandby(); panMotor.write(90); delay(9000);}} }
///////////////////////////// FUNCTIONS /////////////////////////////////////////
void motorDrive (boolean motorNumber, boolean motorDirection, int motorSpeed) {/ * Detta driver en viss motor, i en viss riktning, med en angiven hastighet:-motorNumber: motor1 eller motor2---> Motor 1 eller Motor 2 - motorDirection: turnCW eller turnCCW---> medsols eller motsols - motorSpeed: 0 till 255---> 0 = stopp / 255 = snabb * /
booleska pinIn1; Avser AIN1 eller BIN1 (beroende på motor numret anges)
Ange riktning för att stänga motorn / / medsols: AIN1/BIN1 = hög och AIN2/BIN2 = låg / / disk-motsols: AIN1/BIN1 = låg och AIN2/BIN2 = hög om (motorDirection == turnCW) pinIn1 = hög; annat pinIn1 = låg;
Välj motorn att vända och ange riktningen och hastigheten if(motorNumber == motor1) {digitalWrite (pinAIN1, pinIn1), digitalWrite (pinAIN2,! pinIn1); //This är motsatsen till den AIN1 analogWrite (pinPWMA, motorSpeed);} annat {digitalWrite (pinBIN1, pinIn1), digitalWrite (pinBIN2,! pinIn1); //This är motsatsen till den BIN1 analogWrite (pinPWMB, motorSpeed);}
Slutligen, se till STBY inaktiveras - dra den höga digitalWrite (pinSTBY, hög);
}
void motorBrake (boolean motorNumber) {/ * denna "Kort broms" s angivna motorn, genom att ställa in hastigheten till noll * /
om (motorNumber == motor1) analogWrite (pinPWMA, 0); annat analogWrite (pinPWMB, 0); }
Annullera motorStop (boolean motorNumber) {/ * Detta stoppar angivna motorn genom att ange båda IN stift till låg * / om (motorNumber == motor1) {digitalWrite (pinAIN1, låg), digitalWrite (pinAIN2, låg);} annat {digitalWrite (pinBIN1, låg), digitalWrite (pinBIN2, låg);}}
void motorsStandby() {/ * Detta sätter motorerna i Standby-läge * / digitalWrite (pinSTBY, låg);}