line follower/PID

Hardware geleverd door Arduino
Berichten: 4
Geregistreerd: 24 Nov 2012, 13:00

line follower/PID

Berichtdoor kelly.vanloon » 07 Jan 2013, 11:00

Hallo

ik heb een code voor een line follower, het is dus de bedoeling dat mijn robot de zwarte lijn volgt, maar hij stopt namelijk bij zen zwarte lijn ! Weet iemand aan wat dat kan liegen: montage sensor, code of .... Kan iemand eens kijken naar mijn code en me helpen wat er verkeerd zou zijn.

Alvast bedankt.

#include <QTRSensors.h>
#include <AFMotor.h>

AF_DCMotor motor1(1, MOTOR12_64KHZ);
AF_DCMotor motor2(2, MOTOR12_64KHZ);

#define KP .2
#define KD 5
#define M1_DEFAULT_SPEED 50
#define M2_DEFAULT_SPEED 50
#define M1_MAX_SPEED 255
#define M2_MAX_SPEED 255
#define MIDDLE_SENSOR 3
#define NUM_SENSORS 5
#define TIMEOUT 2500
#define EMITTER_PIN 2 // emitter is controlled by digital pin 2
#define DEBUG 0

QTRSensorsRC qtrrc((unsigned char[]) { 4,5,6,7,8} ,NUM_SENSORS, TIMEOUT, EMITTER_PIN);

unsigned int sensorValues[NUM_SENSORS];

void setup() // voorbereiding
{
delay(1000); // 1000 miliseconden
manual_calibration(); // De methode wordt opgeroepen
set_motors(0,0); // De methode wordt opgeroepen
}

int lastError = 0;
int last_proportional = 0;
int integral = 0;


void loop()
{
unsigned int sensors[5];
// Het aantal sensors definiëren
int position = qtrrc.readLine(sensors);
// 0 tot 5000, waarbij 0 betekent direct onder de sensor of de lijn werd verloren
int error = position - 2000;
// error = positie - 2000
int motorSpeed = KP * error + KD * (error - lastError);
// De snelheid van de motor is gelijk aan 0,2 * error + 5 * (error - lastError)
lastError = error;
// De lastError = error

int leftMotorSpeed = M1_DEFAULT_SPEED + motorSpeed;
// linkermotor = 150 + motorspeed
int rightMotorSpeed = M2_DEFAULT_SPEED - motorSpeed;
// rechtermotor = 150 + motorspeed

set_motors(leftMotorSpeed, rightMotorSpeed);
// Zet de motor snelheid op (linkermotorsnelheid, rechtermotorsnelheid)
}

void set_motors(int motor1speed, int motor2speed)
{
if (motor1speed > M1_MAX_SPEED ) motor1speed = M1_MAX_SPEED;
// limiet van de snelheid van motor 1
if (motor2speed > M2_MAX_SPEED ) motor2speed = M2_MAX_SPEED;
// limiet van de snelheid van motor 2
if (motor1speed < 0) motor1speed = 0;
// Houd motor 1 boven de 0
if (motor2speed < 0) motor2speed = 0;
// Houd motor 2 boven de 0
motor1.setSpeed(motor1speed);
// Zet de motorsnelheid van motor 1 om in rotatie per minuut
motor2.setSpeed(motor2speed);
// Zet de motorsnelheid van motor 2 om in rotatie per minuut
motor1.run(FORWARD);
// Laat de motor 1 voorwaarts rijden
motor2.run(FORWARD);
// laat de motor 2 voorwaarts rijden
}


void manual_calibration() {
int i;
for (i = 0; i < 250; i++)
// de kalibratie neemt enkele seconden
{
qtrrc.calibrate(QTR_EMITTERS_ON);
// Het zet de emitter pin aan
delay(20);
// met een tussenstop van 20 miliseconden
}
if (DEBUG) {
// Als het juist is, zendt de sensor de data naar de seriele output
Serial.begin(9600);
for (int i = 0; i < NUM_SENSORS; i++)
// verhoog i, zolang i kleiner is dan het aantal sensors
{
Serial.print(qtrrc.calibratedMinimumOn[i]);
// print de kalibratie tot een minimum bij een emitter pin die on is, bij sensor i
Serial.print(' ');
// daarna een lege print
}
Serial.println();
// print data naar de seriele output, als menselijke taal via de ASCII tabel
for (int i = 0; i < NUM_SENSORS; i++)
// verhoog i, zolang i kleiner is dan het aantal sensors
{
Serial.print(qtrrc.calibratedMaximumOn[i]);
// print de kalibratie tot een maximum bij een emitter pin die on is, bij sensor i
Serial.print(' ');
// daarna een lege print
}
Serial.println();
// print data naar de seriele output, als menselijke taal via de ASCII tabel
Serial.println();
// print data naar de seriele output, als menselijke taal via de ASCII tabel
}
}

Advertisement

Terug naar Arduino hardware

Wie is er online?

Gebruikers in dit forum: Geen geregistreerde gebruikers en 16 gasten