Linefollower.

Voor het laatste jaar van de bachelor elektromechanica afstudeerrichting automatisering is het de bedoeling om als synthese project een linefollower (letterlijk lijn volger robot) te bouwen en deze minstens 3 toeren te laten rijden op een parkour.

Het is de bedoeling dat we een plan A en een plan B maken. Voor het plan A is het de bedoeling dat we een pcb zelf ontwerpen en solderen. Dit heb ik gedaan via het online programma easyeda. De pcb heb ik laten maken in china via dezelfde website maar door een soldeer fout is het niet gelukt om dit te kunnen maken. Voor plan B is het de bedoeling om met modules en mbv een arduino de linefollower bouwen.

Voor plan A

Zie excel sheet BOM.

en pcb gemaakt via easyeda

gerberfile van de pcb is te vinden in het asbuild dosier

Voor plan B

(1x) arduino leonardo

(1x) qtr-8A board van polulu

(1x) 2S lipo batterij

(1x) HC05 bleutooth module van velleman

(1x) drv8833

(2x) 30:1 motors van polulu

(∞x) dupont wires

Proof of concept:

Dit is een programma die zou moeten aantonen dat alles werkt afzonderlijk

Dit werd vaak gebruikt om te zien of alle componenten nog deftig werken na bijvoorbeeld een aanpassing aan de linfollower zelf

#include <SoftwareSerial.h>

SoftwareSerial Genotronex(10,11); //eventueel veranderen
int vooruitA = 3;
int achteruitA = 5;
int vooruitB = 6;
int achteruitB = 9;
int BluetoothData;
int Knop = 7;
int state = 1;
int ledPin = 12;


void setup() {
  Genotronex.begin(9600);
  Serial.begin(9600);
  Genotronex.println("BL on PLease press 1 or 2 blink LED..");
  pinMode(vooruitA, OUTPUT);
  pinMode(achteruitA, OUTPUT);
  pinMode(vooruitB, OUTPUT);
  pinMode(achteruitB, OUTPUT);
  pinMode(ledPin, OUTPUT);
  pinMode(Knop, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(Knop), interrupt, RISING);
}

void loop() {
  
  digitalWrite(ledPin, state);
  if (Genotronex.available() && state == HIGH){
    BluetoothData = Genotronex.read();
    if(BluetoothData== '1'){
      Allesuit();
      digitalWrite(vooruitA, HIGH);
    }
    if(BluetoothData=='2'){
      Allesuit();
      digitalWrite(vooruitB, HIGH);
    }
    if(BluetoothData=='a'){
      Allesuit();
      digitalWrite(achteruitA, HIGH);
      digitalWrite(achteruitB, HIGH);
    }
    if(BluetoothData=='v'){
      Allesuit();
      digitalWrite(vooruitA, HIGH);
      digitalWrite(vooruitB, HIGH);
    }
    if(BluetoothData=='l'){
      digitalWrite(ledPin, HIGH); 
    }
    if(BluetoothData== '0'){
      Allesuit();
      digitalWrite(ledPin, LOW);
    }
    if(BluetoothData== 'g'){
      getSensor();
    }
  }
  if (state == LOW){
    Allesuit();     
  }
}
void interrupt() {
  state = !state;
  delay(500);
}

int Allesuit(){
  for (int i=2; i<10;i++){
     digitalWrite(i, LOW);
  }

}
int getSensor(){
   int val0 = analogRead(A0);
   Genotronex.print("A0 : ");
   Genotronex.println(val0);
   
   int val1 = analogRead(A1);
   Genotronex.print("A1 : ");
   Genotronex.println(val1);
   
   int val2 = analogRead(A2);
   Genotronex.print("A2 : ");
   Genotronex.println(val2);
   
   int val3 = analogRead(A3);
   Genotronex.print("A3 : ");
   Genotronex.println(val3);
   
   int val4 = analogRead(A4);
   Genotronex.print("A4 : ");
   Genotronex.println(val4);
   
   int val5 = analogRead(A5);
   Genotronex.print("A5 : ");
   Genotronex.println(val5);
  }

Code van het project zelf:

#include "SerialCommand.h"
#include "EEPROMAnything.h"

#include <SoftwareSerial.h>

SoftwareSerial BTSerial(10, 11);
#define SerialPort  BTSerial   // RX | TX
#define Baudrate 9600
//#define SerialPort Serial
//#define Baudrate 115200

//motoren 
int vooruitRechts = 3;
int achteruitRechts = 5;
int vooruitLinks = 6;
int achteruitLinks = 9;


SerialCommand sCmd(SerialPort);

bool debug = false;
bool run = false;

unsigned long previous, calculationTime;
//analoge poorten in een constante 
const int sensor[] = {A0, A1, A2, A3, A4, A5};

int ledOn = 2;
//alle gegeven die we willen dat onthouden worden op het moment dat er een reset komt
struct param_t
{
  unsigned long cycleTime;
  //hier kunnen nog andere gegevens opgeslagen worden in het programma
  int black[6];
  int white[6];
  int power;
  float diff;
  float kp;
  int maxPower;
}params;
String BluetoothData;
void setup()
{
  SerialPort.begin(Baudrate);
  //eerste commando toevoegen
  //als hij een woord niet herkend
  sCmd.setDefaultHandler(onUnknownCommand);
  sCmd.addCommand("debug", onDebug);
  sCmd.addCommand("set", onSet);
  sCmd.addCommand("calibrate",onCalibrate);
  sCmd.addCommand("run",onRun);
  sCmd.addCommand("stop",onStop);
  SerialPort.println("ready");

  EEPROM_readAnything(0, params);

  //motoren
  pinMode(vooruitRechts, OUTPUT);
  pinMode(achteruitRechts, OUTPUT);
  pinMode(vooruitLinks, OUTPUT);
  pinMode(achteruitLinks, OUTPUT);
  
}
int normalised[6];
float position;
int x;
void loop()
{
  //loopfunctie niet blokkeren tot het juiste karakter gelezen wordt
  sCmd.readSerial();
  //nonblocking wait met funcie micros
  unsigned long current = micros();
  

  //een if functie blokkeerd niet vandaar dat er geen wait is 
  if ((current - previous) > params.cycleTime)
  {
    //code die cyclisch moet uitgevoerd worden
    previous = current;

    
    
    for(int i = 0; i< 6; i++){
      
      normalised[i] = map( analogRead(sensor[i]), params.black[i], params.white[i], 1000, 0); 
      
    }
    
    //SerialPort.println();
    int index  =  0;
    for(int i  = 1; i < 6;i++) if (normalised[i]<normalised[index]) index  = i;

    if (normalised[index] > 800) run = false;
    
    if (index == 0){
      position = -25;
    }
    else if (index == 5){
      position = 25;
    }
    else{
      int sNul = normalised[index];
      int sMinEen = normalised[index-1];
      int sPlusEen = normalised[index+1];
      float b = (sPlusEen - sMinEen);
      b = b/2;
      float a = (sPlusEen - b - sNul);

      position = -b/(2*a);
      
      position += index;
      position -= 2.5;

      position *= 15; // moet aangepst worden doordat de afstand tussen de sensoren op de pcb robot iets groter/kleiner is
    }
    float error = -position;
    float output = error * params.kp; //in debug zetten
    

    output = constrain(output, -510, 510);// dit zijn maximumwaardes
    //hier wordt de robot mee aangestuur dit kan niet meer zijn dan 255 en niet kleiner dan 0

    int powerLeft = 0;
    int powerRight = 0;
    /*
    SerialPort.print("output");
    SerialPort.println(output);
    SerialPort.print("error: ");
    SerialPort.println(error);
    */

    if (run == true){
      if (output >= 0){ 
        powerRight = constrain(params.power + params.diff * output, -255, 255);
        powerLeft = constrain(powerRight - output, -255, 255);
        powerRight = powerLeft + output;

        
      }
      else{
        powerLeft = constrain(params.power - params.diff * output, -255, 255);
        powerRight = constrain(powerLeft + output, -255, 255);
        powerLeft = powerRight - output;
      }
    }
    //motoren aansturen
    analogWrite(vooruitLinks, powerLeft > 0 ? powerLeft : 0);
    analogWrite(achteruitLinks, powerLeft < 0 ? -powerLeft : 0);
    analogWrite(vooruitRechts, powerRight > 0 ? powerRight : 0);
    analogWrite(achteruitRechts, powerRight < 0 ? -powerRight : 0);
    /*
    SerialPort.print("powerLeft: ");
    SerialPort.println(powerLeft);
    SerialPort.print("powerRight");
    SerialPort.println(powerRight);
    SerialPort.println();
     */
    //cycletime berekenen
    unsigned long difference = micros() - current;
    if (difference > calculationTime) calculationTime = difference;
  }
}

void onUnknownCommand(char* command){
  SerialPort.print("Unknown comman: \"");
  SerialPort.print(command);
  SerialPort.println("\"");
}

//nieuwe functie
void onDebug(){
  //SerialPort.println("set command");
  //in set de argumenten uitlezen daavoor hebben we de functie sCmd.next();
    //snelheid tonen
    showVal();
  
   //waardes tonen
    SerialPort.print("black Values: ");
    for(int i = 0; i< 6; i++){
       SerialPort.print(params.black[i]);
       SerialPort.print(" ");
    }
    SerialPort.println();
    SerialPort.print("white Values: ");
    for(int i = 0; i< 6; i++){
       SerialPort.print(params.white[i]);
       SerialPort.print(" ");
    }
    //positie tonen
    SerialPort.println();
    SerialPort.print("positie: ");
    SerialPort.println(position);
    SerialPort.print("Values: ");
    for(int i = 0; i< 6; i++){
      SerialPort.print(normalised[i]); //normalised
      SerialPort.print(" ");
    }
    SerialPort.println();
    SerialPort.print("power: ");
    SerialPort.print(params.power);
    SerialPort.println();
    SerialPort.print("diff: ");
    SerialPort.print(params.diff);
    SerialPort.println();
    SerialPort.print("kp: " );
    SerialPort.print(params.kp);
    SerialPort.println();
    SerialPort.print("calculation time: ");
    SerialPort.println(calculationTime);
    calculationTime = 0;
    SerialPort.println(run);
    SerialPort.println(params.maxPower);
    SerialPort.println("----------------------------");
    
  
  
  EEPROM_writeAnything(0,params);
}
void onSet(){
  char*param = sCmd.next();
  char*value = sCmd.next();
  if (strcmp(param, "cycle") == 0) params.cycleTime = atol(value);// om getallen in te lezen
  else if (strcmp(param, "power") == 0) params.power = atol(value);
  else if (strcmp(param, "diff") == 0) params.diff = atof(value);
  else if (strcmp(param, "kp") == 0) params.kp = atof(value);
  else if (strcmp(param, "maxpower") == 0) params.maxPower= atol(value);
}
void onCalibrate()
{
  char* param = sCmd.next();

  if (strcmp(param, "black") == 0)
  {
    SerialPort.print("start calibrating black... ");
    for (int i = 0; i < 6; i++) params.black[i]=map(analogRead(sensor[i]),1023,0,1000,0); //hier is het veranderd analogRead(sensor[i]);
    
    SerialPort.println("done");
  }
  else if (strcmp(param, "white") == 0)
  {
    SerialPort.print("start calibrating white... ");    
    for (int i = 0; i < 6; i++) params.white[i]=map(analogRead(sensor[i]),1023,0,0,1000);  
    SerialPort.println("done");         
  }
  EEPROM_writeAnything(0, params);
}
String showVal(){
  SerialPort.print("cycletime : ");
  SerialPort.println(params.cycleTime);
   
}
void onRun(){
  run = true;
}
void onStop(){
  run = false;
}

Zoals in de inleiding gezegd is dit niet gelukt er is een verbinding aan de drv chip verkeerd gesoldeerd dit zou in principe nog mogelijk zijn om op te lossen maar door gebrek aan tijd heb ik dit niet meer kunnen doen, het is wel de bedoeling om dit in de toekomst nog op te lossen.

Hier vind je alle documenten die bij het project behoren die kun je in de .zip file hieronder halen zowel voor plan A als plan B.