Gruppemedlemmer – Jakob Most, Sebastian Flensted, Troels Deibjerg, Nicklas Simonsen

Formål:

I denne opgave skal der bygges en autonom-robot, der vha. IR Line Sensors, kan navigere efter sorte streger på et kort og flytte en boks fra position A til en anden position, ligeledes skal robotten kunne gøre dette med en position B.

Robottens Opbygning:

Komponent liste

1x Arduino Uno

1x Arduino Motor Shield

2x Lego Motor

2x Sparkfun line sensor(QRE1113)

5x 3mm LED(2x hvid, grøn, rød, gul)

5x 220Ω Resistor

?x Lego Technic

Tanker bag opbygningen:

For at se robotten i aktion, se konklusion!

Hjulene til robotten er små, dette gør at det er nemmere at styre hastigheden og rotationen på robotten. Sensorerne er blevet placeret lidt foran hjulene dette giver en god kontrol over hvor robotten befinder sig i forhold til stregerne. Sensorerne er ligeledes placeret så tæt på gulvet som muligt ca 2-3mm da dette også giver nogle bedre værdier. Ved at placere sensorerne lidt foran hjulene kan vi nå at behandle inputtet inden hjulene når til hvor line sensorerne læser. Med denne placering kører robotten ikke for langt.

For at robotten ikke behøver mere end 2 hjul har vi valgt at give robotten en ”støtte pind”, som bare er en legoklods der holder robotten oppe så den ikke kan vælte bagover.

Derudover er robotten udstyret med to fremadrettede fangarme til at kontrollere kasserne.

 

Motor og Hjul

Størrelsen af hjulene var ikke gennemtænkt fra start af. Første sæt hjul havde en radius på 8 cm dette giver altså en omkreds på 50,26 cm. Dette viste sig hurtigt at give store problemer da proportionerne mellem robottens bredde og hjulenes omkreds gjorde at robotten var umulig at kontrollere. Dette kunne vi have løst ved at lave en gearing mellem hjul og motor, men da der ikke var nogen grund til at hjulene skulle være store (udover offroad), og for at undgå at lave en unødvendig gearing, valgte vi at re-designe.
Til det nye design gik vi efter små hjul og fandt et par med en omkreds på 14,13cm hvilket vil sige cirka 72% mindre end de oprindelige. Dette gjorde at vi kunne styre vores robot da det blev muligt at bruge data fra line sensorerne. Det medførte også et komplet rebuild af robotten som blev meget mere kompakt.

Motorerne sidder placeret direkte på hjulene uden nogen gearing, på denne måde får vi et lavt tyngdepunkt pga. de små hjul, hvilket gør robotten stabil.

Robottens Hardware:

Diagramtegning

Overblik

DoF set forfra.

PCB

PCB boardet er lavet i programmet Eagle. Eagle sketchen (Figur 1) laves så der er plads til 5 Led’er og 5 modstande. At bruge 1 resistor til hver LED er umiddelbart den nemme løsning. Der kunne have været brugt 1 resistor samt nogle ensretter dioder. Led’erne er inddelt så hver LED lyser konstant når en bestemt funktion er i gang. Når robotten kører frem, lyser den grønne, tilbage lyser den røde, 180 grader er gul, højre og venstre er hvid som er placeret i hver side af PCB’et så det er nemt og intuitivt at forstå. Printet er også lavet så alt rundt om alle komponenter på printpladen er GND (Figur 3), dette gør at der kun behøves en GND fra boardet til Arduinoen.

PCB-printet havde været rart at få lavet lidt tidligere, da det kunne have hjulpet med til at gøre fejlfindingen lettere.

Til at starte med tog vi printet som vi havde lavet i Eagle og printede det ud på kalk papir. Herefter tog vi en printplade og tog den blå beskyttelsestape af. Vi klippede så printpapiret og printpladen til, så de passede sammen, lagde papiret oven på pladen og lagde det i UV-lys i 1-2 minutter så printet fra papiret ville blive belyst ind i printpladen. Printpladen lægges derefter i base i 1-2 minutter. Efter det, skylles printet godt i vand. Herefter lægges det i syre i 20-25 minutter og skylles herefter i vand hvorefter det gives 1-2 minutter i UV-lys igen. Til sidst lægges det igen i base 1-2 minutter og så er det færdigt. Husk at have sikkerhedsudstyr på under hele processen – kittel og handsker, for at undgå beskadiget tøj eller kontakt til huden med de forskellige kemikalier.

Når printet var fremstillet med de rigtige mål og markeringer kunne det bores. Det er vigtigt at bore ret præcist så der ikke er mulighed for at lodningerne man sætter rammer GND.  

Til sidst skal printet loddes, det er vigtigt at ramme inde for alle linjer og huller som er lavet, ellers vil det blive koblet til GND.

Line Sensorer – QRE1113(Analog)

Line sensoren virker ved at sende infrarødt (IR) lys ud og måle hvor meget der kommer tilbage. Når den er over noget der er hvidt reflekteres der altså meget mere lys end når den er over noget sort. Line Sensoren består af to dele en IR LED og en IR phototransistor. Phototransistoren ændrer altså hvor meget spænding der kommer til output pinnen. Når der ikke bliver reflekteret særlig meget IR lys får vi et højt output og lavt når der bliver reflekteret meget, altså kan vi sige at når vi får et højt output når den er over noget sort og et lavt output når den over noget hvidt. I vores tests fik vi mellem 50-100 på hvid og mellem 900-1000 på sort.

Arduino Motor Shield


Vi bruger Arduino motor shield til at styre vores motorer da dette sparer os for at have en H-bro.

H-Bro

Med en H-bro kan vi nemlig vende ”retning” på strømmen og lade vores DC-motor rotere både med og mod uret. Dette gøres ved at f.eks lukke S1 og S4 eller S2 og S3.

Med et motor shield (shield er bare en general betegnelse for add-on moduler) som praktisk talt er en stor h-bro, og har predefinerede pins kan vi direkte gennem pins gå ind og styre adfærd. Desuden kan vi tilkoble en højere spænding f.eks et 9V batteri, men også helt op til 12V.

Vi bruger en ekstern strømkilde da vores DC motor er en 9V motor og derfor kører bedst med 9V batteri tilkoblet. Med kun 5V tilsluttet kunne motoren knap dreje vores opstilling,

DC motor samt ekstern strømkilde. (billede fra google)

Hardware Problemer

  • Under opstillingen af robotten fandt vi ud af at afstanden fra underlag til Line sensorene var vigtige i forhold til at få brugbare værdier.
  • Fangarmenes længde og mellemrum: Efter en del forsøg på at opsamle kasserne justerede vi mellemrum og længde indtil fejlmarginen på opsamling var minimal mod 0%.
  • Ledninger i Arduinoen: I opbygningen af robotten er der brugt bløde ledninger hvilket gør det nemmere at arbejde og ”modellere” rundt med når det hele bliver sat tæt sammen, men de bløde ledninger har også svært ved at blive siddende i arduinoen når robotten kører rundt, hvilket en del gange medførte fejl under testning. Dette blev løst ved at sætte noget tape over alle ledninger som sad på Arduinoen så de ikke ville falde ud.
  • PCB krævede 2 kurser at fremstille: Fremstillingen af PCB board krævede både PCB-kursus samt adgangsgivende kursus til brug af boremaskiner. Derudover var PCB-kurset, relativt hurtigt, og der måtte bruges ekstra tid på at gennemgå processen igen.
  • Den ene diode virkede ikke pga. dårlig lodning: Efter fremstilling af PCB board fungerede den ene diode ikke. Dette, efter lidt ”debugging”, skyldes en mangelfuld forbindelse via en dårlig lodning. Problemet blev nemt løst.
  • Manglende 9v batteri: Mangel på 9V batteri gjorde at vi simulerede denne kilde vha. en regulerbar DC strømforsyning. Dette krævede dog lange ledninger der havde en tildens til at trække i robotten underkørsel. Vi måtte derfor konstant holde ledningerne 90 grader over robotten for at sikre minimalt træk. Samtidig var det også godt med en strømkilde der havde et konsekvent output for at hastighed af motorer også var konsekvent.
  • Hjulenes rotationsretning: grundet ombygning af robotten blev motorens retning ikke lige overvejet ved opbygning af den nye robot, hvilket forsagede robotten kørte baglæns og den
  • forkerte vej ved drej. Dette blev løst ved at ændre pins i koden. 

Robottens Opførsel:

Flowchart

Transition conditions:

Den overordnede betingelse for de fleste handlinger af robotten er at begge sensorer skal kunne se hvid på samme tid. Den eneste handling som går direkte imod dette er når robotten skal køre ligeud, da vil den køre indtil begge sensorer ser sort.

Når robotten drejer og skal finde ud af om den har drejet nok vil slut-betingelsen være at begge sensorer ser hvidt, dette vil indikere at sensorerne er på hver side af en stribe. I tilfælde af at dreje 180 grader, vil en af betingelserne samtidig også være at der skal være gået et kort stykke tid, for at sikre sig at den ikke stopper med at dreje halvvejs i sin rotation.

Kode og beskrivelse:

int leftEngine = 13;
int leftEngineBrake = 8;
int leftEngineSpeedPin = 11;
int rightEngine = 12;
int rightEngineBrake = 9;
int rightEngineSpeedPin = 3;

int rightEngineSpeed = 140;
int leftEngineSpeed = 140;

int ledTurnRight = 4; 
int ledTurnLeft = 5; 
int ledTurnGoStraight = 6; 
int ledTurnGoBackwards = 7;
int ledTurnAround = 2;

int sensorPinL = A2;
int sensorPinR = A5;

int prevLeftSensorValue;
int prevRightSensorValue;
int leftSensorValue;
int rightSensorValue;
int sensorBreakOffValue = 500;

int turnDelay = 30;
int readDelay = 5;
int nextOrderDelay = 1;

void setup() {
  pinMode(ledTurnRight,OUTPUT);
  pinMode(ledTurnLeft,OUTPUT);
  pinMode(ledTurnGoStraight,OUTPUT);
  pinMode(ledTurnGoBackwards,OUTPUT);
  pinMode(ledTurnAround,OUTPUT);

  Serial.begin(9600);

  pinMode(leftEngine, OUTPUT); 
  pinMode(leftEngineBrake, OUTPUT); 
  pinMode(leftEngineSpeedPin, OUTPUT); 

  pinMode(rightEngine, OUTPUT); 
  pinMode(rightEngineBrake, OUTPUT);
  pinMode(rightEngineSpeedPin, OUTPUT);

  pinMode(sensorPinL, INPUT);
  pinMode(sensorPinR, INPUT);

}

void loop() {
  delay(2000);
  ReadSensors();
  ReadSensors();
  /*
  //A

  GoStraight();
  GoStraight();
  TurnRight();
  GoStraight();
  GoStraight();
  TurnLeft();
  GoStraightCare();
  Turn180Around(); //PICKUPå
  GoStraight();
  TurnLeft();
  GoStraight();
  GoStraight();
  Party();

  //B
  GoBackwards();
  Turn180Around();
  GoStraight();
  GoStraight();
  TurnRight();
  GoStraightCare();
  Turn180Around(); //PICKUP
  GoStraight();
  TurnLeft();
  GoStraight();
  GoStraight();
  TurnRight();
  GoStraight();
  TurnLeft();
  GoStraight();
  Party();
*/
  //C
  GoBackwards();
  TurnLeft();
  GoStraight();
  TurnLeft();
  GoStraight();
  GoStraight();
  GoStraight();
  TurnRight();
  GoStraight();
  Turn180Around(); //PICKUP
  GoStraight();
  TurnLeft();
  GoStraight();
  GoStraight();
  GoStraight();
  TurnRight();
  GoStraight();
  GoStraight();
  TurnLeft();
  GoStraight();
  Party();

  //D
  GoBackwards();
  TurnLeft();
  GoStraight();
  GoStraight();
  TurnLeft();
  GoStraight();
  GoStraight();
  GoStraight();
  GoStraight();
  TurnRight();
  GoStraight();
  Turn180Around(); //PICKUP
  GoStraight();
  TurnLeft();
  GoStraight();
  TurnRight();
  GoStraight();
  TurnLeft();
  GoStraight();
  GoStraightCare();
  GoStraight();
  Party();

  //F
  /*
  GoBackwards();
  Turn180Around();
  GoStraightCare();
  TurnRight();
  GoStraight();
  TurnLeft();
  GoStraight();
  GoStraight();
  GoStraight();
  GoStraight();
  TurnRight();
  GoStraight();
  Turn180Around();  //PICKUP
  GoStraight();
  GoStraight();
  GoStraight();
  GoStraight();*/

  //E
  /*GoBackwards();
  Turn180Around();
  GoStraightCare();
  TurnRight();
  GoStraight();
  TurnLeft();
  GoStraight();
  GoStraight();
  GoStraight();
  TurnRight();
  GoStraight();
  Turn180Around();  //PICKUP
  GoStraight();
  GoStraight();
  GoStraightReckless();*/
  
   //H
  GoBackwards();  //Go to Pickup
  Turn180Around();
  GoStraightCare();
  TurnRight();
  GoStraight();
  TurnLeft();
  GoStraight();
  GoStraight();
  GoStraight();
  GoStraight();
  GoStraight();
  GoStraight();

  TurnRight();
  GoStraight();
  Turn180Around(); //PICKUP
  GoStraight();
  TurnRight();
  
  GoStraight();
  TurnSharpRight();
  GoStraight();
  TurnSharpLeft();
  GoStraight();
  TurnSharpRight();
  GoStraight();
  
  Party();
  Party();
  Party();
  Party();
  delay(10000); 
}

void ReadSensors(){
  
  if(abs(prevLeftSensorValue - analogRead(sensorPinL)) < 250){
    leftSensorValue = analogRead(sensorPinL);
  }
  if(abs(prevRightSensorValue - analogRead(sensorPinR)) < 250){
    rightSensorValue = analogRead(sensorPinR);
  }
  prevLeftSensorValue = analogRead(sensorPinL);
  prevRightSensorValue = analogRead(sensorPinR);
}

void RightMotorGo(){
  digitalWrite(rightEngine, LOW); //TJEK OM HIGH/LOW ER FREMAD
  digitalWrite(rightEngineBrake, LOW); 
  analogWrite(rightEngineSpeedPin, rightEngineSpeed); //MANGLER PIN INFO
}

void RightMotorGo(int minusValue){
  digitalWrite(rightEngine, LOW); //TJEK OM HIGH/LOW ER FREMAD
  digitalWrite(rightEngineBrake, LOW); 
  analogWrite(rightEngineSpeedPin, rightEngineSpeed - minusValue); //MANGLER PIN INFO
}

void LeftMotorGo(){
  digitalWrite(leftEngine, LOW);
  digitalWrite(leftEngineBrake, LOW);
  analogWrite(leftEngineSpeedPin, leftEngineSpeed); //MANGLER PIN INFO
}

void LeftMotorGo(int minusValue){
  digitalWrite(leftEngine, LOW);
  digitalWrite(leftEngineBrake, LOW);
  analogWrite(leftEngineSpeedPin, leftEngineSpeed - minusValue); //MANGLER PIN INFO
}

void RightMotorReverse(){
  digitalWrite(rightEngine, HIGH); //TJEK OM HIGH/LOW ER FREMAD
  digitalWrite(rightEngineBrake, LOW); 
  analogWrite(rightEngineSpeedPin, rightEngineSpeed); //MANGLER PIN INFO
}

void LeftMotorReverse(){
  digitalWrite(leftEngine, HIGH);
  digitalWrite(leftEngineBrake, LOW);
  analogWrite(leftEngineSpeedPin, leftEngineSpeed); //MANGLER PIN INFO
}

void RightMotorReverse(int minusValue){
  digitalWrite(rightEngine, HIGH); //TJEK OM HIGH/LOW ER FREMAD
  digitalWrite(rightEngineBrake, LOW); 
  analogWrite(rightEngineSpeedPin, rightEngineSpeed - minusValue); //MANGLER PIN INFO
}

void LeftMotorReverse(int minusValue){
  digitalWrite(leftEngine, HIGH);
  digitalWrite(leftEngineBrake, LOW);
  analogWrite(leftEngineSpeedPin, leftEngineSpeed - minusValue); //MANGLER PIN INFO
}

void GoStraight()
{

  Serial.println("I AM STRAIGHT");
  // --- LED INDICATOR --
  digitalWrite(ledTurnGoStraight, HIGH);
  digitalWrite(ledTurnRight, LOW);
  digitalWrite(ledTurnLeft, LOW);
  digitalWrite(ledTurnGoBackwards, LOW);
  digitalWrite(ledTurnAround, LOW);
  

  RightMotorGo();
  
  LeftMotorGo();

  ReadSensors();
  ReadSensors();

  while(leftSensorValue > sensorBreakOffValue && rightSensorValue > sensorBreakOffValue){ //While the sensors only detect black
    delay(readDelay);
    ReadSensors();
  }
  while(leftSensorValue < sensorBreakOffValue || rightSensorValue < sensorBreakOffValue){ //While any of the sensors detects white
    if(rightSensorValue < sensorBreakOffValue){
      RightMotorGo();
    }else{
      digitalWrite(rightEngineBrake, HIGH);
    }
    if(leftSensorValue < sensorBreakOffValue){
      LeftMotorGo();
    }else{
      digitalWrite(leftEngineBrake, HIGH);
    }
    delay(readDelay);
    ReadSensors();
  }

  digitalWrite(rightEngineBrake, HIGH);
  digitalWrite(leftEngineBrake, HIGH);

  delay(nextOrderDelay);
}

void GoStraightReckless(){
  Serial.println("I AM STRAIGHT");
  // --- LED INDICATOR --
  digitalWrite(ledTurnGoStraight, HIGH);
  digitalWrite(ledTurnRight, LOW);
  digitalWrite(ledTurnLeft, LOW);
  digitalWrite(ledTurnGoBackwards, LOW);
  digitalWrite(ledTurnAround, LOW);
  

  RightMotorGo();
  
  LeftMotorGo();

  delay(1500);

  GoStraight();
}

void GoStraightCare()
{

  Serial.println("I AM STRAIGHT");
  // --- LED INDICATOR --
  digitalWrite(ledTurnGoStraight, HIGH);
  digitalWrite(ledTurnRight, LOW);
  digitalWrite(ledTurnLeft, LOW);
  digitalWrite(ledTurnGoBackwards, LOW);
  digitalWrite(ledTurnAround, LOW);
  

  RightMotorGo();
  
  LeftMotorGo();

  delay(turnDelay * 10 / 4);
  
  ReadSensors();

  while(leftSensorValue > sensorBreakOffValue && rightSensorValue > sensorBreakOffValue){ //While the sensors only detect black
    delay(readDelay/2);
    ReadSensors();
  }
  while(leftSensorValue < sensorBreakOffValue || rightSensorValue < sensorBreakOffValue){ //While any of the sensors detects white
    RightMotorGo(40);
    LeftMotorGo(40);
    if(leftSensorValue > sensorBreakOffValue){
      RightMotorGo(40);
      LeftMotorReverse(40);
    }
    if(rightSensorValue > sensorBreakOffValue){
      LeftMotorGo(40);
      RightMotorReverse(40);
    }
    delay(readDelay/2);
    ReadSensors();
  }

  digitalWrite(rightEngineBrake, HIGH);
  digitalWrite(leftEngineBrake, HIGH);

  delay(nextOrderDelay);
}


void TurnRight()
{
    Serial.println("I AM RIGHT");

  // --- LED INDICATOR --
  digitalWrite(ledTurnGoStraight, LOW);
  digitalWrite(ledTurnRight, HIGH);
  digitalWrite(ledTurnLeft, LOW);
  digitalWrite(ledTurnGoBackwards, LOW);
  digitalWrite(ledTurnAround, LOW);

  ReadSensors();

  int iterator = 0;

  while(rightSensorValue > sensorBreakOffValue || leftSensorValue > sensorBreakOffValue || iterator < turnDelay){
    LeftMotorGo();
    RightMotorReverse(50);
    delay(readDelay);
    ReadSensors();
    iterator++;
  }
  
  digitalWrite(rightEngineBrake, HIGH);
  digitalWrite(leftEngineBrake, HIGH);

  delay(nextOrderDelay);
}

void TurnLeft()
{
    Serial.println("I AM LEFT");

  // --- LED INDICATOR --
  digitalWrite(ledTurnGoStraight, LOW);
  digitalWrite(ledTurnRight, LOW);
  digitalWrite(ledTurnLeft, HIGH);
  digitalWrite(ledTurnGoBackwards, LOW);
  digitalWrite(ledTurnAround, LOW);

  ReadSensors();

  int iterator = 0;

  while(rightSensorValue > sensorBreakOffValue || leftSensorValue > sensorBreakOffValue || iterator < turnDelay){
    RightMotorGo();
    LeftMotorReverse(50);
    delay(readDelay);
    ReadSensors();
    iterator++;
  }
  
  digitalWrite(rightEngineBrake, HIGH);
  digitalWrite(leftEngineBrake, HIGH);

  delay(nextOrderDelay);
}

void TurnSharpRight()
{
    Serial.println("I AM RIGHT");

  // --- LED INDICATOR --
  digitalWrite(ledTurnGoStraight, LOW);
  digitalWrite(ledTurnRight, HIGH);
  digitalWrite(ledTurnLeft, LOW);
  digitalWrite(ledTurnGoBackwards, LOW);
  digitalWrite(ledTurnAround, LOW);

  ReadSensors();


  while(rightSensorValue > sensorBreakOffValue){
    LeftMotorGo();
    RightMotorGo();
    delay(readDelay);
    ReadSensors();
  }

  int iterator = -10;

  while(rightSensorValue > sensorBreakOffValue || leftSensorValue > sensorBreakOffValue || iterator < turnDelay){
    LeftMotorGo();
    RightMotorReverse();
    delay(readDelay);
    ReadSensors();
    iterator++;
  }
  
  digitalWrite(rightEngineBrake, HIGH);
  digitalWrite(leftEngineBrake, HIGH);

  delay(nextOrderDelay);
}

void TurnSharpLeft()
{
    Serial.println("I AM LEFT");

  // --- LED INDICATOR --
  digitalWrite(ledTurnGoStraight, LOW);
  digitalWrite(ledTurnRight, LOW);
  digitalWrite(ledTurnLeft, HIGH);
  digitalWrite(ledTurnGoBackwards, LOW);
  digitalWrite(ledTurnAround, LOW);

  ReadSensors();

  while(leftSensorValue > sensorBreakOffValue){
    LeftMotorGo();
    RightMotorGo();
    delay(readDelay);
    ReadSensors();
  }

  int iterator = -10;


  while(rightSensorValue > sensorBreakOffValue || leftSensorValue > sensorBreakOffValue || iterator < turnDelay){
    RightMotorGo();
    LeftMotorReverse();
    delay(readDelay);
    ReadSensors();
    iterator++;
  }
  
  digitalWrite(rightEngineBrake, HIGH);
  digitalWrite(leftEngineBrake, HIGH);

  delay(nextOrderDelay);
}

void GoBackwards()
{
    Serial.println("I AM GOING BACKWARDS");

  // --- LED INDICATOR --
  digitalWrite(ledTurnGoStraight, LOW);
  digitalWrite(ledTurnRight, LOW);
  digitalWrite(ledTurnLeft, LOW);
  digitalWrite(ledTurnGoBackwards, HIGH);
  digitalWrite(ledTurnAround, LOW);

  RightMotorReverse();
  
  LeftMotorReverse();

  ReadSensors();

  while(leftSensorValue > sensorBreakOffValue && rightSensorValue > sensorBreakOffValue){ //While the sensors only detect black
    delay(readDelay);
    ReadSensors();
  }
  delay(50);
  while(leftSensorValue < sensorBreakOffValue || rightSensorValue < sensorBreakOffValue){ //While any of the sensors detects white
    AlignInBackwards();
    RightMotorReverse();
    LeftMotorReverse();
    
    delay(readDelay);
    ReadSensors();
  }

  RightMotorReverse();
  LeftMotorReverse();
  delay(20);

  digitalWrite(rightEngineBrake, HIGH);
  digitalWrite(leftEngineBrake, HIGH);

  delay(nextOrderDelay);
}

void AlignInBackwards(){
  
}

void Turn180Around()
{
    Serial.println("I AM 180!!");

    // --- LED INDICATOR --
  digitalWrite(ledTurnGoStraight, LOW);
  digitalWrite(ledTurnRight, LOW);
  digitalWrite(ledTurnLeft, LOW);
  digitalWrite(ledTurnGoBackwards, LOW);
  digitalWrite(ledTurnAround, HIGH);
  ReadSensors();

  while(leftSensorValue > sensorBreakOffValue){ //While left detects black
    RightMotorGo(20);
    LeftMotorReverse(25);
    delay(readDelay);
    ReadSensors();
  }

  while(leftSensorValue < sensorBreakOffValue){ //while left detects white
    RightMotorGo(20);
    LeftMotorReverse(25);
    delay(readDelay);
    ReadSensors();
  }

  while(leftSensorValue > sensorBreakOffValue){ //While left detects black
    RightMotorGo(20);
    LeftMotorReverse(25);
    delay(readDelay);
    ReadSensors();
  }

/*
 * 1: Move past cross until both are white
 * 2: Move Right sensor to black
 * 3: move left sensor to black, then white ((While keeping right inside black))
 * 4: move right sensor to white
 * 5: move to black both
 */
 /*
  //1
  while(rightSensorValue > sensorBreakOffValue && leftSensorValue > sensorBreakOffValue){ //While any of them are black
    RightMotorGo();
    LeftMotorGo();
    delay(10);
    ReadSensors();
  }
  digitalWrite(rightEngineBrake, HIGH);
  digitalWrite(leftEngineBrake, HIGH);
  //2
  while(rightSensorValue < sensorBreakOffValue){
    RightMotorGo();
    delay(10);
    ReadSensors();
  }
  digitalWrite(rightEngineBrake, HIGH);
  digitalWrite(leftEngineBrake, HIGH);
  //3
  while(leftSensorValue < sensorBreakOffValue){
    
    if(rightSensorValue < sensorBreakOffValue){
      RightMotorGo();
    }else{
      digitalWrite(rightEngineBrake, HIGH);
    }
    
    LeftMotorGo();
    delay(10);
    ReadSensors();
  }
  digitalWrite(rightEngineBrake, HIGH);
  digitalWrite(leftEngineBrake, HIGH);
  while(leftSensorValue > sensorBreakOffValue){
    
    if(rightSensorValue < sensorBreakOffValue){
      RightMotorGo();
    }else{
      digitalWrite(rightEngineBrake, HIGH);
    }
    
    LeftMotorGo();
    delay(10);
    ReadSensors();
  }
  digitalWrite(rightEngineBrake, HIGH);
  digitalWrite(leftEngineBrake, HIGH);
  //4
  while(rightSensorValue > sensorBreakOffValue){
    if(leftSensorValue > sensorBreakOffValue){
      LeftMotorReverse();
    }else{
      digitalWrite(leftEngineBrake, HIGH);
    }
    
    RightMotorGo();
    delay(10);
    ReadSensors();
  }
  //5
  while(leftSensorValue < sensorBreakOffValue || rightSensorValue < sensorBreakOffValue){ //While any of the sensors detects white
    if(rightSensorValue < sensorBreakOffValue){
      RightMotorGo();
    }else{
      digitalWrite(rightEngineBrake, HIGH);
    }
    if(leftSensorValue < sensorBreakOffValue){
      LeftMotorGo();
    }else{
      digitalWrite(leftEngineBrake, HIGH);
    }
    delay(10);
    ReadSensors();
  }*/
  
  
  digitalWrite(rightEngineBrake, HIGH);
  digitalWrite(leftEngineBrake, HIGH);

  delay(nextOrderDelay);
}

void Party(){

  for(int i = 0; i < 3; i ++){
  digitalWrite(ledTurnLeft, HIGH);
  delay(75);
  digitalWrite(ledTurnLeft, LOW);
  
  digitalWrite(ledTurnGoBackwards, HIGH);
  delay(75);
  digitalWrite(ledTurnGoBackwards, LOW);
  
  digitalWrite(ledTurnGoStraight, HIGH);
  delay(75);
  digitalWrite(ledTurnGoStraight, LOW);
  
  digitalWrite(ledTurnAround, HIGH);
  delay(75);
  digitalWrite(ledTurnAround, LOW);
  
  digitalWrite(ledTurnRight, HIGH);
  delay(75);
  digitalWrite(ledTurnRight, LOW);
  }
}



//------------------ TRACK A -----------------------

Koden er baseret på konceptet af “finite state machines”. Hver en kommando er en state som robotten går ind i, og hver state bliver håndteret med et eller flere while loops, som tjekker hvorledes transition conditions for et givet state er fuldført.

Fejlhåndteringen af eventuelle fejlinput fra line sensors fungerer således at hvis den forrige værdi som er blevet givet af sensorerne, er markant anderledes fra den nye værdi, vil den nye værdi ignoreres, men stadig gemmes som den forrige værdi, så hvis næste gang man læser input fra sensorerne, værdierne stadig har den markante ændring vil den nye værdi godtages.

void ReadSensors(){
  
  if(abs(prevLeftSensorValue - analogRead(sensorPinL)) < 250){
    leftSensorValue = analogRead(sensorPinL);
  }
  if(abs(prevRightSensorValue - analogRead(sensorPinR)) < 250){
    rightSensorValue = analogRead(sensorPinR);
  }
  prevLeftSensorValue = analogRead(sensorPinL);
  prevRightSensorValue = analogRead(sensorPinR);
}

Fremad

void GoStraight()
{

  Serial.println("I AM STRAIGHT");
  // --- LED INDICATOR --
  digitalWrite(ledTurnGoStraight, HIGH);
  digitalWrite(ledTurnRight, LOW);
  digitalWrite(ledTurnLeft, LOW);
  digitalWrite(ledTurnGoBackwards, LOW);
  digitalWrite(ledTurnAround, LOW);
  

  RightMotorGo();
  
  LeftMotorGo();

  ReadSensors();
  ReadSensors();

  while(leftSensorValue > sensorBreakOffValue && rightSensorValue > sensorBreakOffValue){ //While the sensors only detect black
    delay(readDelay);
    ReadSensors();
  }
  while(leftSensorValue < sensorBreakOffValue || rightSensorValue < sensorBreakOffValue){ //While any of the sensors detects white
    if(rightSensorValue < sensorBreakOffValue){
      RightMotorGo();
    }else{
      digitalWrite(rightEngineBrake, HIGH);
    }
    if(leftSensorValue < sensorBreakOffValue){
      LeftMotorGo();
    }else{
      digitalWrite(leftEngineBrake, HIGH);
    }
    delay(readDelay);
    ReadSensors();
  }

  digitalWrite(rightEngineBrake, HIGH);
  digitalWrite(leftEngineBrake, HIGH);

  delay(nextOrderDelay);
}

Robotten formodes at starte på et kryds, dvs. Begge sensorer bør give sort til at starte med. Robotten vil da køre ligeud indtil en af de to sensorer læser hvid i stedet. Herfra vil robotten køre fremad, dog med undtagelsen; hvis en sensor læser sort, vil det tilsvarende hjul stoppe med at rotere, indtil sensoren igen læser hvid.

Når begge sensorer læser sort, vil robotten så stoppe igen, og kan gå videre til næste kommando.

Drej

Robotten får besked på at dreje det ene hjul fremad, imens det andet hjul drejer bagud i mindre fart. Det vil den så gøre så længe minimum én af sensorerne kan læse sort, og så længe der ikke er gået mere end 300 millisekunder.

Drej 180 grader

Robotten får besked på at køre fremad med højre hjul, og bagud med venstre, hvilket forsager in så smal vending som muligt. Den gør dette i 3 iterationer som har hvert deres betingelser: Så længe venstre sensor læser sort, derefter så længe venstre sensor læser hvid, og til sidst så længe venstre sensor læser sort. Dette vil betyde at robotten har drejet sig selv ud af det kryds den står i, derefter dreje ind over den linje som den før stod på, og dermed have vendt rundt 180 grader.

Udfordringer med kode:

Selvom man har opstillet præcise krav til hvert state, vurderet hastigheden, så er der altid input fra den virkelige verden der generer robottens udførsel.

f.eks. var der kæmpe forskel hvordan robotten drejer ved forskellige hastigheder, her opdagede vi en enorm forskel på opførslen ved at ændre motorhastigheden fra 160 til 140.

Derudover var opsamling af kasserne også nødt til at foregå i et andet tempo for at undgå at de blev ”slynget” ud under 180 graders rotationen. Derfor blev states som GoStraightCare() konstrueret sammen med andre states til at udføre specifikke opgaver de normale states ikke ville kunne klare. Her blandt andet GoStraightReckless() til at klare opgave F, samt Party() der blinker alle LED ved aflevering af kassen.

En stor udfordring var at visualisere eller forudse hvordan robotten ville opføre sig i det virkelige når den skulle udføre sine states. F.eks ved udfordring H skal der rationaliseres grundigt for at finde de rigtige regler for hvordan den skal opføre sig når den højre sensor bliver sort kontra den venstre. Dette er helt klar en af de mest interessant aspekter af denne opgave. At finde de rigtige regler for præcist at kunne konstruere et godt state der fungerer uafhængigt af andre.

Konklusion:

Daughter of Freedom (DoF) har været igennem en iterationsproces. DoF startede ud som et kæmpe monster med store hjul, men blev til en kompakt elegant robot med små hjul. Udover en smule trial and error med afstand mellem Line Sensors og fangarme har det meste af tweakingen ligget i programmeringen af robotten. Umiddelbart syntes vi at vi har fået lavet nogle simple og gode Transition Conditions som sikrer at der bliver skiftet korrekt mellem states både når der drejes og køres lige ud.

Selve opbygningen af koden blev udført konstruktivt så hvert state har klare conditions, så hvert state kunne kaldes i hvilken som helst ønsket rækkefølge. Dette gør at robotten med meget få ændringer kunne klargøres, vha at hardcode rækkefølgen af states, og udføre en ny udfordring.

 DoF følger både lige og buede linjer rigtig flot, og klarer konsekvent de to obligatoriske ruter A og B. Udover rute A og B klarer DoF også rute C, D og H. Vi håber at få disse filmet snarest og lagt dem op.

Rute A og B

Leave a Reply