Jacob Nielsen

Formål:

At bygge og programmere en farvesorteringsmaskine, der fungerer efter samme principper som tilsvarende maskiner i industrien.

Links til youtube-videoer fra industrien – sortering (f.eks.)
- Tomatsortering.
- Plastik.
- Oliven.
DIY sorteringsmaskiner:
- Plastikklodser.
- M&Ms 1.
- M&Ms 2.

Undersøgelse.

Hvad er RGB-værdier?
Hvad skal man bruge for at kunne detektere farver elektronisk
- Kamera
- Lyssensor med farvefilter
Hvad er fordele og ulemper ved at bruge disse? – gerne holdt op mod hinanden.
Hvor mange farver skal du kunne detektere?
Hvor stor må usikkerheden være – hvor rød er en rød.
Hvad skal man bruge for at kunne bestemme, hvor de forskellige farver ryger hen
- Motor
  - Servo
  - DC-motor
  - Stepper Motor
Hvad er fordele og ulemper ved at bruge disse? – gerne holdt op mod hinanden.
Hvor hurtigt skal maskinen køre?
Hvor præcis skal den være?

Tidsforbrug:

Hvis man kun følger instruktionerne som en tutorial: 4-6 timer

Hvis man samtidig underviser i de delkomponenter, der udgør det samlede projekt: 2-3 dage á 6 timer.

Materialer:

1x Batteriboks til 6 stk AA batterier – link.
1x Arduino UNO – link.
1x Arduino Motor Shield – link.
1x Arduino Prototype Shield – link.
1x 9V Batteriklemme – link.
1x USB Kabel (A-B)
Diverse ledning (stiv) – link.
Pin header til montage på ledning – link.
2 x Kontakter link.
RGB Sensor (TCS34725) – kan købes hos Adafruit – link.
2 stk 10kOhm modstande – link.
LEGO: 2 Stk EV3 Motorer + Diverse Legoklodser (Se liste sammen med byggevejledning)
NXT/EV3 Kabel

Værktøj:

Skruetrækker til Arduino Motor Shield
Skævbider
Afisoleringstang
Loddekolbe

Overordnet funktionsmåde for farvesorteringsmaskine

Nedenstående figur 1 viser 2 forskellige funktionsmåder for en farvesorteringsmaskine. De usorterede elementer lægges på transportbåndet. Over transportbåndet sidder en farvesensor, der kan genkende elementernes farve med en eller anden sikkerhed. Jo flere forskellige farver man vil have sensoren til at genkende, jo bedre skal sensor og lysforhold være. I vores maskine vil vi gerne kunne genkende 3 forskellige farver, der er så forskellige, at vi selv kan kende forskel. Når et elements farve er blevet genkendt på transportbåndet indstilles farvesorteringsslæden eller farvesorteringshjulet, sådan at den rigtige farvesorteringskasse kommer til at stå ud for enden af transportbåndet, når elementet når til enden af transportbåndet. Således lander elementerne med en bestemt farve i en bestemt kasse.

Figur 1.

Den model vi viser et eksempel på i denne vejledning er den første med farvesorteringsslæden.

Herunder ses et billede af den færdige farvesorteringsmaskine på figur 2. De fleste af de ting der skal bygges er dokumenteret herunder, men der vil også være steder, hvor man selv skal finde på løsninger, eller f.eks. bygge ekstra Lego-klodser på modellen for at få den optimeret, så alting virker.

Figur 2.

Beskrivelse af elektronisk kredsløb

I dette afsnit beskrives de komponenter der udgør det elektroniske kredsløb i vores maskine, og nederst vises hvordan komponenterne skal sættes sammen for at maskinen kan virke.

Arduino UNO

Figur 3.

Arduino UNO’en på figur 3 er vores computer. Den består af en programmerbar microcontroller kreds fra firmaet Microchip. En microcontroller er basalt set en hel computer, der er lagt ind i en chip. D.v.s. Den har både en CPU, noget hukommelse og en disk, samt forskellige typer porte man kan skrive til og læse fra. Du kan læse og lære meget mere om Arduino UNO på https://www.arduino.cc/ – og i særdeleshed på https://www.arduino.cc/en/Guide/ArduinoUno.

For at kunne programmere din Arduino skal du bruge en PC og hente softwaren til Arduino på https://www.arduino.cc/en/Main/Software. På samme side er der også online værktøjer der kan bruges til at arbejde med Arduino.

Arduino Motor Shield

Figur 4.

Arduino Motor shield på figur 4 er et udvidelsesprint til Arduino, der gør at man kan styre elektriske motorer. Det får vi brug for, når vi skal styre vores LEGO EV3 motorer. Grunden til, at det er nødvendigt med et ekstra print for at styre motorer, er at motorer ofte bruger højere spænding og mere strøm end der kan leveres direkte fra mikrocontrolleren. Derfor har Motor Shield’et muligheden for at tilføje en anden spændingeskilde via skrueterminalerne i nederste venstre hjørne (Vin og GND) og den store IC, der sidder midt på printet sørger for, at de signaler, der sendes fra Microcontrolleren på Arduino Uno bliver forstærket, så de er stærke nok til at kunne få en motor til at dreje rundt. Derudover bruges kredsen også til at vende spændingen, så man med kontrolsignaler fra Arduino Uno’en kan få motoren til at dreje i begge retninger.

Motor Shield’et kan styre 2 jævnspændings-motorer (også kaldet DC motorer) via de 4 andre skrueterminaler i venstre side af Figur 4. De nederste 2 er motor A + og -, og de øverste 2 er motor B + og -.

Når Motor Shield’et skal bruges bygger man det sammen med Arduino Uno ved at sætte det oven på, således det passer ned i de stik, der er rundt i kanten på Arduino Uno.

Du kan læse mere om Motor Shield’et her: https://www.arduino.cc/en/Main/ArduinoMotorShieldR3?setlang=it.

Når vi skal styre Motor Shield’et fra vores Arduino skal vi bruge følgende porte på Microcontrolleren til at styre hhv. Retning (Direction) og Hastighed (PWM). Bremse (Brake) og Strømmåling (Current Sensing) bruger vi ikke i dette projekt.

Figur 5.

Protoshield

Figur 6.

Protoshieldet i figur 6 er det øverste shield i vore Arduino stak, og det indeholder et breadboard, som vi kan bruge til at sætte vores egne komponenter fast i, så vi undgår at skulle lodde.

I vores udgave af sorteringsmaskinen har vi lavet vores eget protoshield, men man kan også købe et – f.eks. her.

Beskrivelse af sensorer

Der skal bruges 2 typer sensorer til dette projekt. Det ene er farvesensoren, der kan detektere et bestemt antal farver. Det andet er knapper, der bruges til at bestemme endepositionerne af farvesorteringsslæden.

Farvesensor

Farvesensoren vi bruger i dette projekt er TCS34725 RGB Sensoren fra Adafruit. Sensoren måler både RGB (rødt, grønt, blåt) og klart lys (C). Den blokerer infrarødt lys, så det ikke påvirker målingen og derudover har den en lysdiode monteret, der oplyser det element, der skal måles på, for at få den mest nøjagtige farvemåling.

Farvesensoren bruger I2C seriel kommunikation til at kommunikere med Arduinoen, og skal derfor sluttes til SCL og SDA, hvor SCL er den serielle forbindelses clock-signal og SDA er datasignalet – altså det signal der indeholder RGB-værdierne fra sensoren.

Figur 7.

Til dette projekt skal 4 af farvesensorens terminaler forbindes til vores arduino – se figur 7. Udover SCL og SDA, der forbindes til SCL og SDA på Motorshieldet, som vist på figur 11.

Kontakter

Til at bestemme farvesorteringsslædens endepunkter benyttes 2 såkaldte micro-switches, der er kontakter der som udgangspunkt er afbrudt, og som sluttes så længe de trykkes ind.

Vi har valgt at bruge den microswitch, som ses på figur 8, fordi den med sin lange arm er let at trykke ind. Det betyder at motoren ikke skal bruge så mange kræfter på at aktivere kontakten, og at vi så er sikre på at kontakten aktiveres hver gang.

For at forbinde kontakten til resten af kredsløbet skal der loddes ledninger på 2 af kontaktens terminaler. Vi bruger de to terminaler, der er tættest på hvor armen sidder fast på kontakten – altså længst til venstre på figur 8. De 2 terminaler er også benævnt C og ON på kontakten.

Husk at lave ledningerne mindst 30cm lange, så de kan nå hele vejen over til arduinoen.

Figur 8.

I kredsløbet sættes kontakterne i serie med hver deres modstand på 10KOhm på følgende måde:

Figur 9.

Vin er vores forsyningsspænding på 5V. Switchen er vores kontakt, og Pulldown Resistor er vores modstand på 10KOhm. Ground er vores stel, som også er benævnt GND på Arduino.

Det midterste punkt mellem kontakten og modstanden i kredsløbet forbindes til et digitalt input på Arduino’en, som så repræsenterer den Logic Gate, der er vist i figur 9. Grunden til at man laver dette kredsløb, er at vi gerne vil have at der er 5V på vores Logic Gate, når kontakten er sluttet, og 0V, når den er brudt. Det kan vi kun få ved at have modstanden i kredsløbet. Hvis der ikke var nogen modstand ville vi lave en kortslutning mellem 5V og GND, når vi slutter kontakten.

Ledninger

Til at forbinde EV3 motorerne med vores Arduino motorshield har vi i dette projekt valgt at klippe et langt mindstormskabel midt over, så vi får 2 kabler der kan forbindes til vores breadboard på protoshield’et.

Kablerne er lavet som vist på figur 10.

Figur 10.

De stik, der er loddet på i de afklippede ender er pinrækker, der kan knækkes af i længder på 6 stifter, hvor man så lodder hver af de 6 ledninger i kablet fast på hver sin stift i viste rækkefølge/ledningsfarve. For at sikre lodningen mod træk i kablet har vi valgt at bruge varmelim til at isolere og forstærke den øverste del af stikket, som også vist på figur 10.

De resterende ledninger, der bruges i kredsløbet er almindelige stive monteringsledninger som vist herunder. For at de passer i breadboard skal de typisk afisoleres, så 0.5 cm leder bliver fritlagt.

Overordnet elektronisk diagram over farvesorteringsmaskinen.

Herunder ses hvordan de ovenstående komponenter er sat sammen for at få det elektroniske kredsløb til at virke, så man efterfølgende kan bruge det til at styre sorteringsmaskinen.

Det er vigtigt at lægge mærke til at det drejer sig om 3 printkort, der skal sættes ovenpå hinanden med Arduino Uno i bunden, Motorshield’et i midten og protoshield’et øverst.

Figur 11.

Man kan se vores opbygning af det elektroniske kredsløb på de følgende billeder:

Figur 12.

Figur 13.

Figur 14.

Figur 15.

Figur 16.

Lego Model

Først skal legomodellen bygges. Den version vi har fundet på består af 2 hoveddele. Det ene er et transportbånd, hvor de elementer, der skal farvesorteres lægges på og transporteres hen til de kasser, hvor farvesorteringen foregår. Den anden del er farvesorteringsslæden, der kører frem og tilbage for enden af transportbåndet for at få de rigtige farvede elementer i de rigtige kasser.

Byggevejledning som pdf.

Man er meget velkommen til selv at finde på, hvis man ikke lige har adgang til de samme klodser, som vi har brugt. De fleste af klodserne burde dog kunne findes i et standard LEGO Mindstorms EV3 eller NXT sæt.

Den byggevejledning vi har lavet viser ikke, hvordan båndet er monteret på byggepladen og hvordan kontakterne er sat fast, men det kan man se på følgende figurer 17 – 20.

Figur 17.

Figur 18.

Figur 19.

Figur 20.

Program

Når både Elektronisk kredsløb og legomodel er bygget, skal der laves et program der styrer maskinen. Overordnet kan programmets funktionsmåde beskrives på følgende måde.

Opstart

Find den tid det tager motor A at bevæge farvesorteringsslæden fra kontakt 1 til kontakt 2.
Stil farvesorteringsslæden i midten ved at køre den halvdelen af tiden fra den ene kontakt.
Start båndet

Gentag

Læs farvesensorens RGB-værdi
Hvis RGB-farveværdierne ligger inden for en af de ranges vi har bestemt
- Stop båndet
- Kør farvesorteringsslæden til den pågældende farve står ud for båndet
- Start båndet
Hvis ikke
- Stop båndet indtil nogen har fjernet emnet (farven ændrer sig til båndets farve)
- Start båndet

Ovenstående kalder man også for pseudokode, og det er en god måde at få bestemt den overordnede funktionalitet i sit program inden man begynder at skrive programmet i Arduino IDE’en

Inden vi laver hele vores program er det en god ide først at checke at farvesensoren virker og få fundet ud af, hvilke objekter der skal farvesorteres og hvilke farve-ranges de har. Til vores projekt bruger vi små 3D-printede plastikcylindre, men maskinen fungerer også fint med f.eks. m&ms, legoklodser eller andet. Når bare afstanden mellem farvesensoren og elementerne ikke er for stor eller for lille. Det virker som regel bedst, når afstanden mellem sensor og objekt er mellem 1 og 5 mm.

Først og fremmest skal man for at få farvesensoren fra Adafruit til at virke sammen med Arduino IDE benytte denne guide.

Dernæst prøv at skrive følgende program:

#include <Wire.h>
#include "Adafruit_TCS34725.h"

Adafruit_TCS34725 tcs = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_50MS, TCS34725_GAIN_4X);

int farve = 0; // Variabel til at holde den nuværende læste farve


//Variable til farvesensorer
uint16_t c, red, green, blue;
uint32_t sum;
float r, g, b;

int motor_A_Retning = 12; //Ben 12 bruges til at styre motor A's retning
int motor_A_Hastighed = 3; //Ben 3 bruges til at styre motor A's hastighed
int motor_B_Retning = 13; //Ben 12 bruges til at styre motor B's retning
int motor_B_Hastighed = 11; //Ben 3 bruges til at styre motor B's hastighed

void setup() {

  Serial.begin(9600);
  Serial.println("Color View Test!");

  if (tcs.begin()) {
    Serial.println("Found sensor");
  } else {
    Serial.println("No TCS34725 found ... check your connections");
    while (1); // halt!
  }

  // Opsætning af motorpins 
  pinMode(motor_A_Retning, OUTPUT);
  pinMode(motor_A_Hastighed, OUTPUT);
  pinMode(motor_B_Retning, OUTPUT);
  pinMode(motor_B_Hastighed, OUTPUT);

  // Sørg for at motorerne ikke kører
  digitalWrite(motor_A_Retning, HIGH); 
  analogWrite(motor_A_Hastighed, 0);
  digitalWrite(motor_B_Retning, HIGH); 
  analogWrite(motor_B_Hastighed, 0);
  
}

void loop() {
  
      // Læs Farvesensor
      
      tcs.setInterrupt(false);      // turn on LED

      delay(60);  // takes 50ms to read 
  
      tcs.getRawData(&red, &green, &blue, &c);

      tcs.setInterrupt(true);  // turn off LED
      //Færdig med at læse farvesensor

      // Omregn farveværdier til 8-bit (0-255) RGB-værdier
      sum = c;
     
      r = red; r /= sum;
      g = green; g /= sum;
      b = blue; b /= sum;
      r *= 256; g *= 256; b *= 256;

      red = int(r);
      green = int(g);
      blue = int(b);

      // Færdig med omregning

      Serial.print("\tR:\t"); Serial.print(int(r));
      Serial.print("\tG:\t"); Serial.print(int(g));
      Serial.print("\tB:\t"); Serial.print(int(b));
      Serial.println();

      delay(500);
}

Programmet benytter det bibliotek til Adafruit som vi lige har installeret, og det første vi gør i programmet er at inkludere det, så vi kan bruge funktionerne fra det.

Derefter sætter vi nogle variable til at holde styr på de værdier, der læses fra farvesensoren. Og da vi også har motorshield’et med i vores elektroniske kredsløb er vi nødt til at sætte motorerne til at stå stille – så derfor er der også variable til at holde motorernes porte.

I setup() sætter vi den Serielle port op til at kommunikere med PC’en, så vi kan holde øje med, hvad der sker i Arduino IDE’ets serial monitor.

Hvis der kan registreres forbindelse til farvesensoren står der “Color View Test!” og “Found sensor” som det første, når man åbner Serial Monitoren. Hvis farvesensoren ikke findes skriver den “No TCS34725 found … check your connections”. Og så skal man gå sine forbindelser efter til farvesensoren og checke at den er forbundet rigtigt.

I loop() læses farvesensoren med noget standardkode fra Adafruit. Tcs-biblioteket indeholder de funktioner der skal bruges til at læse fra sensoren, og det er derfor at kommandoerne f.eks. hedder tcs.getRawData(&red, &green, &blue, &c);.

I den følgende kode regnes de værdier der læses fra farvesensoren om til 8-bit (0-255) RGB værdier. Værdien c siger noget om den overordnede reflekterede lysstyrke, og er den værdi vi bruger til at detektere om det er båndet vi ser eller en farvet genstand på båndet.

sum = c;
     
r = red; r /= sum;
g = green; g /= sum;
b = blue; b /= sum;
r *= 256; g *= 256; b *= 256;

red = int(r);
green = int(g);
blue = int(b);

Til sidst printes r, g og b – værdierne ud, så vi kan aflæse dem i serial monitoren.

Der vil altid være usikkerheder på farveaflæsningerne fra sensoren, fordi elementerne ikke altid har samme farve over hele fladen eller fordi elementerne ligger lidt forskelligt og har forskellig afstand til farvesensoren, når de kører ind under den. Derfor er det en god ide ikke kun at reagere på en bestemt værdi for r,g og b, når man skal detektere farve, men i stedet reagere på en range af hhv. r, g og b.

Derfor kan det være en god ide f.eks. at tage 10 objekter af samme farve og holde dem under sensoren og notere RGB værdierne for alle 10 og derefter finde minimum og maksimum værdier for R, G og B. for en given farve. Dette skal man så gøre for alle de 3 farver man vil have sin maskine til at sortere.

I vores tilfælde har vi f.eks. fundet frem til følgende min- og max-værdier for hhv gul, grøn og rød for de 3D-printede platikpastiller vi bruger.

Gul: R[101, 111], G[103,111], B[42, 47]

Rød: R[171, 191], G[44, 57], B[42, 53]

Grøn: R[53, 62], G[113, 118], B[91, 94]

Programmet til sorteringsmaskinen ser således ud:

#include <Wire.h>
#include "Adafruit_TCS34725.h"

Adafruit_TCS34725 tcs = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_50MS, TCS34725_GAIN_4X);

//Opsætning af variable
int motor_A_Retning = 12; //Ben 12 bruges til at styre motor A's retning
int motor_A_Hastighed = 3; //Ben 3 bruges til at styre motor A's hastighed
int motor_B_Retning = 13; //Ben 12 bruges til at styre motor B's retning
int motor_B_Hastighed = 11; //Ben 3 bruges til at styre motor B's hastighed
int tryk_Knap_1 = 6; //Ben hvor kontakten sidder
int tryk_Knap_2 = 7; //Ben hvor kontakten sidder
int farve = 0; // Variabel til at holde den nuværende læste farve
int tilstand = 0; //Variabel til at holde den nuværende tilstand


//Variable til farvesensorer
uint16_t c, red, green, blue;
uint32_t sum;
float r, g, b;

//Tidsvariable
unsigned long starttid, sluttid, koeretid;

//En enum (enumerator - nummerering), der definerer de farver vi er interesserede i. En enum tildeler i virkeligheden blot en konstant værdi til et variabelnavn.
enum farver {
  SORT,  //0
  GROEN, //1
  ROED,  //2
  GUL   //3
};

//En enum, der definerer de tilstande robotten kan befinde sig i.
enum tilstande {
  FIND_MM,             //0
  STIL_KASSER_RIGTIGT, //1
  AFLEVER_MM,          //2
  STIL_KASSER_MIDTEN   //3
};


void setup() {
// Herunder setup skal vi først fortælle vores Arduino hvilke
// ben den skal bruges til at styre motor A og B, samt knapper og farvesensor

  pinMode(motor_A_Retning, OUTPUT);
  pinMode(motor_A_Hastighed, OUTPUT);
  pinMode(motor_B_Retning, OUTPUT);
  pinMode(motor_B_Hastighed, OUTPUT);
  pinMode(tryk_Knap_1, INPUT);
  pinMode(tryk_Knap_2, INPUT);

  Serial.begin(9600);
  Serial.println("Color View Test!");

  if (tcs.begin()) {
    Serial.println("Found sensor");
  } else {
    Serial.println("No TCS34725 found ... check your connections");
    while (1); // halt!
  }

  
  // Kalibrering ----------------------------------------------------
  
  digitalWrite(motor_A_Retning, HIGH); //Kør mod venstre - skal muligvis ændres til LOW afhængigt af gearing m.m.
  analogWrite(motor_A_Hastighed, 220); //Fuld hastighed - husk gearing er nødvendigt, hvis det skal blive nogenlunde nøjagtigt.
  while(digitalRead(tryk_Knap_1) == LOW){ //Så længe kontakten ikke bliver trykket ind
    //Gør ingenting 
  }
  analogWrite(motor_A_Hastighed, 0); //Stop Motor A, når kontakten er trykket ind
  digitalWrite(motor_A_Retning, LOW); //Kør mod højre - skal muligvis ændres til HIGH afhængigt af gearing m.m.
  analogWrite(motor_A_Hastighed, 220); //Fuld hastighed - husk gearing er nødvendigt, hvis det skal blive nogenlunde nøjagtigt.
  starttid = millis(); //start tidtagningen
  while(digitalRead(tryk_Knap_2) == LOW){ //Så længe kontakten ikke bliver trykket ind
    //Gør ingenting 
  }
  sluttid = millis(); //stop tidtagningen
  analogWrite(motor_A_Hastighed, 0); //Stop Motor A, når kontakten er trykket ind
  koeretid = sluttid - starttid; //Tidtagningen for kørslen mellem kontakt 1 og 2 er stoptiden - sluttiden

  //Hvordan får vi så motoren til at køre ud og stille sig midt mellem kontakt 1 og 2?
  digitalWrite(motor_A_Retning, HIGH); //Kør mod venstre - skal muligvis ændres til LOW afhængigt af gearing m.m.
  analogWrite(motor_A_Hastighed, 220);
  delay(koeretid/2); //Kør i hlvdelen af tiden, det tog at køre fra kontakt 1 til kontakt 2
  analogWrite(motor_A_Hastighed, 0); //Stop motoren

  tilstand = FIND_MM; //Så er vi klar til at finde M&M's med farvesensoren

}

void loop() {
  switch(tilstand){
    case FIND_MM:
      // Læs Farvesensor
      
      tcs.setInterrupt(false);      // turn on LED

      delay(60);  // takes 50ms to read 
  
      tcs.getRawData(&red, &green, &blue, &c);

      tcs.setInterrupt(true);  // turn off LED
      //Færdig med at læse farvesensor

      // Omregn farveværdier til 8-bit (0-255) RGB-værdier
      sum = c;
     
      r = red; r /= sum;
      g = green; g /= sum;
      b = blue; b /= sum;
      r *= 256; g *= 256; b *= 256;

      red = int(r);
      green = int(g);
      blue = int(b);

      // Færdig med omregning

      if(c < 800){
        digitalWrite(motor_B_Retning, HIGH); //Kør mod venstre - skal muligvis ændres til LOW afhængigt af gearing m.m.
        analogWrite(motor_B_Hastighed, 180);
        farve = SORT;
        Serial.println("Transportbaand");
      } else {
        analogWrite(motor_B_Hastighed, 0);
        
        Serial.print("\tR:\t"); Serial.print(int(r));
        Serial.print("\tG:\t"); Serial.print(int(g));
        Serial.print("\tB:\t"); Serial.print(int(b));
        Serial.println();
        
    
        if((red >= 171 && red <= 192) && (green >= 44 && green <= 57) && (blue >= 42 && blue <= 53)) {
          farve = ROED;
          Serial.println("Rød");
          tilstand = STIL_KASSER_RIGTIGT;
        } else if((red >= 101 && red <= 111) && (green >= 103 && green <= 111) && (blue >= 42 && blue <= 47)) {
          farve = GUL;
          Serial.println("Gul");
          tilstand = STIL_KASSER_RIGTIGT;
        } else if((red >= 53 && red <= 62) && (green >= 113 && green <= 118) && (blue >= 91 && blue <= 94)) {
          farve = GROEN;   
          Serial.println("Grøn");
          tilstand = STIL_KASSER_RIGTIGT;
        } else {
          Serial.println("Farve ikke genkendt");
        } 
      }
      break;
    case STIL_KASSER_RIGTIGT:
      switch(farve) {
        case ROED:
          //Bliv Stående
          tilstand = AFLEVER_MM;
          break;
        case GUL:
          // 
          digitalWrite(motor_A_Retning, LOW); //Kør mod venstre - skal muligvis ændres til LOW afhængigt af gearing m.m.
          analogWrite(motor_A_Hastighed, 220); //Fuld hastighed - husk gearing er nødvendigt, hvis det skal blive nogenlunde nøjagtigt.
          while(digitalRead(tryk_Knap_2) == LOW){ //Så længe kontakten ikke bliver trykket ind
            //Gør ingenting 
          }
          analogWrite(motor_A_Hastighed, 0); //Stop Motor A, når kontakten er trykket ind
          tilstand = AFLEVER_MM;
          break;
        case GROEN:
          digitalWrite(motor_A_Retning, HIGH); //Kør mod venstre - skal muligvis ændres til LOW afhængigt af gearing m.m.
          analogWrite(motor_A_Hastighed, 220); //Fuld hastighed - husk gearing er nødvendigt, hvis det skal blive nogenlunde nøjagtigt.
          while(digitalRead(tryk_Knap_1) == LOW){ //Så længe kontakten ikke bliver trykket ind
            //Gør ingenting 
          }
          analogWrite(motor_A_Hastighed, 0); //Stop Motor A, når kontakten er trykket ind
          tilstand = AFLEVER_MM;
          break;
      }
      break;
    case AFLEVER_MM:
      analogWrite(motor_B_Hastighed, 180);
      delay(2000);
      analogWrite(motor_B_Hastighed, 0);
      tilstand = STIL_KASSER_MIDTEN;
      break;
    case STIL_KASSER_MIDTEN:
      switch(farve) {
        case ROED:
          //Bliv Stående
          tilstand = FIND_MM;
          break;
        case GUL:
          // 
          digitalWrite(motor_A_Retning, HIGH); //Kør mod venstre - skal muligvis ændres til LOW afhængigt af gearing m.m.
          analogWrite(motor_A_Hastighed, 220); //Fuld hastighed - husk gearing er nødvendigt, hvis det skal blive nogenlunde nøjagtigt.
          delay(koeretid/2);
          analogWrite(motor_A_Hastighed, 0); //Stop Motor A, når kontakten er trykket ind
          tilstand = FIND_MM;
          break;
        case GROEN:
          digitalWrite(motor_A_Retning, LOW); //Kør mod venstre - skal muligvis ændres til LOW afhængigt af gearing m.m.
          analogWrite(motor_A_Hastighed, 220); //Fuld hastighed - husk gearing er nødvendigt, hvis det skal blive nogenlunde nøjagtigt.
          delay(koeretid/2);
          analogWrite(motor_A_Hastighed, 0); //Stop Motor A, når kontakten er trykket ind
          tilstand = FIND_MM;
          break;
      } 
      break;
  }
}

Al koden til at læse sensoren og styre motorerne er den samme som vi brugte i koden til at læse farvesensoren. Den vigtigste ændring i ovenstående program er dér hvor vi finder centerpositionen for vores farvesorteringsslæde.

// Kalibrering ----------------------------------------------------
  
  digitalWrite(motor_A_Retning, HIGH); //Kør mod venstre - skal muligvis ændres til LOW afhængigt af gearing m.m.
  analogWrite(motor_A_Hastighed, 220); //Fuld hastighed - husk gearing er nødvendigt, hvis det skal blive nogenlunde nøjagtigt.
  while(digitalRead(tryk_Knap_1) == LOW){ //Så længe kontakten ikke bliver trykket ind
    //Gør ingenting 
  }
  analogWrite(motor_A_Hastighed, 0); //Stop Motor A, når kontakten er trykket ind
  digitalWrite(motor_A_Retning, LOW); //Kør mod højre - skal muligvis ændres til HIGH afhængigt af gearing m.m.
  analogWrite(motor_A_Hastighed, 220); //Fuld hastighed - husk gearing er nødvendigt, hvis det skal blive nogenlunde nøjagtigt.
  starttid = millis(); //start tidtagningen
  while(digitalRead(tryk_Knap_2) == LOW){ //Så længe kontakten ikke bliver trykket ind
    //Gør ingenting 
  }
  sluttid = millis(); //stop tidtagningen
  analogWrite(motor_A_Hastighed, 0); //Stop Motor A, når kontakten er trykket ind
  koeretid = sluttid - starttid; //Tidtagningen for kørslen mellem kontakt 1 og 2 er stoptiden - sluttiden

  //Hvordan får vi så motoren til at køre ud og stille sig midt mellem kontakt 1 og 2?
  digitalWrite(motor_A_Retning, HIGH); //Kør mod venstre - skal muligvis ændres til LOW afhængigt af gearing m.m.
  analogWrite(motor_A_Hastighed, 220);
  delay(koeretid/2); //Kør i hlvdelen af tiden, det tog at køre fra kontakt 1 til kontakt 2
  analogWrite(motor_A_Hastighed, 0); //Stop motoren

  tilstand = FIND_MM; //Så er vi klar til at finde M&M's med farvesensoren

Her bevæger vi først vores motor mod venstre indtil knappen aktiveres. Så starter vi tidtagning med kommandoen, starttid = millis(); //start tidtagningen.

Derefter bevæger vi motoren mod højre indtil den højre knap aktiveres og vi læser sluttiden med kommandoen, sluttid = millis(); //stop tidtagningen.

Næste skridt er så at beregne hvor lang tid slæden skal køre for at nå til centerpositionen. Først beregnes den samlede køretid mellem kontakt 1 og 2 i millisekunder: koeretid = sluttid - starttid; //Tidtagningen for kørslen mellem kontakt 1 og 2 er stoptiden - sluttiden

Nu kan vi sætte slæden til at køre til centerpositionen midt mellem kontakterne.

//Hvordan får vi så motoren til at køre ud og stille sig midt mellem kontakt 1 og 2?
  digitalWrite(motor_A_Retning, HIGH); //Kør mod venstre - skal muligvis ændres til LOW afhængigt af gearing m.m.
  analogWrite(motor_A_Hastighed, 220);
  delay(koeretid/2); //Kør i halvdelen af tiden, det tog at køre fra kontakt 1 til kontakt 2
  analogWrite(motor_A_Hastighed, 0); //Stop motoren

For at kunne reagere på de farvede elementer har vi valgt at dele vores program op i nogle tilstande i starten af programmet. Dette gøres med en såkaldt, enum,

//En enum, der definerer de tilstande robotten kan befinde sig i.
enum tilstande {
  FIND_MM,             //0
  STIL_KASSER_RIGTIGT, //1
  AFLEVER_MM,          //2
  STIL_KASSER_MIDTEN   //3
};

Tilstanden FIND_MM er dér hvor sensoren kigger efter kendte farver på båndet

Tilstanden STIL_KASSER_RIGTIGT er dér hvor vi flytter farvesorteringsslæden så den rigtige kasse står ud foran båndet

Tilstanden AFLEVER_MM er dér hvor båndet kører frem indtil det farvede element er læsset ned i den rigtige kasse

Tilstanden STIL_KASSER_MIDTEN er dér hvor vi flytter farvesorteringsslæden tilbage til centerpositionen.

Ved at benytte tilstande kan man som regel lettere navigere rundt i ens kode og bliver lettere at overskue, hvad der skal ske, når en bestemt tilstand er overstået.

Til at bestemme hvilken tilstand der udføres ved hvert gennemløb af loop() bruger vi vores enum sammen med en case-switch struktur.

I loop() læses først farvesensoren ligesom i det tidligere program. Når man så har en red, green og blue værdi samt en c-værdi kigger vi først på om c < 800. Det betyder at vi kun ser transportbåndet. Farven sættes så til SORT. Hvis vi ser en anden farve end sort undersøger vi om det er rød, gul eller grøn farve:

if((red >= 171 && red <= 192) && (green >= 44 && green <= 57) && (blue >= 42 && blue <= 53)) {
  farve = ROED;
  Serial.println("Rød");
  tilstand = STIL_KASSER_RIGTIGT;
} else if((red >= 101 && red <= 111) && (green >= 103 && green <= 111) && (blue >= 42 && blue <= 47)) {
  farve = GUL;
  Serial.println("Gul");
  tilstand = STIL_KASSER_RIGTIGT;
} else if((red >= 53 && red <= 62) && (green >= 113 && green <= 118) && (blue >= 91 && blue <= 94)) {
  farve = GROEN;   
  Serial.println("Grøn");
  tilstand = STIL_KASSER_RIGTIGT;
} else {
  Serial.println("Farve ikke genkendt");
}

Det gør vi med 3 if / else if sætninger, hvor vi i hver checker op mod de minimum og maximum værdier vi har fundet for hhv. R, G og B for de tre farver.

Hvis vi finder en af vores tre foruddefinerede farver skifter vi tilstand til STIL_KASSER_RIGTIGT.

I tilstanden STIL_KASSER_RIGTIGT bruger vi den farve der er fundet til at bestemme hvor farvesorteringsslæden skal køre hen og skifter derefter til tilstanden AFLEVER_MM.

I tilstanden AFLEVER_MM kører transportbåndet fremad i 2 sekunder indtil elementet er afleveret i den rigtige kasse i slæden. Derefter skifter den til tilstanden STIL_KASSER_MIDTEN, hvor man ud fra farven kan bestemme om farvesorteringsslæden skal køre til højre, venstre eller blive stående.

Til sidst

Der er mange muligheder for at lave både program og konstruktion anderledes, og det kan sagtens være at man bliver nødt til at lave flere af de ovenstående ting om for at få maskinen til at virke. Vi håber dog at have beskrevet tingene i nok detaljer til at den opgave ikke er helt umulig. Prøv f.eks. at bygge maskinen om til at kunne sortere 4 forskellige farver. Hvordan ændrer det på både konstruktion og program?

Lego og Arduino Sorteringsmaskine – V2

Jacob Nielsen