PathFinder (11 / 14 steg)

Steg 11: Arduino - kod

Följande kod redigerades använder Arduino 1.6.7. Detta är en fullständig lista över koden.

#include
RedBotSensor vänster = RedBotSensor(A3); initiera en vänster sensor objekt på A3
RedBotSensor center = RedBotSensor(A6); initiera en center sensor objekt på A6
RedBotSensor höger = RedBotSensor(A7); initiera en rätt sensor objekt på A7
konstanter som används i koden. LINETHRESHOLD är den att upptäcka
om sensorn är på linjen eller inte. Om sensorn värdet är större än detta
sensorn är ovanför en mörk linje.
//
HASTIGHETEN anger nominella hastigheten
#define LINETHRESHOLD 800
#define hastighet 60 / / anger nominella hastigheten. Tilldela ett värde mellan 0 - 255.
#define CORRECTIONFACTOR 10
#define ADJUSTMENTTHRESHOLD 4000 //milliseconds att starta en justering i förväg

RedBotMotors motorer.
int leftSpeed; variabeln som används för att lagra leftMotor hastighet
int rightSpeed; variabeln som används för att lagra rightMotor hastighet
osignerade långa startTime;
int correctionIndex = 0;
int correctionCounter = 0;
osignerade långa korrigeringar [20].
int correctionsDir [20].
char åtgärder.
bool biljett = false;
void setup()
{
Serial.BEGIN(9600);
Serial.println ("Välkommen till experiment - följande raden");
Serial.println("---");
Delay(2000);
Serial.println ("IR-sensorn avläsningar:");
Delay(500);
startTime = millis();
korrigeringar [0] = 0;
leftSpeed =-hastighet;
rightSpeed = hastighet;
}
void loop()
{
Serial.Print(left.Read());
Serial.Print("\t"); Tab-tecken
Serial.Print(Center.Read());
Serial.Print("\t"); Tab-tecken
Serial.Print(Right.Read());
Serial.println();
Serial.Print ("Action:"); Tab-tecken
Serial.Print(action);
Serial.println();
om (åtgärder == "g") {
om alla sensorer på svart eller upp i luften, stoppa motorerna.
annars kör motors ges kontroll hastigheterna ovan.
om ((left.read() > LINETHRESHOLD) || (Center.Read() > LINETHRESHOLD))
{
Turn(0);
}
annars om ((center.read() > LINETHRESHOLD) || (Right.Read() > LINETHRESHOLD))
{
Turn(1);
}
annat
{
applyAdjustment(nextCorrection());
motors.leftMotor(leftSpeed);
motors.rightMotor(rightSpeed);
}
Delay(0); lägga till en fördröjning för att minska känsligheten.
}
annat {
startTime = millis();
Motors.Brake();
}
}
Annullera tur (int direkt)
{
om (correctionCounter > = 0) correctionCounter--;
int justera = correctionCounter * 500;
addCorrection(direct,adjust);
Motors.stop();
leftSpeed = 200.
rightSpeed = -200;
motors.leftMotor(leftSpeed);
motors.rightMotor(rightSpeed);
Delay(100);
om (direkt == 0)
{
leftSpeed =-(SPEED - 50) * 2;
}
annat
{
rightSpeed = hastighet - 50 * 2.
}
motors.leftMotor(leftSpeed);
motors.rightMotor(rightSpeed);
Delay(250);
leftSpeed = - hastighet;
rightSpeed = hastighet;
}
void addCorrection (int riktning, int correctionTime)
{
korrigeringar [correctionIndex] = millis() - startTime-correctionTime;
correctionsDir [correctionIndex] = riktning;
Serial.println("Correction");
Serial.println(Corrections[correctionIndex]);
Serial.println(correctionsDir[correctionIndex]);
correctionIndex ++;
correctionCounter ++;
}
int nextCorrection()
{
int jag = 0;
osignerade långa currentTime = millis() - startTime;
medan ((korrigeringar [i] < currentTime) & & (jag < correctionIndex)) i ++;
tillbaka i;
}
void applyAdjustment (int nextCorrectionIndex)
{
osignerade långa currentTime = millis() - startTime;
osignerade långa remainingTime = korrigeringar [nextCorrectionIndex] - currentTime;
Tid att rätta
om (remainingTime < ADJUSTMENTTHRESHOLD)
{
om (correctionsDir [nextCorrectionIndex] == 1)
{
rightSpeed = hastighet + applyWeight(remainingTime);
leftSpeed =-(SPEED - applyWeight(remainingTime));
}
annat
{
rightSpeed = hastighet - applyWeight(remainingTime);
leftSpeed =-(hastighet + applyWeight(remainingTime));
}
biljett = sant;
}
annars om ((currentTime > corrections[nextCorrectionIndex]) & & (biljett == true))
{
biljett = false;
correctionIndex ++;
}
annat {//No korrigering
leftSpeed = - hastighet;
rightSpeed = hastighet;
}
}
int applyWeight (int wRemainingTime) {
osignerade långa currentTime = millis() - startTime;
int remainingTime=(int)abs(correctionsDir[nextCorrectionIndex]-currentTime);
int newSpeed = 0;
om ((wRemainingTime=2000)) {
newSpeed = CORRECTIONFACTOR * 1;
}
annars om ((wRemainingTime<2000) & (wRemainingTime > = 1000)) {
newSpeed = CORRECTIONFACTOR * 2;
}
annars om ((wRemainingTime<1000) & (wRemainingTime > = 400)) {
newSpeed = CORRECTIONFACTOR * 3;
}
annars om (wRemainingTime < 400) {
newSpeed = CORRECTIONFACTOR * 1;
}
återvända newSpeed;
}
void serialEvent() {
om (Serial.available()) {
Action = (char)Serial.read();
om (åtgärder == "g") {
correctionIndex = 0;
correctionCounter = 0;
}
Serial.println("EVENTO");
}
}

Koden i "setup"-metoden är vanlig standard koden för att ställa in meddelande till seriella portar och diagnos information. De "förseningarna" är viktigt så att du ger tid för alla att systemet är upp och löpande.

"Loop" koden

Loop koden är ansvarig för kontinuerligt titta på värdet i sensorerna. När en av dessa sensorer når en fördefinierad tröskel bör roboten vidta lämpliga åtgärder.

Om vänster-sensorn når bandet (tröskelvärde), sedan, måste roboten vända till höger. Likaså om höger-sensorn når bandet (tröskelvärde), måste roboten vända sig till vänster.

Roboten använder en kunskapsdatabas där det registrerar tiden när det kraschade mot linjen. Under varje körning, roboten registrerar varje krasch i framtiden går det kommer att söka i dess kunskapsbas att "look-ahead" för krascher och korrigerar sin väg.

Det finns också "justeringar". Till exempel, låt oss säga att i den första körningen roboten kraschade på sekunder 4 och 9. I kraschen på andra 4 roboten var tvungen att gå tillbaka lite för att korrigera sökvägen och därmed förlorar lite tid. Det innebär att kraschen på andra 9 skulle hända vid en annan tidpunkt i nästa tar om kraschen på andra 4 undviks. Detta "undvikande" fall spåras i variabeln "correctionCounter". Riktningen av korrigeringen också spåras.

Metoden "applyWeight" används för att lägga till en vikt till korrigering hastighet beroende på hur nära vi är att nästa förutspådde kraschen. Ju närmare till nästa kraschen desto snabbare anpassning måste ske.

"Turn" koden

Denna kod gör roboten sväng åt vänster eller höger beroende på de argument som skickas till den. Det verkliga arbetet i denna kod är att kunna bestämma avstånd och hastighet för att flytta för att komma tillbaka på rätt spår. Detta bestäms i princip av försök och fel strategi.

Se hur hastigheten på motorn är från början inställt till en bas av 200 och -200 för vänster och höger motors respektive. Denna värden i hastigheten kommer att göra roboten att gå bakåt.

Beroende på vilken riktning att vända, beräknas värdet av hastigheten för en motor eller annat. Detta är det sätt vi gör roboten att gå tillbaka på rätt spår.

Så enkelt som denna kod kan se ut, detta gör roboten att gå runt alla slags väg och när når ett slut, den kan finna sin väg runt att gå tillbaka och börja vägen igen.