buongiorno a tutti,
sto realizzando un progetto di un robot che segue una linea nera ed evita gli ostacoli...ho completato i due programmi separatamente, ma non riesco ad unirli...il robot deve dare la precedenza alla linea e quando finisce la linea deve usare il sensore ad ultrasuoni, per poi ripassare alla linea...
Questi sono i due programmi:
#include <QTRSensors.h>
#define Kp 0.1 // Azione proporzionale
#define Kd 0.2 // Azione derivatrice ( Nota: Kp < Kd)
#define rightMaxSpeed 150 // Velocità Massima del motore destro
#define leftMaxSpeed 150 // Velocità Massima del motore sinistro
#define rightBaseSpeed 110 // Velocità quando il robot segue perfettamente la linea, motore destro
#define leftBaseSpeed 110 // Velocità quando il robot segue perfettamente la linea, motore sinistro
#define NUM_SENSORS 6 // Numero di sensori IR usati
#define TIMEOUT 2500 // waits for 2500 us for sensor outputs to go low
#define EMITTER_PIN QTR_NO_EMITTER_PIN
#define in1 9 // Marcia avanti motore destro
#define in2 8 // Marcia indietro motore destro
#define enA 10 //Abilità il motore destro
#define in3 7 // Marcia avanti motore sinistro
#define in4 6 // Marcia indietro motore sinistro
#define enB 5 // Abilità il motore sinistro
QTRSensorsRC qtrrc((unsigned char[]) {A0,A1,A2,A3,A4,A5}, 6); // 6 pin usati per la barra IR
unsigned int sensorValues[NUM_SENSORS];
void setup()
{
pinMode(in1, OUTPUT);
pinMode(in2, OUTPUT);
pinMode(enA, OUTPUT);
pinMode(in3, OUTPUT);
pinMode(in4, OUTPUT);
pinMode(enB, OUTPUT);
int i;
for (int i = 0; i < 100; i++) // Calibrazione facend passare il sensore attraverso la linea
qtrrc.calibrate();
delay(20);
wait();
delay(2000); // Aspetta per 2s per posizionare il robot prima di entrare in loop
// Stampa nel monitor seriale la calibrazione
Serial.begin(9600);
for (int i = 0; i < NUM_SENSORS; i++)
{
Serial.print(qtrrc.calibratedMinimumOn[i]);
Serial.print(' ');
}
Serial.println();
for (int i = 0; i < NUM_SENSORS; i++)
{
Serial.print(qtrrc.calibratedMaximumOn[i]);
Serial.print(' ');
}
Serial.println();
Serial.println();
}
int lastError = 0;
void loop()
{
unsigned int sensors[6];
int position = qtrrc.readLine(sensors); // get calibrated readings along with the line position, refer to the QTR Sensors Arduino Library for more details on line position.
int error = position - 2500;
int motorSpeed = Kp * error + Kd * (error - lastError);
lastError = error;
int rightMotorSpeed = rightBaseSpeed + motorSpeed;
int leftMotorSpeed = leftBaseSpeed - motorSpeed;
if (rightMotorSpeed > rightMaxSpeed ) rightMotorSpeed = rightMaxSpeed; // Permette al motore destro di non superare la velocità massima
if (leftMotorSpeed > leftMaxSpeed ) leftMotorSpeed = leftMaxSpeed; // Permette al motore sinistro di non superare la velocità massima
if (rightMotorSpeed < 0) rightMotorSpeed = 0; // Rende la velocità del motore destro positiva
if (leftMotorSpeed < 0) leftMotorSpeed = 0; // Rende la velocità del motore sinistro positiva
{
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
analogWrite(enA, rightMotorSpeed);
digitalWrite(in3, HIGH);
digitalWrite(in4, LOW);
analogWrite(enB, leftMotorSpeed);
}
}
void wait(){
}
#define echoPin 2 // Echo Pin
#define trigPin 4 // Trigger Pin
int maximumRange = 200; // Range massimo del sensore
int minimumRange = 10; // Range minimo del sensore
int in1 = 9; // Marcia avanti motore destro
int in2 = 8; // Marcia indietro motore destro
int enA = 10; // Marcia indietro motore destro
int in3 = 7; // Marcia avanti motore sinistro
int in4 = 6; // Marcia indietro motore sinistro
int enB = 5; // Abilità il motore sinistro
long duration, distance;
void setup() {
Serial.begin (9600);
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
pinMode(enA, OUTPUT);
pinMode(enB, OUTPUT);
pinMode(in1, OUTPUT);
pinMode(in2, OUTPUT);
pinMode(in3, OUTPUT);
pinMode(in4, OUTPUT);
}
void loop(){
// Invio e ricezione del segnale inviato (TX - RX)
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
delay(500);
digitalWrite(trigPin, HIGH);
delayMicroseconds(2);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
// Calcola la distanza in centimetri dall'oggetto*/
distance = duration/90;
if (distance <= minimumRange){
// Ha rilevato un ostacolo quindi sterza verso destra
Serial.println("Ostacolo");
digitalWrite(in1, LOW);
digitalWrite(in2, LOW);
digitalWrite(in3, HIGH);
digitalWrite(in4, LOW);
analogWrite(enA, 0);
analogWrite(enB, 150);
}
else {
// Non ha rilevato ostacoli quindi prosegue dritto
Serial.println(distance);
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
digitalWrite(in3, HIGH);
digitalWrite(in4, LOW);
analogWrite(enA, 100);
analogWrite(enB, 100);
}
}
