Robot suiveur de ligne axée sur l’Arduino à l’aide du capteur de ligne Pololu QTR-8RC (3 / 3 étapes)

Étape 3: L’Arduino code 1.0

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

AF_DCMotor motor1 (1, MOTOR12_8KHZ) ; BROCHE 11 – créer moteur #1 pwm
AF_DCMotor motor2 (2, MOTOR12_8KHZ) ; BROCHE 3 - créer moteur pwm #2

Modifiez les valeurs ci-dessous en fonction des moteurs de votre robot, poids, type de roue, etc..
#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 / / nombre de capteurs utilisés
#define TIMEOUT 2500 / / attend 2500 nous pour capteur sorties pour aller en bas
#define EMITTER_PIN 2 / / émetteur est contrôlée par la broche numérique 2
#define DEBUG 0 / / la valeur 1 si nécessaire de la sortie de débogage serial

PololuQTRSensorsRC qtrrc ((unsigned 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) ;
}

lastError int = 0 ;
int last_proportional = 0 ;
intégrante d’int = 0 ;

void loop()
{
unsigned int capteurs [5] ;
int position = qtrrc.readLine(sensors) ;
int erreur = position - 2000 ;

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

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

régler les vitesses du moteur en utilisant les deux variables de vitesse du moteur ci-dessus
set_motors (leftMotorSpeed, rightMotorSpeed) ;
}

void set_motors (int motor1speed, int motor2speed)
{
Si (motor1speed > M1_MAX_SPEED) motor1speed = M1_MAX_SPEED ; limite de vitesse maximale
Si (motor2speed > M2_MAX_SPEED) motor2speed = M2_MAX_SPEED ; limite de vitesse maximale
Si (motor1speed < 0) motor1speed = 0 ; garder le moteur au-dessus de 0
Si (motor2speed < 0) motor2speed = 0 ; garder la vitesse du moteur au-dessus de 0
motor1.setSpeed(motor1speed) ;     régler la vitesse du moteur
motor2.setSpeed(motor2speed) ;     régler la vitesse du moteur
motor1.Run(Forward) ;
motor2.Run(Forward) ;
}

void manual_calibration() {}

int i ;
pour (i = 0; j’ai 250 <; i ++) / / l’étalonnage prendra quelques secondes
{
qtrrc.CALIBRATE(QTR_EMITTERS_ON) ;
Delay(20) ;
}

Si (déboguer) {/ / si elle est vraie, générer capteur dats via sortie série
Serial.Begin(9600) ;
pour (int i = 0; i < NUM_SENSORS; i ++)
{
Serial.Print(qtrrc.calibratedMinimumOn[i]) ;
Serial.Print(' ') ;
}
Serial.println() ;

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