Arduino gebaseerde lijn volgeling robot met behulp van Pololu kW-8RC lijn sensor (3 / 3 stap)

Stap 3: De code van de Arduino 1.0

#include < PololuQTRSensors.h >
#include < AFMotor.h >

AF_DCMotor motor1 (1, MOTOR12_8KHZ); PIN 11 - Maak motor #1 pwm
AF_DCMotor motor2 (2, MOTOR12_8KHZ); PIN 3 - Maak motor #2 pwm

Wijzig de waarden hieronder om aan te passen van uw robot motoren, gewicht, type wiel, enz.
#define KP.2
#define KD 5
#define M1_DEFAULT_SPEED 50
#define M2_DEFAULT_SPEED 50
#define M1_MAX_SPEED 70
#define M2_MAX_SPEED 70
#define MIDDLE_SENSOR 3
#define NUM_SENSORS 5 / / aantal sensoren gebruikt
#define TIMEOUT 2500 / / wacht 2500 ons voor sensor output ga laag
#define EMITTER_PIN 2 / / emitter wordt gecontroleerd door digitale pin 2
#define DEBUG 0 / / ingesteld op 1 als seriële debug output nodig

PololuQTRSensorsRC qtrrc ((niet-ondertekende char[]) {18,17,16,15,14}, NUM_SENSORS, TIMEOUT, EMITTER_PIN);

unsigned int sensorValues [NUM_SENSORS];

VOID Setup
{
delay(1000);
manual_calibration();
set_motors(0,0);
}

int lastError = 0;
int last_proportional = 0;
int integraal = 0;

void loop
{
unsigned int sensoren [5];
int positie = qtrrc.readLine(sensors);
int fout = positie - 2000;

int motorSpeed = KP * fout + KD * (fout - lastError);
lastError = fout;

int leftMotorSpeed = M1_DEFAULT_SPEED + motorSpeed;
int rightMotorSpeed = M2_DEFAULT_SPEED - motorSpeed;

motor snelheden met behulp van de twee bovenstaande motorsnelheid-variabelen instellen
set_motors (leftMotorSpeed, rightMotorSpeed);
}

VOID set_motors (int motor1speed, int motor2speed)
{
Als (motor1speed > M1_MAX_SPEED) motor1speed = M1_MAX_SPEED; limiet topsnelheid
Als (motor2speed > M2_MAX_SPEED) motor2speed = M2_MAX_SPEED; limiet topsnelheid
Als (motor1speed < 0) motor1speed = 0; motor boven 0 houden
Als (motor2speed < 0) motor2speed = 0; Houd motor snelheid groter dan 0
motor1.setSpeed(motor1speed);     instellen van de motorsnelheid
Motor2.setSpeed(motor2speed);     instellen van de motorsnelheid
motor1.run(forward);
Motor2.run(forward);
}

ongeldig manual_calibration() {}

int i;
voor (ik = 0; ik < 250; i ++) / / de kalibratie duurt een paar seconden
{
qtrrc.Calibrate(QTR_EMITTERS_ON);
vertraging(20);
}

Als (DEBUG) {/ / als de waarde true, generate sensor dats via uitgang voor seriële
Serial.begin(9600);
for (int i = 0; ik < NUM_SENSORS; i ++)
{
Serial.Print(qtrrc.calibratedMinimumOn[i]);
Serial.Print(' ');
}
Serial.println();

for (int i = 0; ik < NUM_SENSORS; i ++)
{
Serial.Print(qtrrc.calibratedMaximumOn[i]);
Serial.Print(' ');
}
Serial.println();
Serial.println();
}
}