Programma

Hier zal ik dieper ingaan over hoe het programma is opgebouwd. In het plan B wordt er zoals verteld enkel gebruikt gemaakt van een P-regeling daardoor zal ik pas later dieper ingaan over hoe de I-regeling en de D-regeling werken.

Eerst en vooral wordt er voor het programma gebruikt gemaakt van het EEPROM geheugen. Hierdoor moeten verschillende parameters niets steeds opnieuw ingegeven worden maar worden die opgeslagen ook al de spanning weg valt. Hiervoor wordt er gebruik gemaakt vna de bibliotheek "EEPROMAnything.h". Hieronder ziet u alle constanten die opgeslagen worden.

struct param_t
{
  unsigned long cycleTime;

  long white[6];
  long black[6];

  float kp;

  int power;
  float diff;
} params;

Zoals je hier wel uit kan afleiden zijn dit ook de constanten die aangepast kunnen worden tijdens het debuggen. Aan de hand van verschillende commando's kunnen deze parameters aangepast worden.

In het stukje code hieronder kan je ziet bij welke commando's je welke waarde kan aanpassen. Bijvoorbeeld "set cycle ##" veranderd de cycle time in de ingegeven waarde. "set debug on" zorgt ervoor dat het programma begint te lopen en dat de robot begint te rijden.

Dus er moeten serieel commando's ingelezen worden die de status vna de robot kan terug sturen, en dit zonder dat de rest van het programma blokeert en zonder dat de cycletime van het programma langer wordt.

Voor de seriële commandos wordt er gebruik gemaakt van volgnede bibliotheek "SerialCommand" waardoor het programmeer werk voor ons eenvoudiger wordt.

void onSet()
{
  char* param = sCmd.next();
  char* value = sCmd.next();

  if (strcmp(param, "debug") == 0)
  {
    if (strcmp(value, "on") == 0) debug = true;
    else if (strcmp(value, "off") == 0) debug = false;
  }
  else if (strcmp(param, "cycle") == 0) params.cycleTime = atol(value);
  else if (strcmp(param, "kp") == 0) params.kp = atof(value);
  else if (strcmp(param, "diff") == 0) params.diff = atof(value);
  else if (strcmp(param, "power") == 0) params.power = atol(value);
  
  EEPROM_writeAnything(0, params);
}

Natuurlijk is het ook mogelijk om de robot te kalibreren bij verschillend papier. Want de gelezen witte en zwarte waardes kunnen verschillend zijn bij verschillende lichtomstandigheden of bij verschillende afdrukkwaliteit.

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] = 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] = analogRead(sensor[i]);
    }
    SerialPort.println("done");
  }

  EEPROM_writeAnything(0, params);
  SerialPort.print("done!");
}

Een ander commando zorgt ervoor dat ik zie welke constanten er allemaal ingesteld zijn. Hiervoor moet er gewoon "debug" ingegeven worden en dit is dan de lijst die zichtbaar is. Als er een I en een D regelaar wordt toegevoegd zullen deze constanten ook in deze lijst zichtbaar worden.

In de loop wordt de positie van de robot berekend en hier wordt ook de P regeling toegepast. Dit houdt gewoon in dat de berekende error van de 0 positie (het midden) vermenigvuldigd wordt met de Kp waarde. Hierdoor kan je sterker of minder sterk gaan regelen.

void loop()
{
  sCmd.readSerial();

  unsigned long current = micros();
  if (current - previous >= params.cycleTime)
  {
    previous = current;

    for (int i = 0; i < 6; i ++)
    {
      long value = analogRead(sensor[i]);
      
      normalised[i] = map(value, params.black[i], params.white[i], 0, 1000);
    }

    int index = 0;
    for (int i = 1; i < 6; i++) if (normalised[i] < normalised[index]) index = i;

    if (index == 0) index = 1;
    else if (index == 5) index = 4; 

    long Szero = normalised[index];
    long SminusOne = normalised[index - 1];
    long SplusOne = normalised[index + 1];

    float b = ((float) (SplusOne - SminusOne)) / 2;
    float a = SplusOne - b - Szero;

    position = -b / (2 * a);
    position += index - 2.5;
    position *= 10;  /*sensor distance in mm*/
    error = 0 - position; /*error = setpoint - input*/
    output = error * params.kp;
    output = constrain(output, -510, 510);

    int powerLeft = 0;
    int powerRight = 0;
    
    if (debug)
    {
      if (output >= 0)
      {
        powerLeft = constrain(params.power + params.diff * output, -255, 255);
        powerRight = constrain(powerLeft - output, -255, 255);
        powerLeft = powerRight + output;
      }
      else
      {
        powerRight = constrain(params.power - params.diff * output, -255, 255);
        powerLeft = constrain(powerRight + output, -255, 255);
        powerRight = powerLeft - output;
      }

      analogWrite(AIN_1, powerLeft > 0 ? powerLeft : 0);
      analogWrite(AIN_2, powerLeft < 0 ? powerLeft : 0);
      analogWrite(BIN_1, powerRight > 0 ? powerRight : 0);
      analogWrite(BIN_2, powerRight < 0 ? powerRight : 0);
    }
 
  }

  unsigned long difference = micros() - current;
  if (difference > calculationTime) calculationTime = difference;
}

Wat je ook nog moet weten als je dit programma zou gebruiken is dat er gebruik gemaakt wordt van een HC-05 module voor de communicatie, deze werkt bij mij via COM4 op Arduino maar zelf kan je bij "Apparaatbeheer --> Poorten (COM & TPT)" vinden om welke COM-poort uw bluetooth module zich bevindt. Als je niet wil communiceren aan de hand vna uw computer kan er ook gebruikt gemaakt worden van een app op uw gsm waarmee u dezelfde commando's kan doorsturen (de Seriële commando's) als via uw computer. Een voorbeel van een app dat gebruikt kan worden is: "Serial Bluethooth Terminal".

Soms is het natuurlijk eenvoudiger om te debuggen aan de hand van een usb kabel. Als u dit wil doen moet u gewoon bij deze stap "Serial" zetten als u wil communiceren via usb. Of "Serial1" zetten als u wil communiceren via bluetooth.

#define SerialPort Serial1

Natuurlijk zijn er nog andere delen in het programma waar er hier niet dieper op ingegaan wordt. Het volledige programma kan terug gevonden worden bij de documentatie.

Na het nakijken van de code is er een fout gevonden. Namelijk bij het schrijven dan je waarde powerLeft en powerRight wordt er soms een negatieve waarde doorgestuurd, wat niet kan. Om dit te vermijden moeten er 2 mintekens toegevoegd worden in dit stukje code:

      analogWrite(AIN_1, powerLeft > 0 ? powerLeft : 0);
      analogWrite(AIN_2, powerLeft < 0 ? -powerLeft : 0);
      analogWrite(BIN_1, powerRight > 0 ? powerRight : 0);
      analogWrite(BIN_2, powerRight < 0 ? -powerRight : 0);

Als dit gebeurd is rijdt de robot veel vlotter en schokt deze niet meer in de bochten. De juiste code is nog steeds te vinden aan de hand van de link bij documentatie.

In deze video kan je het eindresultaat vinden.

In de les is er hier een snelheidsmeting op gedaan en hier kwam ik tot 0.368 m/s maar hier is er nog veel ruimte voor verbetering. Zeker als er een I en een D regeling wordt toegevoegd. Deze code kan u ook vinden in documentatie maar het resultaat hiervan is nooit gemeten. Maar de robot ging wel beduidend sneller.