Om de robot een lijn te laten volgen maak ik gebruik van twee LDR's. Een LDR heeft een bepaalde weerstand bij een bepaalde lichte intensiteit. Dit is dan een input waarde voor het regel proces. Hieronder zijn de afbeeldingen te zien die ik heb geschetst om tot mijn algoritme te komen.
 |
| Hier een afbeelding van het proces analyse |
 |
| Hier nog een |
 |
| Hier de eerste op set van een PSD |
 |
| Hier een schematische schets van de opstelling van de LED's en LDR's |
 |
| Hier een gedetailleerde uitwerking van een PSD |
 |
| Hier op deze afbeelding een test opstelling geschetst van hoe ik de Line tracking kan testen. |
 |
| Hier op de afbeelding een proces functie blok schets. |
Resultaat
Hieronder het C++ programma.
/*
|--------------------------------------------------------------------------|
| Naam : Mark. |
| Project : line tracking robot "Tommy". |
| Programma : PI regeling line tracking robot. |
| Versie : 3.1 |
| Datum : 03-05-2013 |
---------------------------------------------------------------------------|
*/
int motor_rechts = 6; // motor rechts uitgang op arduino.
int motor_links = 9; // motor links uitgang op arduino.
int sensor_links = A1; // Sensor aan de linker kant van de robot.
int sensor_rechts = A0; // Sensor aan de rechter kant van de robot.
int led = 8; // Deze uitgang schakeld de LEDs aan van de sensoren.
unsigned long last_time; // tijd opname na meting.
unsigned long now; // de tijd op dit moment.
double timechange; // Het verschil tussen now en last time.
double M_correctie; // motor correctie snelheid.
float Kp = 0.2 ; // versterkings factor van de Propotionele versterking
float Ki = 0.1; // versterkings factor van de integratie versterking
int Error =0; // De fout die ontstaat tussen de PV en SP.
int MotorspeedLN = 0; // Nieuwe motor snelheid links met correctie factor.
int motorspeed_links = 60; // Ingesteld motor snelheid links kan ingesteld worden tussen de 0 en 255.
int MotorspeedRN = 0; // Nieuwe motor snelheid rechts met correctie factor.
int motorspeed_rechts = 60; // Ingesteld motor snelheid links kan ingesteld worden tussen de 0 en 255.
int motorspeed_links_opstart = 100; // Opstart snelheid om aanloopkoppel te overwinnen.
int motorspeed_rechts_opstart = 100; // Opstart snelheid om aanloopkoppel te overwinnen.
int ref = 123 ; // ingesteld referentie waarde. Dit is de waarde kleur van de lijn.
float S_links;
float S_total;
float S_afwijking;
float S_rechts;
void setup()
{
pinMode (motor_links, OUTPUT);
pinMode (motor_rechts, OUTPUT);
pinMode (led, OUTPUT);
pinMode (sensor_links, INPUT);
pinMode (sensor_rechts, INPUT);
Serial.begin (9600);
digitalWrite (8,led);
delay (4000);
analogWrite (9, motorspeed_links_opstart);
analogWrite (6, motorspeed_rechts_opstart);
delay (1000);
}
void loop()
{
double M_Correctie_factor = 0;
double M_correctie = 0;
analogWrite (9, motorspeed_links);
analogWrite ( 6, motorspeed_rechts);
do // in deze do while loop wordt de afwijking bepaald, of de robot zich naar links verplaats of naar rechts.
{
S_links = analogRead (sensor_links);
S_rechts = analogRead (sensor_rechts);
S_total = -S_links + S_rechts;
S_afwijking = 0-S_total;
{
if (S_afwijking > 0 )
{
do // in deze do while loop verplaatst de robot zicht naar links. dan wordt het PI algoritme ingeschakeld
{
S_links = analogRead (sensor_links);
Error = S_links - ref;
M_correctie = Kp * Error + Ki * Error;
MotorspeedLN = motorspeed_links + M_correctie;
MotorspeedRN = motorspeed_rechts;
analogWrite (6, MotorspeedRN);
analogWrite (9, MotorspeedLN);
S_links = analogRead (sensor_links);
S_rechts = analogRead (sensor_rechts);
Serial.print (" L ");
Serial.print (" ");
Serial.print (S_links);
Serial.print (" ");
Serial.print (Error);
Serial.print (" ");
Serial.print (timechange);
Serial.print (" ");
Serial.print (M_correctie);
Serial.print (" ");
Serial.print (MotorspeedLN);
Serial.print (" ");
Serial.println (MotorspeedRN);
S_rechts = analogRead (sensor_rechts);
S_links = analogRead (sensor_links);
last_time = now;
}
while ( S_rechts <= ref);
}
else
{
do // in deze do while loop verplaats de robot zich naar rechts. Hier wordt een algoritme uitgevoerd die afwijking te corrigeren.
{
S_rechts = analogRead (sensor_rechts);
S_links = analogRead (sensor_links);
S_rechts = (S_links/S_rechts)*S_rechts;
Error = S_rechts - ref;
M_correctie = Kp * Error + Ki * Error;
MotorspeedRN = motorspeed_rechts + M_correctie;
MotorspeedLN = motorspeed_links;
analogWrite (6, MotorspeedRN);
analogWrite (9, MotorspeedLN);
Serial.print (" R ");
Serial.print (" ");
Serial.print (S_rechts);
Serial.print (" ");
Serial.print (Error);
Serial.print (" ");
Serial.print (timechange);
Serial.print (" ");
Serial.print (M_correctie);
Serial.print (" ");
Serial.print (MotorspeedLN);
Serial.print (" ");
Serial.println (MotorspeedRN);
S_rechts = analogRead (sensor_rechts)* 0,95;
S_links = analogRead (sensor_links);
last_time = now;
}
while ( S_links <= ref);
}
}
}
while ( S_afwijking == 0);
Serial.println ("geen afwijking");
}
Geen opmerkingen:
Een reactie posten