Af Morten S. Jensen (mojen17) og Benjamin Parbst (bepar17)

Dette indlæg vil vise og beskrive vores udvikling af den selvkørende robot vi har lavet i forbindele med vores Portfolieopgave 2 i hardware og robot teknologi.

Opbygning af hardware:

Kredsløbet i TinkerCad er bygget op efter to forskellige tilstande, “line tracking” eller “blind tracking”. Line tracking er den tilstand robotten er i, når den følger en streg og dermed anvender lys-sensorer som primært input. Blind tracking, er når der ikke er en streg at følge, hvorved robotten må finde vej med sensor input fra ultralydssensoren.

Sensorer og servo-kredsløb:

Lyssensorer er bygget op af en hvid LED og en lys modstand. Når den hvide LED projektere lys ned på overfladen vil den reflekterede lyskilde, som opfanges af lys modstanden, variere baseret på overfladens farve. Jo mørkere, jo mindre refleksion og jo lysere, jo mere refleksion. Der er to af disse lys-sensorer og baseret på afvigelsen mellem højre og venstre lyssensor, drejer robotten i varierende grad ved at ændre hastigheden på de to hjul uafhængigt af hinanden.

Ultralydssensoren virker ved at sende en lydimpuls ud og så opfange den tilbagevendende lydrefleksion med en lyssensor. Denne sensor bruges i blind tracking tilstanden, og baseret på hvor tæt på en mur robotten er, ændres køreegenskaberne på robotten. Vi har i tinkercad sat robotten til at stoppe hvis den er en halv meter fra en mur. Derudover drejes servoen, således at ultralydssensoren kan opfange et nyt lydinput. Ultralydssensoren skal nemlig placeres på servoen. Når robotten kører ligeud, peger denne ultralydssensor også ligeud.

Servoen bruges til at dreje ultralydssensoren med for at sensoren kan få måle input i en 180 graders vinkel foran robotten. I tinkercad drejes denne servomotor rundt, hvis ultralydssensoren måler en distance, som er mindre end den accepterede distance til en forhindring.

Figur 2

Kredsløbet for den hvide LED[7] fra lyssensoren ses øverst til venstre på figur 2. Den skal bruge en spænding mellem 3 og 3.6V og har en maksimal forward current på 30mA så formodstanden til LED’erne er beregnet til 100 ohm, for at nå en spænding på 3V:

R = 3.0V/0.03A = 100 Ohm

Kredsløbet til lysmodstanden, der opfanger LED’ens refleksion ses øverst til højre på figur 2. Lysmodstanden her tager udgangspunkt i den udleverede lysmodstand i lab-timerne, som ved svag belysning ligger på 50 ohm, og ved høj belysning, 55K Ohm. Vi har placeret 150 en ohm modstand i serie med denne, så at vi kan måle spændningsforskellen over lysmodstanden med arduinoen. Arduinoen er tilkoblet kredsløbet med analog in pins mellem lysmodstanden og modstanden. Der suppleres strøm til lyssensorkredsløbet fra arduinoen.

Ultralydssensoren[4] ses nederst til venstre på figur 2 og får strøm fra arduinoen, fordi den kun skal bruge 30 mA, hvilket arduinoen godt kan levere. GND pin er tilkoblet et fælles ground og SIG pin er tilkoblet en digital pin på arduinoen, som skiftevis kan input og output signal til denne pin for at sende og modtage lyd med sensoren.

Servoen[5] ses nederst til højre på figur 2, og denne er tilkoblet en ekstern strømforsyning, fordi dens maksimum strømtræk er 140mA +/- 50mA, hvilket arduinoen ikke kan levere. Servoen skal bruge mellem 4 og 6 volt på stromøforsynings pin’en, og derfor er 3 AA 1.5v batterier i serie tilkoblet denne. Grunden til at det er 3, er fordi batterierne ved maksimal kapacitet kan outputte 1.8 v, og for ikke at overskride de 6 volt, som servoen kan håndtere, så bruges 3 så batterierne til at starte med levere 5.4v, som over tid går mod 4.5v, som også er over de 4v, som servoen minimum skal bruge. Signal pin’en på servoen er tilkoblet en digital pin på arduinoen, som styrer servoen via arduinos servo library og GND pin’en er tilkoblet et fælles ground for arduinoen og ground på batterierne.

Motorkredsløbet

De to motorer er koblet til et H-bro kredsløb med 9v strømforsyning til hver motor, som vil tilsvare den udleverede Lego motor fra PF1. Robotten drejer ved at outputte forskellige spændinger til de to motorer, baseret på et input. Disse motorer er placeret bagerst på robotten i unity således at robotten kan dreje mest muligt.

Figur 3

Motorkredsløbets kan ses på figur 3 og dets komponenter består af en L293D H-bro[2], 2 stk PF-large lego motorer[3] og 6 stk 9v batterier[1]. Vi har valgt at sætte 6stk 9v batterier i parallel, fordi at PF-large lego motoren trækker op mod 450 mA, hvilket et 9v batteri ikke levere. Det 9v batteri, som vi har valgt til kredsløbet, har en maksimal afladningsstrøm på 150 mA, hvilket betyder at vi skal have 2 stk motor * 450 mA / 150 mA pr batteri = 6 batterier for ikke at overbelaste nogle af batterierne. Disse batterier er derefter parallelt tilkoblet VCC1 og VCC2, for at fordele spændingen mellem disse to. VCC1 og VCC2 har maksimalt input på 36V, men den anbefalede spænding på VCC1 er mellem 4.5v og 7v.

DC motorene er tilkoblet output pin’s 1-4Y, og retningen motorene bestemmes af input pins 1-4A, som er tilkoblet arduinoes digitale pin’s. 1,2EN og 3,4EN bruges til at styre hastigheden af henholdsvis højre og venstre dc motor og disse er også tilkoblet arduinoens digitale pin’s. Ground pin’en er tilkoblet et fælles ground for batterierne og arduinoen.

Arduinoen skal også bruge strøm, og dette kunne suppleres med en parallel forbindelse til de batterier, som levere strøm til servo-moteren.

Komponentliste:

Figur 4

På figur 4 ses de komponenter, som er brugt i TinkerCAD. Der er ikke så meget fleksibilitet i TinkerCAD i forhold til komponenters specificering generelt, så dimensionering passer ikke 100% overens med tinkerCAD og kredsløbet, som det havde været samlet fysisk. De komponenter, som vi ville have samlet kredsløbet med fysisk består af:

2 stk DC motor (Lego Power Functions Large Motor)[3]:

  • Max spænding: 9v
  • Stall current: 450 mA

2 stk Lysmodstande:

  • Høj belysning: 50 ohm
  • Svag belysning: 55 kOhm

1 stk L293D H-bro[2]

1 stk Arduino Uno

2 stk 100 Ohm modstande

2 stk 150 Ohm modstande

1 stk Parallex Ping ultralydssensor[4]

1 stk Parallex Standard Servo[5]

6 stk 9v genopladelige batterier[1]

3 stk 1.5v genopladelige batterier[6]

Kode:

Dette er koden fra tinkerCAD, den har en implementering af de to overordnede adfærder, LINE_TRACKING OG BLIND_TRACKING, som vi anvender i Unity. Adfærden kan skiftes ved at sætte state variablen til at være enten LINE_TRACKING eller BLIND_TRACKING i setup metoden.

#include <Servo.h>

#define LEFT_LED 2
#define RIGHT_LED 3
#define LEFT_PHOTO_SENSOR 4
#define RIGHT_PHOTO_SENSOR 5

#define LEFT_FORWARD 6
#define LEFT_BACKWARD 7
#define RIGHT_FORWARD 8
#define RIGHT_BACKWARD 9

#define LEFT_MOTOR_CONTROL 10
#define RIGHT_MOTOR_CONTROL 11

#define SERVO_CONTROL 12
#define DISTANCE_SENSOR 13

#define LEFT_PHOTO_SENSOR_INPUT A0
#define RIGHT_PHOTO_SENSOR_INPUT A1

enum States {
	LINE_TRACKING, BLIND_TRACKING
};

// cycle update rate
int cycleTime = 20;

// the states that the robot is in for evaluation in real-time
States state;

// photo sensor value readings
int leftSensorReading = 0;
int rightSensorReading = 0;
int distanceSensorReading = 0;

// the maximum speed of the robot, which constraints the real-time speed
int maxSpeed = 120;
int speed = 0;
int cruisingSpeed = 200;

// the degree of turning on a scale from 0 to 1, 0.5 being straight forward
double degreeOfTurning = 0.5;

double sensorMean = 0;
double sensorDeviation = 0;

int servoAngle = 90;
Servo servo;

long cmFromObstacle = 100;

/**
* Accelerates the car to the given speed limit in the argument. There is a gradual
* acceleration or decceleration to the given speed limit and speed is slowed down
* in turns based on the sharpness of the turn.
*/
void accelerate(int speedLimit, bool forward) {
  // increases speed if max speed is not achieved yet
  speed = speed < speedLimit ? speed + 1 : speed - 1;

  // calculates the sharpness of turns on a scale from 0 to 1
  double speedTurningDecrease = (degreeOfTurning - 0.5) / 0.5;
  speedTurningDecrease = speedTurningDecrease < 0 ? speedTurningDecrease * -1 : speedTurningDecrease;

  // factor to decrease speed by up to 80% based on the sharpness of the turn
  double speedDecreaseFactor = 1 - (speedTurningDecrease * 0.8);

  if (forward) {
    // makes sure the H-bridge does not have input on the opposite direction
    digitalWrite(LEFT_BACKWARD, LOW);
    digitalWrite(RIGHT_BACKWARD, LOW);
    
    digitalWrite(LEFT_FORWARD, HIGH);
    digitalWrite(RIGHT_FORWARD, HIGH);

    // outputs speed to the dc motor actuator
    analogWrite(LEFT_MOTOR_CONTROL, 2 * (int) ((speed * (1 - degreeOfTurning)) * speedDecreaseFactor));
    analogWrite(RIGHT_MOTOR_CONTROL, 2 * (int) ((speed * degreeOfTurning) * speedDecreaseFactor));
  } else {
    // makes sure the H-bridge does not have input on the opposite direction
    digitalWrite(LEFT_FORWARD, LOW);
    digitalWrite(RIGHT_FORWARD, LOW);
    
    digitalWrite(LEFT_BACKWARD, HIGH);
    digitalWrite(RIGHT_BACKWARD, HIGH);

    // outputs speed to the dc motor actuator
    analogWrite(LEFT_MOTOR_CONTROL, 2 * (int) ((speed * (1 - degreeOfTurning)) * speedDecreaseFactor));
    analogWrite(RIGHT_MOTOR_CONTROL, 2 * (int) ((speed * degreeOfTurning) * speedDecreaseFactor));
  }

}

/**
* Sets the degree of turning based on the deviation between two sensor inputs from the
* light sensors. This sensor deviation will be translated to a range from 0 to 1
* where 0.5 is straight forward, 0 being right and 1 being left.
*/
void steer(double sensorPercentageDeviation) {
  degreeOfTurning = 0.5 + (sensorPercentageDeviation * 0.5);
}

/**
* Arduino methods starts from here
*/
void setup() {
  Serial.begin(9600);
  
  state = BLIND_TRACKING;
  
  pinMode(LEFT_LED, OUTPUT);
  pinMode(RIGHT_LED, OUTPUT);
  pinMode(LEFT_PHOTO_SENSOR, OUTPUT);
  pinMode(RIGHT_PHOTO_SENSOR, OUTPUT);
  
  pinMode(LEFT_FORWARD, OUTPUT);
  pinMode(LEFT_BACKWARD, OUTPUT);
  pinMode(RIGHT_FORWARD, OUTPUT);
  pinMode(RIGHT_BACKWARD, OUTPUT);
  
  pinMode(LEFT_MOTOR_CONTROL, OUTPUT);
  pinMode(RIGHT_MOTOR_CONTROL, OUTPUT);
  
  servo.attach(SERVO_CONTROL);
  pinMode(DISTANCE_SENSOR, INPUT);
}

void loop() {
  
  // run sequence based on the state of the robot
  switch (state) {
  	case LINE_TRACKING:
    	digitalWrite(LEFT_LED, HIGH);
        digitalWrite(RIGHT_LED, HIGH);

        digitalWrite(LEFT_PHOTO_SENSOR, HIGH);
        digitalWrite(RIGHT_PHOTO_SENSOR, HIGH);
    
    	// the light sensors input value based on the ground, being on the line means a
        // lower value, and not on line, a higher value
        leftSensorReading = analogRead(LEFT_PHOTO_SENSOR_INPUT);
        rightSensorReading = analogRead(RIGHT_PHOTO_SENSOR_INPUT);

        // calculates the mean between the two light sensor inputs and the deviation
        // between thems
        sensorMean = (leftSensorReading + rightSensorReading) / 2;
        sensorDeviation = (double) rightSensorReading - sensorMean;

        // acceleration and steering actuation of the robot
        steer(sensorDeviation / sensorMean);
        accelerate(cruisingSpeed, true);
    
    	break;
    
    case BLIND_TRACKING:
    	// send out sound
    	pinMode(DISTANCE_SENSOR, OUTPUT);
    
    	digitalWrite(DISTANCE_SENSOR, LOW);
    	delayMicroseconds(2);
    
    	digitalWrite(DISTANCE_SENSOR, HIGH);
    	delayMicroseconds(2);
    
    	digitalWrite(DISTANCE_SENSOR, LOW);
    
    	// capture the response sound and convert it to cm
    	pinMode(DISTANCE_SENSOR, INPUT);
    	distanceSensorReading = pulseIn(DISTANCE_SENSOR, HIGH);
    	cmFromObstacle = distanceSensorReading / 29 / 2;
    
    	Serial.println(cmFromObstacle);
    
        if (cmFromObstacle < 50) {
			accelerate(0, true);
          
            if (servoAngle > 0) {
				servo.write(servoAngle--);
            }
        } else {
          	accelerate(cruisingSpeed, true);
          
          	if (servoAngle < 90) {
				servo.write(servoAngle++);
            } else if (servoAngle > 90) {
             	servo.write(servoAngle--);
            }
        }
    
    	
    
    	break;
    
    default:
    	break;
  }
  
  delay(cycleTime); // Wait for 1000 millisecond(s)
}

Link til tinkercad:

https://www.tinkercad.com/things/1Sk01hog1j1-pf2/editel?sharecode=fasfiF95Cnj5vMMKwqW2A0YDxs5jav25BbUKhPjhsL0

Opbygning af adfærd:

  • Opbygning af programmet er baseret på at vores robot har forskellige states/actions baseret på hvilke udfordringer den skal løse. LINE_TRACKING, DRIVE_AROUND_OBSTACLE, ALIGN_ROBOT, BLIND_TRACKING og STOP.
  • LINE_TRACKING: Dette er den første state som robotten er i. Her baserer vores robot primært dens bevægelser på lys sensor input. Vores lyssensor er placeret help op af hinanden midt under robotten. Dette er gjort for at følge stregen så præcist så muligt. sensor inputtet vi får fra de 2 lyssensor finder vi gennemsnittet af og dette bliver vores normal værdi. Vi kan derefter måle om vores input fra enten højre eller venstre sensor har store udsving og dermed om vore robot dreje til henholdsvis højre eller venstre. Hvis robotten registrerer at den skal korrigere sætter den farten lidt ned og laver den korrosion den nu skal. Hvis forskellen i gennemsnits lys inputtet bliver højer eller laver med mere end 10% bliver der sendt et signal til vores servo, hvorpå vores distance sensor sidder, om at den skal dreje i den retning robotten drejer. Hvis det input distance sensoren returnerer er under en minimumsværdi, ved robotten at der står, noget i vejen. Når dette sker skifter robotten state.
  • DRIVE_AROUND_OBSTACLE: Når robotten er i dette state er det fordi den skal køre uden om den forhindring der nu står i vejen for den. Robotten drejer nu om egen akse, så den får en vinkel til forhindringen således at den kan køre uden om. Servoen drejer nu således at distance sensoren peger på forhindringen og måler derefter afstanden. Robotten køre nu i en bue rund om forhindringen, indtil den igen rammer den sorte streg med den lyssensor. Når robotten gør det drejer den igen rundt om sig selv, dog lidt hurtigere på det ene hjul end det andet, indtil at den modsatte lyssensor rammer den sorte streg, og robotten skifte tilbag til LINE_TRACKING statet. 
  • LINE_TRACKING: Igen køre robotten primært på lyssensor input og følger stregen rundt. Når den kommer til enden at den sorte streg fortsætter robotten indtil distance sensorens måling rammer vores minimumsværdi. Her skifter robotten igen state.
  • ALIGN_ROBOT: Servoen drejer nu, samtidigt med den måle distance indtil, at distance sensoren måler en værdi som er større end den foregående. Dette betyder at har fundet den mur som er tættest på den. (Man ville kunne have lavet således at distance sensoren drejede til begge side og målte afstanden, og ud fra dette valgte hvilken retning at dreje til, men da vi kendte banens layout kunne vi nøjes med at checke den ene retning). Robotten drejer nu igen om egen akse ca tilsvarende til den mængde servoen drejede, således at den robotten kan køre hen til muren. Når robotten har drejet begynder den at køre ligeud indtil at distance igen registrer vores minimumsværdi. Robotten skifter nu igen state.
  • BLIND_TRACKING: Robotten drejer nu om egen akse således der kommer til være ca parallel med muren. Servoen drejer distance sensoren helt om mod muren og måler afstanden. Dette gør den så længe robotten køre langs en mur. Hvis distancen mellem robotten og muren bliver enten for stor eller for lille drejer robotten enten væk fra muren eller ind mod muren. 

Når robotten kommer til enden af muren stopper den og drejer servoen således at distance sensoren peger lige ud for robotten. Robotten køre nu lige ud indtil distance sensoren måling er lig med minimumsværdien. Den drejer nu igen om egen akse om og peger distance sensoren ind mod muren, lige som den gjorde første gang. Processen gentager sig nu igen indtil at robottens lyssensor rammer den sorte tværgående steg. Og robotten skifter til det sidste state STOP.

using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using System.Threading;

public class ArduinoMain : MonoBehaviour
{
    public Breadboard breadboard;
    public Servo servo;
    
    // speed variables for both weels and offsets for each wheel
    private int speed = 0;
    private int rightWheelSpeedOffset = 0;
    private int leftWheelSpeedOffset = 0;

    // the variable determining the "normal" light intake from the ground
    private int normalLeftLightInput = 0;
    private int normalRightLightInput = 0;

    // line tracking
    private bool driveRight = false;
    private bool driveLeft = false;
    private bool firstIteration = true;
    // the angle of the servo, going from 0 to 180 degrees. 90 is straight forward
    private int servoAngle = 90;
    
    // arduino input pin constants
    private const int LEFT_FORWARD = 3;
    private const int LEFT_BACKWARD = 2;
    private const int RIGHT_FORWARD = 1;
    private const int RIGHT_BACKWARD = 0;
    private const int LEFT_SENSOR = 4;
    private const int RIGHT_SENSOR = 5;
    private const int DISTANCE_SENSOR = 6;

    // constraints on the speed
    private const int MAX_SPEED = 50;
    private const int MIN_SPEED = 20;

    private int cruisingSpeed = 50;
    private double degreeOfTurning = 0.5;

    // default: false
    private bool obstacleIsPassed = false;
    private bool wallOnLeftSide = false;

    // how close can the car be to an obstacle
    // before value = 700
    private const int DISTANCE_THRESHHOLD = 500;

    private ulong obstacleDistance = 0;
    // before value = 100
    private ulong obstacleDistanceSlack = 50;

    // states of actions the robot can do
    private enum Actions {
        LINE_TRACKING, DRIVE_AROUND_OBSTACLE, ALIGN_ROBOT, STOP, BLIND_TRACKING
    }

    // declared actions enum
    Actions actions;

    /**
    *  ***********************
    *  OUR METHODS STARTS HERE
    *  ***********************
    */
    private void accelerate(int speedLimit, bool forward) {
        // increases speed if max speed is not achieved yet
        speed = speed < speedLimit ? speed + 1 : speed - 1;

        // calculates the sharpness of turns on a scale from 0 to 1
        double speedTurningDecrease = (degreeOfTurning - 0.5) / 0.5;
        speedTurningDecrease = speedTurningDecrease < 0 ? speedTurningDecrease * -1 : speedTurningDecrease;

        // factor to decrease speed by up to 80% based on the sharpness of the turn
        double speedDecreaseFactor = 1 - (speedTurningDecrease * 0.8);

        if (forward) {
            // makes sure the H-bridge does not have input on the opposite direction
            analogWrite(LEFT_BACKWARD, 0);
            analogWrite(RIGHT_BACKWARD, 0);

            // outputs speed to the dc motor actuator
            analogWrite(LEFT_FORWARD, 2 * (int) ((speed * (1 - degreeOfTurning)) * speedDecreaseFactor));
            analogWrite(RIGHT_FORWARD, 2 * (int) ((speed * degreeOfTurning) * speedDecreaseFactor));
        } else {
            // makes sure the H-bridge does not have input on the opposite direction
            analogWrite(LEFT_FORWARD, 0);
            analogWrite(RIGHT_FORWARD, 0);

            // outputs speed to the dc motor actuator
            analogWrite(LEFT_BACKWARD, 2 * (int) ((speed * (1 - degreeOfTurning)) * speedDecreaseFactor));
            analogWrite(RIGHT_BACKWARD, 2 * (int) ((speed * degreeOfTurning) * speedDecreaseFactor));
        }

        // debugging
        Debug.Log("Accelerate (speed): " + speed);
        Debug.Log("Accelerate (speed limit): " + speedLimit);
        Debug.Log("Accelerate (degrees of turning): " + degreeOfTurning);
        
    }

     private void steer(double sensorPercentageDeviation) {
        degreeOfTurning = 0.5 + (sensorPercentageDeviation * 0.5);
        Debug.Log("Steering (sensor deviation): " + sensorPercentageDeviation);
    }

    private void rotateRobot(bool clockwise, int rotationSpeed) {
        // makes sure no output is given to the dc motors
        speed = speed < rotationSpeed ? speed + 1 : speed - 1;

        if (clockwise) {
            analogWrite(LEFT_FORWARD, speed);
            analogWrite(RIGHT_BACKWARD, speed);
        } else {
            analogWrite(LEFT_BACKWARD, speed);
            analogWrite(RIGHT_FORWARD, speed);
        }

    }
    

    // gets input from the sonar sensor on how close the car is to an obstacle
    private void checkDistanceSensor(){
        // gets input reading
        
        ulong distanceMeasure = pulseIn(DISTANCE_SENSOR);
        Debug.Log(distanceMeasure);
        // if distance measure is below an accepted threshhold of distance and is
        // not 0 (no return of sound)
        if ((distanceMeasure < DISTANCE_THRESHHOLD && distanceMeasure != 0) && !obstacleIsPassed) {
            // sets drive around obstacle state as active
            Debug.Log("Around");
            setAction(Actions.DRIVE_AROUND_OBSTACLE);
            obstacleDistance = distanceMeasure;
        } else if ((distanceMeasure < DISTANCE_THRESHHOLD + 180 && distanceMeasure != 0) && obstacleIsPassed) {
            Debug.Log("BLIND");

            setAction(Actions.ALIGN_ROBOT);
            obstacleDistance = distanceMeasure;
        }
    }

    // set the angle of the servo according to the direction    
    private void setServoAngle(){
        //determines the angle based on the direction
        

        if(degreeOfTurning >= 0.6)
        {
            servoAngle --;
        } else if( degreeOfTurning <= 0.4)
        {
            servoAngle ++;
        }
            

        //output the particular angle to the servo
        servo.write(servoAngle);
    }


    private void setAction(Actions action){
        actions = action;
        firstIteration = true;
    }

    private void stopForASecond() {
        analogWrite(LEFT_FORWARD, 0);
        analogWrite(RIGHT_FORWARD, 0);
        analogWrite(LEFT_BACKWARD, 0);
        analogWrite(RIGHT_BACKWARD, 0);
    }
    
    /**
    *  ***********************
    *   OUR METHODS ENDS HERE
    *  ***********************
    */

    IEnumerator setup()
    {
        //Your code goes here:

        // # 1
        actions = new Actions();
        actions = Actions.LINE_TRACKING;

        // # 2

        normalLeftLightInput = analogRead(LEFT_SENSOR);
        normalRightLightInput = analogRead(RIGHT_SENSOR);

        servo.write(servoAngle);

        //Example of delay:
        Debug.Log("pre-delay log");
        yield return delay(2000); //2 second delay
        //Your code ends here -----

        //following region ensures delay-functionality for setup() & loop(). Do not delete, must always be last thing in setup.
        #region PremadeSetup
        yield return StartCoroutine(loop());
        #endregion PremadeSetup
    }

    IEnumerator loop()
    {
        //Your code goes here:
        // #1
        switch (actions)
        {   //Line tracking is when the robot is driveing along a line on the road
            case Actions.LINE_TRACKING:
                // linetrack
                
                int leftSensorInput = analogRead(LEFT_SENSOR);
                int rightSensorInput = analogRead(RIGHT_SENSOR);

                Debug.Log("Left Sensor Input: " + leftSensorInput);
                Debug.Log("Right Sensor Input: " + rightSensorInput);

                // calculates the mean between the two light sensor inputs and the deviation
                // between thems
                double sensorMean = (leftSensorInput + rightSensorInput) / 2;
                double sensorDeviation = (double) rightSensorInput - sensorMean;

                // acceleration and steering actuation of the robot
                checkDistanceSensor();
                if(!obstacleIsPassed)
                {
                    setServoAngle();
                }
                steer(sensorDeviation / sensorMean);
                accelerate(cruisingSpeed, true);

                // check if the line has come to a stop
                if (sensorMean >= 1023) {
                    
                    setAction(Actions.ALIGN_ROBOT);
                    
                    // break the car
                    while (speed != 0) {
                        checkDistanceSensor();
                        steer(0);
                        accelerate(0, true);
                        yield return delay(10);
                    }
                }

                if (actions == Actions.LINE_TRACKING)
                    firstIteration = false;
                
                
                break;

            case Actions.ALIGN_ROBOT:

                servoAngle = 40;
                servo.write(servoAngle);
                yield return delay(1000);

                // sets up variables to hold sound sensor input
                ulong distanceSensorInput = pulseIn(DISTANCE_SENSOR);
                ulong previousInputReading = distanceSensorInput;

                // turn the servo until it points somewhat straight into the wall in front of it
                while (distanceSensorInput <= previousInputReading) {
                    previousInputReading = distanceSensorInput < previousInputReading ? distanceSensorInput : previousInputReading;
                    
                    // rotate the servo
                    servo.write(servoAngle--);
                    //rotateRobot(false, 10);

                    yield return delay(50);

                    //Debug.Log("Distance Sensor Input: " + distanceSensorInput);

                    distanceSensorInput = pulseIn(DISTANCE_SENSOR);
                }

                previousInputReading = distanceSensorInput;

                // rotate the robot so that it faces straight into the wall as well
                while (servoAngle != 90) {
                    distanceSensorInput = pulseIn(DISTANCE_SENSOR);
                    rotateRobot(false, 10);

                    if (distanceSensorInput > previousInputReading ) {
                        servo.write(servoAngle++);
                    }

                    yield return delay(10);
                }

                // stop rotation of the robot
                while (speed != 0) {
                    rotateRobot(false, 0);
                    yield return delay(10);
                }

                setAction(Actions.BLIND_TRACKING);

                break;
            //Drive around obstacle is the distance sensor tells the robot that something is in it's way,
            //but still needs to find a line to follow againn
            case Actions.DRIVE_AROUND_OBSTACLE:
                // stop the car 
                Debug.Log("Obstacle in the way!");
                if (firstIteration) {
                    stopForASecond();
                    yield return delay(2000);

                    analogWrite(LEFT_BACKWARD, 20);
                    analogWrite(RIGHT_FORWARD, 20);

                    yield return delay(3200);

                    stopForASecond();

                    while (pulseIn(DISTANCE_SENSOR) < obstacleDistance - obstacleDistanceSlack || pulseIn(DISTANCE_SENSOR) > obstacleDistance + obstacleDistanceSlack) {
                        int angle = servo.read();

                        if (angle < 180 && angle > 0)
                            servo.write(servoAngle++);

                        yield return delay(10);
                    }

                }

                while (
                    analogRead(LEFT_SENSOR) >= normalLeftLightInput
                ) {
                    // tjekke lydsensor
                    if (pulseIn(DISTANCE_SENSOR) < obstacleDistance - obstacleDistanceSlack || 
                        pulseIn(DISTANCE_SENSOR) > obstacleDistance + obstacleDistanceSlack
                    ) {
                        leftWheelSpeedOffset = 20;
                        rightWheelSpeedOffset = -5;
                    }

                    // kører ligeud
                    analogWrite(LEFT_FORWARD, MIN_SPEED + leftWheelSpeedOffset);
                    analogWrite(RIGHT_FORWARD, MIN_SPEED + rightWheelSpeedOffset);

                    // bestemme offsets

                    yield return delay(10);
                }

                stopForASecond();

                //analogWrite(RIGHT_FORWARD, MIN_SPEED);
                analogWrite(RIGHT_FORWARD, MIN_SPEED);

                yield return delay(2000);

                //yield return delay(2000);
                while (analogRead(RIGHT_SENSOR) >= normalLeftLightInput) {
                    analogWrite(RIGHT_FORWARD, MIN_SPEED);
                    analogWrite(LEFT_BACKWARD, 5);
                    servo.write(90);
                    yield return delay(10);
                }

                stopForASecond();
                
                obstacleIsPassed = true;

                setAction(Actions.LINE_TRACKING);

                checkDistanceSensor();

                firstIteration = false;

                break;
                //Stop is when the robot's line sensors both are ative
            case Actions.STOP:
                // stop
                analogWrite(LEFT_FORWARD, 0);
                analogWrite(RIGHT_FORWARD, 0);
                break;
                // When there isen't a line for the robot to follow and it has to figur out how to procide 
        
                //When the robot has to drive only on the distance sensor
            case Actions.BLIND_TRACKING:
                Debug.Log("BLIND TRACKING");
                // blind track that shit
                    stopForASecond();
                // sound sensor input
                ulong soundSensorInput = pulseIn(DISTANCE_SENSOR);
                bool drivingAlongWall = false;
                // kør ligeud indtil en vis afstand fra muren
                if (soundSensorInput > DISTANCE_THRESHHOLD - obstacleDistanceSlack && soundSensorInput != 0 && !drivingAlongWall)
                {
                    // sensor ligeud
                    servo.write(90);
                    
                    analogWrite(LEFT_FORWARD, MIN_SPEED);
                    analogWrite(RIGHT_FORWARD, MIN_SPEED);
                } else {
                    firstIteration = false;
                    
                    stopForASecond();

                    if (wallOnLeftSide) {
                        // drej servor og mål afstand
                        analogWrite(RIGHT_BACKWARD, MIN_SPEED);
                        analogWrite(LEFT_FORWARD, MIN_SPEED);
                        yield return delay(2900);
                    } else {
                        // drej servor og mål afstand
                        analogWrite(LEFT_BACKWARD, MIN_SPEED);
                        analogWrite(RIGHT_FORWARD, MIN_SPEED);
                        yield return delay(2300);
                    }

                    stopForASecond();

                    drivingAlongWall = true;

                    if (wallOnLeftSide) {
                        servo.write(0);
                    } else {
                        servo.write(180);
                    }

                    yield return delay(3000);
                }
                
                while (drivingAlongWall) {
                    if (wallOnLeftSide) {
                        servo.write(0);
                    } else {
                        servo.write(180);
                    }

                    soundSensorInput = pulseIn(DISTANCE_SENSOR);
                    if (soundSensorInput > DISTANCE_THRESHHOLD && soundSensorInput != 0 && soundSensorInput <= DISTANCE_THRESHHOLD + obstacleDistanceSlack)
                    {
                        // drej ind mod muren
                        if (wallOnLeftSide) {
                            rightWheelSpeedOffset = 5;
                            leftWheelSpeedOffset = 0;
                        } else {
                            leftWheelSpeedOffset = 5;
                            rightWheelSpeedOffset = 0;
                        }
                        
                    } else if (soundSensorInput > DISTANCE_THRESHHOLD + obstacleDistanceSlack || soundSensorInput == 0) {
                        Debug.Log("NOT DRIVING ALONG WALL ANYMORE!!");
                        drivingAlongWall = false;

                        // NU MÅ DEN KUN KØRE LIGEUD

                        stopForASecond();
                        servo.write(90);

                        yield return delay(2000);

                        if (pulseIn(DISTANCE_SENSOR) == 0) {
                            analogWrite(LEFT_FORWARD, MIN_SPEED);
                            analogWrite(RIGHT_FORWARD, MIN_SPEED);

                            while (
                                analogRead(LEFT_SENSOR) >= normalLeftLightInput &&
                                analogRead(RIGHT_SENSOR) >= normalRightLightInput
                            ) {
                                analogWrite(LEFT_FORWARD, MIN_SPEED);
                                analogWrite(RIGHT_FORWARD, MIN_SPEED);
                                
                                yield return delay(10);
                            }

                            setAction(Actions.STOP);
                        }

                        wallOnLeftSide = true;
                    } else if (soundSensorInput < DISTANCE_THRESHHOLD && soundSensorInput != 0)
                    {
                        // drej væk fra muren
                        if (wallOnLeftSide) {
                            leftWheelSpeedOffset = 5;
                            rightWheelSpeedOffset = 0;
                        } else {
                            rightWheelSpeedOffset = 5;
                            leftWheelSpeedOffset = 0;
                        }
                    } else {
                        leftWheelSpeedOffset = 0;
                        rightWheelSpeedOffset = 0;
                    }
                    
                    analogWrite(LEFT_FORWARD, MIN_SPEED + leftWheelSpeedOffset);
                    analogWrite(RIGHT_FORWARD, MIN_SPEED + rightWheelSpeedOffset);

                    yield return delay(10);
                }

                //Drej 90 grader omkring egen akse

                //Mål afstand til mur
                
                //setAction();
                
                break;
                
            default:
                // bla bla
                break;
        }

        yield return delay(10);

        //Following region is implemented as to allow "yield return delay()" to function the same way as one would expect it to on Arduino.
        //It should always be at the end of the loop()-function, and shouldn't be edited.
        #region DoNotDelete
        //Wait for one frame
        yield return new WaitForSeconds(0);
        //New loop():
        yield return loop();
        #endregion DoNotDelete 
    }

    #region PremadeDefinitions
    void Start()
    {
        servo = FindObjectOfType<Servo>();
        if (servo == null)
        {
            Debug.Log("No servo found in the scene. Consider assigning it to 'ArduinoMain.cs' manually.");
        }
        Time.fixedDeltaTime = 0.005f; //4x physics steps of what unity normally does - to improve sensor-performance.
        StartCoroutine(setup());


    }

    IEnumerator delay(int _durationInMilliseconds)
    {
        float durationInSeconds = ((float)_durationInMilliseconds * 0.001f);
        yield return new WaitForSeconds(durationInSeconds);
    }

    public long map(long s, long a1, long a2, long b1, long b2)
    {
        return b1 + (s - a1) * (b2 - b1) / (a2 - a1);
    }

    public ulong millis()
    {
        return (ulong)(Time.timeSinceLevelLoad * 1000f);
    }

    public ulong abs(long x)
    {
        return (ulong)Mathf.Abs(x);
    }

    public long constrain(long x, long a, long b)
    {
        return (x < a ? a : (x > b ? b : x));
    }

    #endregion PremadeDefinitions

    #region InterfacingWithBreadboard
    public int analogRead(int pin)
    {
        return breadboard.analogRead(pin);
    }
    public void analogWrite(int pin, int value)
    {
        breadboard.analogWrite(pin, value);
    }
    public bool digitalRead(int pin)
    {
        return breadboard.digitalRead(pin);
    }
    public void digitalWrite(int pin, bool isHigh)
    {
        breadboard.digitalWrite(pin, isHigh);
    }

    public ulong pulseIn(int pin)
    {
        return breadboard.pulseIn(pin);
    }
    #endregion InterfacingWithBreadboard
}

Video af robot

https://www.dropbox.com/s/fkk4d81yvdacikc/pf2%20robot.mov?dl=0&fbclid=IwAR2nw5Dst30KtDZil8WDFBQVhPzq0R461WcfcayL_REgk_fRzqKvdSyEwoc

Hvordan er samspillet mellem mekanik, elektronik og software?: 

Man kan sammenligne vores robots, mekaniske, elektroniske og software komponenters samspil, med et menneske.

Kroppen er det samme som de mekaniske komponenter, elektroniske er det samme som et menneskes sanser(Se, lytte, mærke, høre), og software der styre de mekaniske baseret på de elektroniske, ligesom hjerne gør det ved mennesker. De mekaniske komponenter består af 3 motor. En til hvert hjul og en servo motor som er placeret oven på robottens base til at styre distance sensors retning. 

De elektroniske komponenter registrer nogle inputs som de sender til softwaren. De elektriske komponenter består af, 2 LDR modstande, brugt til at måle lys input, og en ultralydssensor, brugt til at mål afstanden til en genstand målt i millisekunder. 

Baseret på disse inputs analyser softwaren dem og lave en handling baseret på disse inputs. Software komponenterne er implementeret via en arduino, som modtager og sender elektriske impulser.  Disse impulser/handlinger bliver sendt ud til de mekaniske komponenter, som udføre dem. Om det er at dreje, stoppe eller lignende har softwaren sendt til de mekaniske komponenter.  

Konklusion – hvor godt løser robotten opgaven: 

I forhold til de krav som var sat i forhold til denne opgave, løser vores robot dem til perfektion. Opførelsen for robotten fungerer som forventet, og det kommer igennem banen hver gang på ca 150-160 sek. De pågældende restriktioner som er givet i opgaven og for den oprindelige ikke covid-19 opgave ville kunne blive udført af vores virtuelle robot. Alt dette betyder at vores robot løser alle udfordringer den bliver stillet for på den pågældende bane.  

Perspektivering – Hvad kan man gøre for at gøre den bedre?

Software: Desværre er vores software løsning i løbet af udviklingsprocessen blevet lidt for skræddersyet til præcis banen i denne aflevering. Der kom hen af vejen flere og flere løsninger som specifik viden omkring denne bane.  Dette kunne vi godt have tænkte os at have lavet mere generisk og med mere tid også tænkte noget AI lignende adfærd ind i vores løsning. Vi kunne også godt have tænkt os at koden så lidt pænere ud, men igen det virker og det gør hvad det skal. 

Vi kunne godt have gjort vores kode(BLIND_TRACKING) mindre kompliceret og mere overskuelig ved at dele noget af funktionaliteten ud i mindre og mere genanvendelig metoder  således at vi ikke hele tiden skal lave checks for hvor vi er på banen og hvad robotten nu skal fortage sig.

For at kunne færdiggøre banen hurtigere ville vi kunne justerer nogle få parameter. Vi kunne sætte de delays vi har i unity programmet ned således at det var mere optimeret til hastigheder som robotten har til at udføre visse handlinger(dreje servomotoren, dreje om egen akse, osv.). Disse justeringer ville kunne skrabe nogle få sekunder af vores tid.  Vi ville også kunne sætte max hastigheden som vores robot har lidt op således at den ville køre hurtigere når der var mulighed for det. Men ellers som programmet er bygget op lige pt. vil dette ikke blive vanvittigt meget hurtigere. Vi valgte at gå en mere forsigtig og sikker vej, end en hurtig og mere klodset. 

Referencer:

  1. https://www.batteribyen.dk/media/blfa_files/ultralife-u9vl-jp-technical-data-sheet.pdf
  2. http://www.ti.com/lit/ds/symlink/l293.pdf
  3. https://www.lego.com/en-us/themes/power-functions/element-spec
  4. https://docs.rs-online.com/ca69/0900766b81382974.pdf
  5. https://docs.rs-online.com/0e85/0900766b8123f8d7.pdf
  6. https://data.energizer.com/pdfs/l91.pdf
  7. http://www1.futureelectronics.com/doc/EVERLIGHT%C2%A0/334-15__T1C1-4WYA.pdf

Leave a Reply