Seiten

Freitag, 15. März 2013

Getting Started: Ardubot - Linefollower

Bau des Ardubot-Linefollower:




Verwendete Bauteile:

Arduino Uno
Ardubot Platine
Line Sensor-Array
H-Bridge Motor Treiber 1A
2x Getriebemotoren
2x Modellbau Räder
2x Getriebemotor Halterung
Stützrad
Stiftleisten
Drahtbügel
Schrauben, Muttern
9V Blockbatterie





















Bestückung der Ardubot Platine:

Löten Sie den Motor Driver auf der Unterseite der Platine (Seite mit Sparkfun Logo) ein. Achten Sie auf die Übereinstimmung der Markierung auf der Platine und des IC's.




Löten Sie den Schiebeschalter auf der Oberseite der Platine ein.



Weiters montieren Sie die 2 Getriebemotoren mit der Halterung und löten Sie 2 paar Stiftleisten ein, um sie anschließend mit den Motoren verlöten zu können.



Danach können Sie das Stützrad montieren.



Der Arduino benötigt auch Stiftleisten die man einlöten muss, damit man ihn raufstecken kann.



Auch die Sensoren und der Schiebeschalter benötigen Stifleisten die man einlöten muss.
Hier ist die Sensorhalterung zu sehen die mit dem 3D Drucker gefertigt wurde.
Hier der stl file für den Sensorwinkel

Beim Sensor Array werden allerdings nicht alle 8 Sensoren benötigt, sodass man die 2 mit der gepunkteten Linie abbrechen kann.





Wer sich eine Batteriehalterung drucken möchte - hier der stl file


Der Programmcode:

#include <QTRSensors.h>  //library for the Sensors

int A_1 = 3;//Left Motor
int A_2 = 5;//Left Motor
int B_1 = 6;//Right Motor
int B_2 = 9;//Right Motor


// Change this values
#define KP 3      // value for the proportional
#define KD 10     //value for the deviate
#define M1_DEFAULT_SPEED 50 //Value for the Speed of Motor1
#define M2_DEFAULT_SPEED 50//Value for the Speed of Motor2
#define M1_MAX_SPEED 150
#define M2_MAX_SPEED 150
#define MIDDLE_SENSOR 3
#define NUM_SENSORS  5      // sensors what are used
#define TIMEOUT       2500  // waits for 2500 us for sensor outputs to go low
#define EMITTER_PIN   2     // emitter is controlled by digital pin 2
#define DEBUG 1 // set to 1 if serial debug output needed

QTRSensorsRC qtrrc((unsigned char[]) { 18, 17, 16, 15, 14} ,NUM_SENSORS, TIMEOUT, EMIT-TER_PIN);     //decleration of the sensor pins

unsigned int sensorValues[NUM_SENSORS];

void setup()
{
  Serial.begin(9600);
  delay(1000);
  manual_calibration();  //function for calibration one time
  pinMode (A_1, OUTPUT);
  pinMode (A_2, OUTPUT);
  pinMode (B_1, OUTPUT);
  pinMode (B_2, OUTPUT);
}

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


void loop()
{
  unsigned int sensors[5];
  int position = qtrrc.readLine(sensors);
  int error = position - 2000;
  Serial.println(error);
 

  int motorSpeed = KP * error + KD * (error - lastError);  //calculates the motor speed
  lastError = error;
  Serial.print("MotorSpeed ");
  Serial.println(motorSpeed);
 

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

  //
    set_motors(leftMotorSpeed, rightMotorSpeed);
}

void set_motors(int motor1speed, int motor2speed)
{
  if (motor1speed > M1_MAX_SPEED ) motor1speed = M1_MAX_SPEED; // limit top speed
  if (motor2speed > M2_MAX_SPEED ) motor2speed = M2_MAX_SPEED; // limit top speed
  if (motor1speed < 0) motor1speed = 0; // keep motor above 0
  if (motor2speed < 0) motor2speed = 0; // keep motor speed above 0
 
  analogWrite(A_2, motor1speed);
  digitalWrite(A_1, LOW);
  Serial.print("motor1speed ");
  Serial.println(motor1speed);
  digitalWrite(B_2, LOW);
  analogWrite(B_1, motor2speed);
  Serial.print("motor2speed ");
  Serial.println(motor2speed);
}




void manual_calibration() {

  int i;
  for (i = 0; i < 250; i++)  // the calibration will take a few seconds
  {
    qtrrc.calibrate(QTR_EMITTERS_ON);  // specifies that the emitters should be turned on for the
                        //reading, which results in a measure of reflectance

    delay(20);
  }

  if (DEBUG) { // if true, generate sensor dats via serial output
    Serial.begin(9600);
    for (int i = 0; i < NUM_SENSORS; i++)
    {
      Serial.print(qtrrc.calibratedMinimumOn[i]);  // The calibrated minimum values measured for each
                        //sensor, with emitters on.

      Serial.print(' ');
    }
    Serial.println();

    for (int i = 0; i < NUM_SENSORS; i++)
    {
      Serial.print(qtrrc.calibratedMaximumOn[i]); //]);  // The calibrated maximum values measured for
                            //each sensor, with emitters on.

      Serial.print(' ');
    }
    Serial.println();
    Serial.println();
  }
}