PROGRAMMA

Arduino code met bluetooth:

De Arduino code kan gedownload worden in het technisch dossier

#include
#include “QTRSensors.h”
#include “SerialCommand.h”
#include
#include “EEPROMAnything.h”

#define SerialPort Serial1
#define Baudrate 38400
#define Ain1 9
#define Ain2 6
#define Bin1 5
#define Bin2 3

SerialCommand sCmd(SerialPort);
QTRSensors qtr;
bool debug;
unsigned long previous, calculationTime;
int BluetoothData;
float powerLeft;
float powerRight;
float output;
float sumError;
float rateError;
float lastError;

struct param_t
{
unsigned long cycleTime;
bool aan;
unsigned int power;
float kp;
float diff;
float ki;
float kd;

} params;

int positie;
int state;
const uint8_t SensorCount = 6;
uint16_t sensorValues[SensorCount];

void setup()
{

qtr.setTypeRC();
qtr.setSensorPins((const uint8_t[]){A5, A4, A3, A2, A1, A0}, SensorCount);
//qtr.setEmitterPin(2);

delay(500);
pinMode(LED_BUILTIN, OUTPUT);
digitalWrite(LED_BUILTIN, HIGH); // turn on Arduino’s LED to indicate we are in calibration mode

for (uint16_t i = 0; i < 400; i++)
{
qtr.calibrate();
}
digitalWrite(LED_BUILTIN, LOW); // turn off Arduino's LED to indicate we are through with calibration

Serial1.begin(Baudrate);
sCmd.addCommand("set", onSet);
sCmd.addCommand("debug", onDebug);
sCmd.addCommand("start",onStart);
sCmd.addCommand("stop",onStop);
sCmd.setDefaultHandler(onUnknownCommand);

EEPROM_readAnything(0, params);
// print the calibration minimum values measured when emitters were on
for (uint8_t i = 0; i < SensorCount; i++)
{
Serial.print(qtr.calibrationOn.minimum[i]);
//Serial.print(' ');
}
// Serial.println();

// print the calibration maximum values measured when emitters were on
for (uint8_t i = 0; i = params.cycleTime)
{
previous = current;
if (params.aan == true){
uint16_t position = qtr.readLineBlack(sensorValues);
positie = position-2500;
positie = positie / 100;

//SerialPort.print(“position: “);
// SerialPort.println(positie);

sumError += positie * params.cycleTime; //integraal
rateError = (positie – lastError)/params.cycleTime; //afgeleide
lastError = positie;

output = positie * params.kp + sumError * params.ki + rateError * params.kd;
output = constrain(output, -510,510);
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;
}
// SerialPort.println(powerRight);
// SerialPort.println(powerLeft);

//——–H-brug——–
if (powerRight<0)
{
analogWrite(Ain1, 0);
analogWrite(Ain2, -powerRight);
}else{
analogWrite(Ain1, powerRight);
analogWrite(Ain2, 0);
}
if (powerLeft calculationTime) calculationTime = difference;

}

void onSet()
{
char* param = sCmd.next();
char* value = sCmd.next();
if (strcmp(param, “cycle”) == 0) params.cycleTime = atol(value); // atol -> tekst omzetten naar long
//if (strcmp(param, “getal”) == 0) params.getal = atoi(value); //atoi -> tekst omzetten in integer
if (strcmp(param, “power”) == 0) params.power = atoi(value);
if (strcmp(param, “diff”) == 0) params.diff = atof(value);
if (strcmp(param, “kp”) == 0) params.kp = atof(value);
if (strcmp(param, “ki”) == 0) params.ki = atof(value);
if (strcmp(param, “kd”) == 0) params.kd = atof(value);
EEPROM_writeAnything(0, params);
}

void onDebug()
{

SerialPort.print(“cycle time: “);
SerialPort.println(params.cycleTime);

SerialPort.print(“kp: “);
SerialPort.println(params.kp);

SerialPort.print(“ki: “);
SerialPort.println(params.ki);

SerialPort.print(“kd: “);
SerialPort.println(params.kd);

SerialPort.print(“power (PWM): “);
SerialPort.println(params.power);

SerialPort.print(“diff: “);
SerialPort.println(params.diff);

SerialPort.print(“status: “);
SerialPort.println(params.aan);

SerialPort.print(“calculation time: “);
SerialPort.println(calculationTime);
calculationTime = 0;

}
void onStart()
{
params.aan = true;
}
void onStop()
{
params.aan = false;
analogWrite(Ain1, 0);
analogWrite(Ain2, 0);
analogWrite(Bin1, 0);
analogWrite(Bin2, 0);
}
void onUnknownCommand(char *command)
{
SerialPort.print(“unknown command: \””);
SerialPort.print(command);
SerialPort.println(“\””);
}