Basados en Arduino robot velocista QTR-8RC Pololu sensor de línea (3 / 3 paso)

Paso 3: El Arduino código 1.0

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

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

Cambiar los valores siguientes para todos los motores de tu robot, peso, tipo de rueda, 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 / / número de sensores utilizados
#define espera 2500 / / espera para 2500 para sensor salidas para ir bajo
#define EMITTER_PIN 2 / / emisor es controlado por el pin digital 2
#define DEBUG 0 / / el valor 1 si es necesario el resultado de la depuración serie

PololuQTRSensorsRC qtrrc ((unsigned char[]) {18,17,16,15,14}, NUM_SENSORS, tiempo de espera, EMITTER_PIN);

unsigned int sensorValues [NUM_SENSORS];

void setup()
{
Delay(1000);
manual_calibration();
set_motors(0,0);
}

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

void loop()
{
sensores unsigned int [5];
int posición = qtrrc.readLine(sensors);
int error = posición - 2000;

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

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

conjunto motor velocidades usando las dos variables de velocidad del motor por encima
set_motors (leftMotorSpeed, rightMotorSpeed);
}

void set_motors (int motor1speed, int motor2speed)
{
Si (motor1speed > M1_MAX_SPEED) motor1speed = M1_MAX_SPEED; límite de velocidad máxima
Si (motor2speed > M2_MAX_SPEED) motor2speed = M2_MAX_SPEED; límite de velocidad máxima
Si motor1speed (motor1speed < 0) = 0; mantener el motor por encima de 0
Si motor2speed (motor2speed < 0) = 0; mantener la velocidad del motor por encima de 0
motor1.SETSPEED(motor1speed);     ajustar la velocidad del motor
motor2.SETSPEED(motor2speed);     ajustar la velocidad del motor
motor1.Run(Forward);
motor2.Run(Forward);
}

void manual_calibration() {}

int i;
para (i = 0; i < 250; i ++) / / la calibración llevará unos segundos
{
qtrrc.Calibrate(QTR_EMITTERS_ON);
Delay(20);
}

Si (depurar) {/ / si es cierto, generar DAT sensor vía salida serie
Serial.Begin(9600);
para (int i = 0; i < NUM_SENSORS; i ++)
{
Serial.Print(qtrrc.calibratedMinimumOn[i]);
Serial.Print(' ');
}
Serial.println();

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