Arduino-baserad rad efterföljare robot med hjälp av Pololu kvart-8RC line sensor (3 / 3 steg)
Steg 3: Arduino 1.0-kod
Datum: februari 12, 2012
Baserat på exempelkod som tillhandahålls av Pololu.com
Kontakt: techbitar på gmail punkt com
#include < PololuQTRSensors.h >
#include < AFMotor.h >
AF_DCMotor motor1 (1, MOTOR12_8KHZ); STIFT 11 - skapa motor #1 pwm
AF_DCMotor motor2 (2, MOTOR12_8KHZ). PIN 3 - skapa motor #2 pwm
Ändra värdena nedan som passar din robotens motorer, vikt, hjul typ, 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 / / antal sensorer används
#define TIMEOUT 2500 / / väntar 2500 oss för sensor utgångar för att gå lågt
#define EMITTER_PIN 2 / / utsläppskälla styrs av digital stift 2
#define DEBUG 0 / / ange 1 om serial debug utmatning behövs
PololuQTRSensorsRC qtrrc ((osignerade 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 integrerad = 0;
void loop()
{
unsigned int sensorer [5].
int position = qtrrc.readLine(sensors);
int fel = position - 2000.
int motorSpeed = KP * fel + KD * (fel - lastError);
lastError = fel;
int leftMotorSpeed = M1_DEFAULT_SPEED + motorSpeed;
int rightMotorSpeed = M2_DEFAULT_SPEED - motorSpeed;
ställa in motor hastigheter med två varvtal variablerna ovan
set_motors (leftMotorSpeed, rightMotorSpeed);
}
void set_motors (int motor1speed, int motor2speed)
{
om (motor1speed > M1_MAX_SPEED) motor1speed = M1_MAX_SPEED; Limit toppfart
om (motor2speed > M2_MAX_SPEED) motor2speed = M2_MAX_SPEED; Limit toppfart
om (motor1speed < 0) motor1speed = 0; hålla motor ovan 0
om (motor2speed < 0) motor2speed = 0; hålla varvtal över 0
motor1.setSpeed(motor1speed); Ange varvtal
motor2.setSpeed(motor2speed); Ange varvtal
motor1.Run(forward);
motor2.Run(forward);
}
void manual_calibration() {
int i;
för (jag = 0; jag < 250; i ++) / / kalibrering tar några sekunder
{
qtrrc.Calibrate(QTR_EMITTERS_ON);
Delay(20);
}
om (DEBUG) {/ / om sant, generera sensor dats via seriell utgång
Serial.BEGIN(9600);
för (int jag = 0; jag < NUM_SENSORS; i ++)
{
Serial.Print(qtrrc.calibratedMinimumOn[i]);
Serial.Print(' ');
}
Serial.println();
för (int jag = 0; jag < NUM_SENSORS; i ++)
{
Serial.Print(qtrrc.calibratedMaximumOn[i]);
Serial.Print(' ');
}
Serial.println();
Serial.println();
}
}