EAL - Den Fuldautomatiske Farvesorterings Robotarm

Dette projekt er udarbejdet af Rasmus Nielsen og Rikke Ejlersen på Erhvervsakademiet Lillebælt 2. semester automationsteknolog.

Denne fuldautomatiske robotarm er i stand til at registrere indkommende emner, som den samler op og sortere ud til 4 forskellige punkter på platformen. Ideen er at ved hjælp af en infrarød sensor, vil robotarmen registrere at der er et emne klar til afhentning og med en elektromagnet samle emnet op. Herfra vil robotarmen bringe emnet over til en farvesensor, som vil registrere hvilke farve emnet har og ud fra farven vil den ligge emnet i den tilhørende boks.

Step 1: Materialer

Til dette projekt har vi brugt følgende materialer

Arduino Mega 2560

Relæ modul

Infrarød sensor modul

Farve sensor modul

Spændeskiver

Isoleringstape

Tændstikæsker

Robot arm

3 servo motorer

Elektromagnet

5 V forsyningskilde

Step 2: Beskrivelse Af Sensorene

TCS230 farvesensor modul

På det første billede ses farvesensoren. Den fungere ved at de 4 dioder udsender lys og i mellem dioderne sidder en sensor der registre når det lys der reflekteres. Ved hjælp af forskellige farvefiltre i sensoren kan den registrere hvor meget lys der kommer af rød, grøn og blå. Modulet sender en frekvens svarende til styrken af lyset. Den kan i arduino laves om til tal der kan bruges til at aflæse farven.

Indfrarød sensor modul

På det andet billede ses infrarød sensoren. Den fungerer ved at sensoren sender et lys ud, når lyset rammes af et objekt vil signalet reflekteres tilbage til sensoren og blive modtaget og sende signalet til robotten om at der er emne klar til afhentning.

Step 3: Ledningsdiagram

Det første billede viser ledningsdiagram af vores opstilling, hvor man kan se hvordan de forskellige komponenter er forbundet til Arduino Mega'en. Ved siden af ses printpladen, som vi har lavet for at samle alle ledningerne til de 3 servo motorer og elektromagneten. Printpladen har fået sin egen forsyningskilde, for ikke at belaste arduinoen. På printpladen er der fastspændt et et relæ, som vi kan slå til og fra via arduinoen og på den måde styre elektromagneten. De næste billeder viser hvilke ben der anvendes på arduinoen, IR-sensoren og farvesensoren.

Step 4: Step 3: Emner Til Løft

Der blev lavet test med forskellige materialer til emnet som robotarmen skulle samle op.
Den første ide var at emnerne skulle være små metal kugler fra kuglelejer. Denne ide blev hurtigt kasseret, efter som elektromagneten ikke kunne få fat på kuglerne. Herfra blev der gjort forsøg med små og store møtrikker. De små blev droppede fordi de ikke var tunge nok i forhold til elektromagneten - de faldt ikke af ved endestationen. De store viste sig at være alt for tunge for elektromagneten, så ved bevægelse af robotarmen, faldt emnet af inden endestationen. Billedet foroven viser den endelig løsning på emner som robotarmen skulle flytte - flade spændeskiver. Der var problemer med at de heller ikke var tunge nok til at elektromagneten ville slippe dem. Det virkede heldigvis da der blev sat isoleringstape på dem, som blev løsningen for at give emnerne forskellige farver.

Step 5: Styring

For at gøre det nemmere at forstå hvordan robotten skal bevæge sig, er der lavet et flowdiagram. Efter at den starter op vil den gå til hjemmepositionen og vente på at emne bliver lagt foran IR-sensoren. Herefter vil robotten flytte ned til emnet og samle den op med elektromagneten. Robotten vil flytte emnet henover farvesensoren. Sensoren aflæser farven og robotten flytter emnet til den tilhørende æske og slukker magneten så emnet falder i æsken. Robotten vil flytte til hjem positionen og afvente et nyt emne.

Step 6: Kode

Koden er vedhæftet forneden af step 6.

Da der ikke var plads til kommentarer i højre side i intructable, er de placeret foroven i koden herunder.

Der er i projektet brugt et servo bibliotek til at styre servoerne. Det gør at der kan skrives en vinkel, fra 0 til 180, til hver servo, hvorefter den flytter sig til den position. Vi har lavet en funktion der hedder moveArm(), der gør at vi kan skrive til alle servoer på en gang.

Der er også en funktion til at aflæse farven. Den returnere ud fra nogle parametre om farven, at tal mellem 1 og 4. Det tal bruges i en switch case til at bestemme hvilken kasse emnet skal lægges i. Der er desuden en funktion der flytter emnet til den rigtige kasse. Sekvensen er den samme for alle kasserne, det eneste der er forskelligt er den vinkel der gives til servoerne.

#include <Servo.h>                      //Tilføjer servo bibliotek

// Funktions prototyper
void moveArm(int x, int y, int z);
void moveToBox(int a, int b, int c, int d, int e, int f);
int readColor();</servo.h>

// Servopositioner for de 3 servo motorer.
// Servo 1 er længst til venstre, 2 er midt og 3 er højre servo.
// Hjem position
int home1 = 170; int home2 = 7; int home3 = 30;
// Hent emne position
int pickUp1 = 150; int pickUp2 = 7; int pickUp3 = 45;           
// Position for arm løftet med farvesensor
int colorUp1 = 150; int colorUp2 = 7; int colorUp3 = 10;
// Position for arm sænket med farvesensor
int colorDown1 = 140; int colorDown2 = 7; int colorDown3 = 20;   

// Servopositioner for de fire æsker
int pos11Up = 160; int pos12Up = 80; int pos13Up = 20;
int pos11Down = 150; int pos12Down = 80; int pos13Down = 30;

int pos21Up = 155; int pos22Up = 63; int pos23Up = 15;
int pos21Down = 150; int pos22Down = 63; int pos23Down = 25;

int pos31Up = 180; int pos32Up = 80; int pos33Up = 30;
int pos31Down = 170; int pos32Down = 80; int pos33Down = 40;

int pos41Up = 170; int pos42Up = 55; int pos43Up = 30;
int pos41Down = 160; int pos42Down = 55; int pos43Down = 40;


const int magnet = 2;       // Magnet sættes til ben 2
const int irSensor = 6;     // Ir sensor sættes til ben 6
int irValue;                // Variabel for ir sensor værdi

// Farvesensor benopsætning
const int S0 = 8;
const int S1 = 9;
const int S2 = 10;
const int S3 = 11;
const int sensorOut = 12;
int frequency = 0;        // Variabel til frekvensen fra farvesonsor
int color=0;              // Variabel til hvilken farve emnet har

// De tre servo objekter laves for at kontrollere servo'erne
Servo myservo1;
Servo myservo2;
Servo myservo3;

void setup()
{
  Serial.begin(9600);       // Setup af serielkommunikation

  // Vedhæfter bennummer til servo objekterne
  myservo1.attach(3);
  myservo2.attach(4);
  myservo3.attach(5);

  // Sætter magnetens ben til output og irSensorens ben til input
  pinMode(magnet, OUTPUT);
  pinMode(irSensor, INPUT);
  
  // Sætter farvesensorens input og output
  pinMode(S0, OUTPUT);
  pinMode(S1, OUTPUT);
  pinMode(S2, OUTPUT);
  pinMode(S3, OUTPUT);
  pinMode(sensorOut, INPUT);

  // Sætter frekvens skalering for farveseonsor til 20%
  digitalWrite(S0, HIGH);
  digitalWrite(S1, LOW);
}

void loop()
{  
  // Funktion der flytter robotarmen til hjem position 
  moveArm(home1, home2, home3);
  // Aflæser ir sensor værdi       	   
  irValue = digitalRead(irSensor);
  // Hvis ir sensor værdi er LOW er sensoren påvirket og programmet starter.
  // Først er et delay der gør at robotten venter når et emne er lagt det er for ikke at mase fingrene. Robotten flytter ned til emnet og magneten aktiveres og emnet løftes og føres over til farvesensoren og sænker den ned til farvesensoren. Herefter startes en funktion der aflæser farven og giver et tal mellem 1 og 4   
  if(irValue == LOW)
    {
    delay(1000);                          
    moveArm(pickUp1, pickUp2, pickUp3);            
    digitalWrite(magnet, HIGH);              
    delay(1000);
    moveArm(home1, home2, home3);           
    delay(500);
    moveArm(colorUp1, colorUp2, colorUp3);  
    delay(500);
    moveArm(colorDown1, colorDown2, colorDown3); 
    delay(500);
    color = readColor();             
    delay(10);
    
    // Switch case funktion aflæser farven og går til den rigtige case
    // Hver case indeholder en funktion der flytter armen til en kasse og positionerne på den kasse, både løftet og sænket
    switch (color)                                  
    {   
      case 1:                                      
        moveToBox(pos11Up, pos12Up, pos13Up, pos11Down, pos12Down, pos13Down);
        Serial.println("Case 1 - Sort");
      break;
    
      case 2:
        moveToBox(pos21Up, pos22Up, pos23Up, pos21Down, pos22Down, pos23Down);
        Serial.println("Case 2 - Roed");
      break;
     
      case 3:
        moveToBox(pos31Up, pos32Up, pos33Up, pos31Down, pos32Down, pos33Down);
        Serial.println("Case 3 - Gul");
      break;
     
      case 4:
        moveToBox(pos41Up, pos42Up, pos43Up, pos41Down, pos42Down, pos43Down);
        Serial.println("Case 4 - Blaa");
      break;
    }
    moveArm(home1, home2, home3);                  // Robot hjem
    color = 0;                                     // Reset farve
  }  
}

// Funktion der giver værdier til de 3 servoer
void moveArm(int x, int y, int z)
{
  myservo1.write(x);
  myservo2.write(y);
  myservo3.write(z);
}

// Funktion der flytter robot til en kasse og afleverer emnet
void moveToBox(int a, int b, int c, int d, int e, int f)
{
  moveArm(a, b, c);           // Robot til valgte kasse, løftet
  delay(500);
  moveArm(d, e, f);           // Robot til valgte kasse, sænket
  delay(500);
  digitalWrite(magnet,LOW);   // Magnet sluk, emnet tabes
  delay(1000);
  moveArm(a, b, c);           // Robot til valgte kasse, løftet
  delay(500);
}

// Funktion der aflæser farven
int readColor()
{
  // Sætter rødt farvefilter på farvesensor, rød farve aflæses
  digitalWrite(S2, LOW);
  digitalWrite(S3, LOW);
  // Farvesensorens udgangsfrekvens aflæses
  frequency = pulseIn(sensorOut, LOW);
  int R = frequency;
  // Skriver værdien på seriel monitor
  Serial.print("R= ");
  Serial.print(frequency);
  Serial.print("  ");
  delay(50);

  // Sætter grønt farvefilter på farvesensor, grøn farve aflæses
  digitalWrite(S2, HIGH);
  digitalWrite(S3, HIGH);
  // Farvesensorens udgangsfrekvens aflæses
  frequency = pulseIn(sensorOut, LOW);
  int G = frequency;
  // Skriver værdien på seriel monitor
  Serial.print("G= ");
  Serial.print(frequency);
  Serial.print("  ");
  delay(50);
  
  // Sætter blåt farvefilter på farvesensor, blå farve aflæses
  digitalWrite(S2, LOW);
  digitalWrite(S3, HIGH);
  // Farvesensorens udgangsfrekvens aflæses
  frequency = pulseIn(sensorOut, LOW);
  int B = frequency;
  // Skriver værdien på seriel monitor
  Serial.print("B= ");
  Serial.print(frequency);
  Serial.println("  ");
  delay(50);

  // Sammenligner de målte værdier med typiske parametre for de forskellige farver
  if (B>95 & G>95)          // Parametre for sort farve
  {
    color = 1;              // Sort
  }
  else if(R<60 & G>80)      // Parametre for rød farve
  {
    color = 2;              // Rød
  }
  else if(R<40 & G<60)      // Parametre for gul farve
  {
    color = 3;              // Gul
  }
  else                      // Hvis ingen passer
  {
    color = 4;              // Blå
  }
  
  return color;             // Sender farven retur, bliver brugt i switch case
}

Robotarm.ino
Download

Step 7: Robotarm I Action

Videoer af robotarmen.

Step 8: Problemer Og Forbedringer

Det er ikke altid at robotarmen sorterer emnerne. Dette skyldes at farvesensoren ikke altid kan registrere farverne, hvis der et for meget lys fra omgivelserne. Det hjalp da vi satte robotarmen op af en væg, hvor lys niveauet var konstant.

På nuværende tidspunkt aktiveres sensoren med hjælp af vores hænder når der indleveres et emne. Hvis indleveringen skulle være automatisk og emnerne blev leveret af fx et transportbånd eller af en anden robotarm, så skulle der findes et emne med den rette højde.

Når robotarmen bevæger sig vil den nogle gange ændre stilling, som skyldes at den bevæger sig for voldsomt. Det medvirker til at de faste positioner inde i programmering skal justeres.

Introduction: EAL - Den Fuldautomatiske Farvesorterings Robotarm