Hello, I want to realize a linefollowing tank bot. I got this following parts: - Arduino Mega 2560 - Arduino MotorShield Rev03 - Pololu QTR-8RC Array - Pololu RP5 Tankchassis (6V DC Motors) Now all parts are together and the code must be completed. Therefore I found a really nice PID-based tutorial for linefollower: http://letsmakerobots.com/blog/enigmerald/pid-tutorials-line-following I create a program, but after calibrating the sensors, the bot doesn't drive on the line. It makes no sense were it drives. Most time it do 90° turns and so on. Do you find a better solution for my code? [code] #include #include /////////////////////////// // define constants // /////////////////////////// #define AUTO_CALIB false // Autokalibrierung aktivieren #define DEBUG_ENABLE true // false=keine Serial Ausgaben #define NUM_SENSORS 8 // Anzahl der verwendeten Sensoren #define TIMEOUT 2500 // waits for 2500 microseconds for sensor outputs to go low #define EMITTER_PIN QTR_NO_EMITTER_PIN // Default: kein Emitterpin definiert #define MOTOR_SPEED 100 // PWM Signal 0-255 (Basismotorgeschwindigkeit) #define MOTOR_MAX_SPEED 255 // PWM Signal für Maximalgeschwindigkeit #define KP 0.2 // KP-Wert für PID Regler #define KD 5 // KD-Wert für PID Regler ( Beachte: Kp < Kd) #define SERVO_PIN1 11 // PWM Ausgang für Servo1 #define SERVO_PIN2 12 // PWM Ausgang für Servo2 #define CALIB_LED 13 // LED Ausgang für Kalibrierungsphase ////////////////////////// // define variables // ////////////////////////// // Zuweisung der Sensoren 0-7 an die digitalen IO's 22-29 QTRSensorsRC qtrrc((unsigned char[]) {22, 23, 24, 25, 26, 27, 28, 29}, NUM_SENSORS, TIMEOUT, EMITTER_PIN); unsigned int sensorValues[NUM_SENSORS]; int MotorLeftSpeed=3; // PWM pin for speed control left int MotorRightSpeed=11; // PWM pin for speed control right int MotorLeftDirection=12; // digital pin for direction control left int MotorRightDirection=13; // digital pin for direction control right int MotorLeftBrake=9; // digital pin for brake control left int MotorRightBrake=8; // digital pin for brake control right int lastError = 0; ///////////////////// // Initialisierung // ///////////////////// void setup() { // Initialisierung verschiedner IOs pinMode(CALIB_LED, OUTPUT); // Definiert LED Ausgang für Kalibierungsphase // Initialisierung linker Motor pinMode(MotorLeftDirection, OUTPUT); // Definiert Ausgang für den linken Motor pinMode(MotorLeftBrake, OUTPUT); // Definiert Bremse für den linken Motor pinMode(MotorLeftSpeed, OUTPUT); // Definiert Geschwindigkeitsausgang für linken Motor // Initialisierung rechter Motor pinMode(MotorRightDirection, OUTPUT); // Definiert Ausgang für den rechten Motor pinMode(MotorRightBrake, OUTPUT); // Definiert Bremse für den rechten Motor // Initialisierung Servo Motoren servo1.attach(SERVO_PIN1); // Zuweisung PWM PIN für Servo1 servo2.attach(SERVO_PIN2); // Zuweisung PWM PIN für Servo2 if (DEBUG_ENABLE) { Serial.begin(9600); Serial.print("Debug Modus aktiviert:\n"); } delay(500); digitalWrite(CALIB_LED, HIGH); // turn on Arduino's LED to indicate we are in calibration mode for (int i = 0; i < 400; i++) // make the calibration take about 10 seconds { qtrrc.calibrate(); // reads all sensors 10 times at 2500 us per read (i.e. ~25 ms per call) } delay(20); // The 20 ms delay in the calibration code is intended to make the routine take a reasonable amount of time. digitalWrite(CALIB_LED, LOW); // turn off Arduino's LED to indicate we are through with calibration if (DEBUG_ENABLE) { // print the calibration minimum values measured when emitters were on for (int i = 0; i < NUM_SENSORS; i++) { Serial.print(qtrrc.calibratedMinimumOn[i]); Serial.print(' '); } Serial.println(); // print the calibration maximum values measured when emitters were on for (int i = 0; i < NUM_SENSORS; i++) { Serial.print(qtrrc.calibratedMaximumOn[i]); Serial.print(' '); } Serial.println(); Serial.println(); } delay(1000); } /////////////////// // Main Program // /////////////////// void loop() { // read calibrated sensor values and obtain a measure of the line position from 0 to 5000 // To get raw sensor values, call: // qtrrc.read(sensorValues); instead of unsigned int position = qtrrc.readLine(sensorValues); // Variable positon zeigt die genaue Position (0-7000) an unsigned int position = qtrrc.readLine(sensorValues); // get calibrated readings along with the line position, refer to the QTR Sensors Arduino Library for more details on line position. int error = position - 3500; // 3500 is the desired position if (DEBUG_ENABLE) { // print the sensor values as numbers from 0 to 1000, where 0 means maximum reflectance and // 1000 means minimum reflectance, followed by the line position for (unsigned char i = 0; i < NUM_SENSORS; i++) { Serial.print(sensorValues[i]); Serial.print('\t'); } Serial.print(position); // comment this line out if you are using raw values } int motorSpeed = KP * error + KD * (error - lastError); lastError = error; int rightMotorSpeed = MOTOR_SPEED + motorSpeed; int leftMotorSpeed = MOTOR_SPEED - motorSpeed; if (rightMotorSpeed > MOTOR_MAX_SPEED ) rightMotorSpeed = MOTOR_MAX_SPEED; // Verhindert dass die Maximalgeschwindigkeit überschritten wird if (leftMotorSpeed > MOTOR_MAX_SPEED ) leftMotorSpeed = MOTOR_MAX_SPEED; // Verhindert dass die Maximalgeschwindigkeit überschritten wird if (rightMotorSpeed < 0) rightMotorSpeed = 0; // Verhindert einen negativen Geschwindigkeitswert if (leftMotorSpeed < 0) leftMotorSpeed = 0; // Verhindert einen negativen Geschwindigkeitswert { // Ansteuerung der Motoren durch PID Berechnung digitalWrite(MotorRightBrake, LOW); // Motorbremse rechts deaktivieren digitalWrite(MotorLeftBrake, LOW); // Motorbremse links deaktivieren digitalWrite(MotorRightDirection, HIGH); // Motor rechts im Uhrzeigersinn --> fährt vorwärts digitalWrite(MotorLeftDirection, LOW); // Motor links gegen den Uhrzeigersinn --> fährt vorwärts analogWrite(MotorRightSpeed, rightMotorSpeed); // Motorgeschwindigkeit rechts in Abhängigkeit des PID Reglers analogWrite(MotorLeftSpeed, leftMotorSpeed); // Motorgeschwindigkeit links in Abhängigkeit des PID Reglers } } [/code]
