onsdag 30 september 2015

Kod för att styra robot längs eltejp och "stationer"


/***********************************************************************
* The following is explained in Swedish
* En svart linje av eltejp läggs på golvet
* Ställ Redbot rakt över eltejpen (längs med) så den kan följa eltejpen
* Redbot kommer köra rakt fram med hastighet 75/255
* Om vänster sensor känner av ett förhöjt värde (svart eltejp) kommer Redbot svänga åt höger
* Om höger sensor känner av ett förhöjt värde (svart eltejp) kommer Redbot svänga åt vänster
* På så sätt håller sig Redbot längs eltejpen
* Sätt ett rakt streck av eltejp som ett kors någonstans längs "spåret"
* Detta är en "station"
* När Redbot kommer till station stannar motorerna
* Först blinkar lilla Leden på Redbot 5 ggr långsamt
* Sedan blinkar lilla Leden på Redbot 5 gg fortare
* Sedan startar Redbot från stationen och programmet återvänder till "normaldrift" framåt
*
* This code was written with lots of help from the Arduino community and Sparkfun Electronics
* This code is released into the public domain
*
* Kristoffer Nordekvist 30.09.15
 ***********************************************************************/

//Här kommer först instruktioner om vilka program som ska vara med:

#include <RedBot.h>
RedBotSensor IRSensor1 = RedBotSensor(A3); // initialize a sensor object on A3
RedBotSensor IRSensor2 = RedBotSensor(A6); // initialize a sensor object on A6
RedBotSensor IRSensor3 = RedBotSensor(A7); // initialize a sensor object on A7

RedBotMotors motors; //startar motorernas kontroll

int Led = 13; //LED anknuten till pin 13 (finns på breadboard)

void setup() //här kommer instruktioner om vilka program som ska köras en gång
{
  Serial.begin(9600);
  Serial.println("Här följer mätning av IR-sensorerna");
  Serial.println("-----------------------------------");
  }

void loop() //Här kommer instruktioner om det som ska köras "för alltid"
{
  Serial.print("IR Sensor Readings: ");
  Serial.print(IRSensor1.read());
  Serial.print("\t");  // tab character
  Serial.print(IRSensor2.read());
  Serial.print("\t");  // tab character
  Serial.print(IRSensor3.read());
  Serial.println();
  delay(50);

  //HUVUDKOMMANDO säger att kör framåt i 75/255 hastighet
 
    motors.rightMotor(75);//startar höger motor
    motors.leftMotor(-75);//startar vänster motor

      //SVÄNGKOMMANDO 1
      if(IRSensor1.read()>900)//om IRSensor1 läser ett värde större än 900
        {
          motors.leftMotor(-25);//ska vänster motor sänka varvtal
          motors.rightMotor(125);//höger motor kör på
          // Svänger till vänster
          }
          //SVÄNGKOMMANDO 2
      if(IRSensor3.read()>900)//om IRsensor3 läser ett värde större än 900
      {
          motors.rightMotor(25);//ska höger motor sänka varvtal
          motors.leftMotor(-125);//vänster motor kör på
          //Svänger till höger
        }
          //STOPPKOMMANDO
          if(IRSensor3.read()>900 && (IRSensor2.read()>900 || IRSensor1.read()>900))
            //Om alla tre sensorerna läser högt värde
            {
              motors.brake(); //ska båda motorerna stanna
                  for (int i=0; i<5; i++) //skapar en loop som ska köras 5 ggr inuti void loop
                 //startar räkningen på 0. Räknar UPP till 5. Varje gång adderas ett.
                  { //BLINKA LÅNGSAMT
                  digitalWrite(Led, HIGH); //slå på Led
                  delay(500);  //vänta 0,5 s (500 ms)
                  digitalWrite(Led, LOW); //slå av Led
                  delay(500); //vänta 0,5 s
                 }
                   for (int i=0; i<5; i++) //skapar en loop som ska köras 5 ggr inuti void loop
                    //startar räkningen på 0. Räknar UPP till 5. Varje gång adderas ett.
                   { //BLINKA FORT
                   digitalWrite(Led, HIGH);
                   delay(100); //vänta 0,1 s
                   digitalWrite(Led, LOW);
                    delay(100); //vänta 0,1 s
                    }
                //KÖR UT FRÅN STATIONEN
                      motors.rightMotor (50);
                      motors.leftMotor (-50);
                      delay (500);
               
            }

}

Inga kommentarer:

Skicka en kommentar