Hello guys,
I am new to Arduino and need some help. I am using a QTR-8A Refectance Sensor with a Adafruit Motorshield V2 on a Arduino Uno. Besides that, there are two DC-Motors connected on the Motor Shield. I am using 6 of the 8 Sensors (2-7) on the Analog Pins A0-A5. The robot is powered with a 9V Battery. My Code is correct, but the Motors are not working. The Sensors calibrate correctly and give correct values but the Motors do not start. Besides that, if i define AFMS.begin(); the program does not progress any further. Why is that? I would be happy if someone could help me.
Best regards
#include <QTRSensors.h>
#include <Adafruit_MotorShield.h>
Adafruit_MotorShield AFMS = Adafruit_MotorShield();
Adafruit_DCMotor *leftMotor = AFMS.getMotor(3); // M3
Adafruit_DCMotor *rightMotor = AFMS.getMotor(4); // M4
QTRSensors qtr;
void setup()
{
Serial.begin(2000000);
Serial.println("Test");
// Optional: wait for some input from the user, such as a button press.
// Initialize the sensors.
// In this example we have three sensors on pins A0 - A2.
//qtr.setTypeRC();
qtr.setTypeAnalog();
qtr.setSensorPins((const uint8_t[]){A0, A1, A2, A3, A4, A5}, 6);
pinMode(LED_BUILTIN, OUTPUT);
digitalWrite(LED_BUILTIN, HIGH);
// Optional: change parameters like RC timeout, set an emitter control pin...
// Then start the calibration phase and move the sensors over both reflectance
// extremes they will encounter in your application. This calibration should
// take about 5 seconds (250 iterations * 20 ms per iteration).
//
// Passing a QTRReadMode to calibrate() is optional; it should match the mode
// you plan to use when reading the sensors.
Serial.println("Starte Kalibrierung");
for (uint8_t i = 0; i < 250; i++)
{
qtr.calibrate();
delay(20);
}
digitalWrite(LED_BUILTIN, LOW);
Serial.println("Kalibrierung abgeschlossen");
// Optional: signal that the calibration phase is now over and wait for
// further input from the user, such as a button press.
}
#define KP 0.1 // Proportional constant
#define KD 5.0 // Derivative constant
#define M1 200 // Base motor speed for motor 1
#define M2 200 // Base motor speed for motor 2
void loop()
{
static int16_t lastError = 0;
uint16_t sensors[6];
// Get calibrated sensor values returned in the sensors array
int16_t position = qtr.readLineBlack(sensors);
// Ausgabe der Sensorwerte auf dem seriellen Monitor
Serial.print("Sensor Werte: ");
for (int i = 0; i < 6; i++) {
Serial.print(sensors[i]);
Serial.print(" ");
}
Serial.println();
int16_t error = position - 500; // Annahme, dass die Mittelposition bei etwa 3000 liegt
// Set the motor speed based on proportional and derivative PID terms
int16_t motorSpeed = KP * error + KD * (error - lastError);
lastError = error;
// Calculate individual motor speeds based on the PID control
int16_t m1Speed = M1 + motorSpeed;
int16_t m2Speed = M2 - motorSpeed;
// Ensure motor speeds are within valid range
if (m1Speed < 0) { m1Speed = 0; }
if (m2Speed < 0) { m2Speed = 0; }
// Set motor speeds using the two motor speed variables above
// Assuming leftMotor and rightMotor are defined globally
leftMotor->setSpeed(m1Speed);
rightMotor->setSpeed(m2Speed);
// Run the motors in the forward direction
leftMotor->run(FORWARD);
rightMotor->run(FORWARD);
}
