146 lines
3.6 KiB
C++
146 lines
3.6 KiB
C++
// This code works for the PAMI robots marked with an S and a 1
|
|
// It makes the robot follow the black line and stop in green squares.
|
|
// For a second green square stop, tape sensor_2 on the right side, at 3.5cm from
|
|
// the center (0.5cm from the edge). For a first green square stop, tape sensor_2
|
|
// on the left side, at 3.5cm from the center (0.5cm from the edge).
|
|
|
|
// Ultrasound sensor
|
|
const int trigPin = 12;
|
|
const int echoPin = 13;
|
|
float duration;
|
|
float distance;
|
|
unsigned long lastDistanceCheck = 0;
|
|
|
|
|
|
// Motor pins
|
|
const int A_Direction = 2; // A = left motor
|
|
const int A_Speed = 5;
|
|
const int B_Direction = 4; // B = right motor
|
|
const int B_Speed = 6;
|
|
|
|
const int sensorPin = 8;
|
|
const int sensorPin_2 = 9; //side sensor Ligne noire: 1cm width, rectangle noir: 5 cm width
|
|
|
|
// Tirette
|
|
const int cablePin = 7;
|
|
|
|
// Timers
|
|
//unsigned long thresholdTime = 100;
|
|
bool timerStarted = false;
|
|
unsigned long startTime = 0;
|
|
bool done = false;
|
|
bool isMoving = true;
|
|
const int Black = 1; // 1 = robot to the left of the line , // 0 = right
|
|
|
|
// Duration calculation for obstacle deteaction
|
|
float distanceUltra() {
|
|
digitalWrite(trigPin, LOW);
|
|
delayMicroseconds(2);
|
|
digitalWrite(trigPin, HIGH);
|
|
delayMicroseconds(10);
|
|
digitalWrite(trigPin, LOW);
|
|
duration = pulseIn(echoPin, HIGH) / 58.00; // cm
|
|
return duration;
|
|
}
|
|
|
|
// Obstacle Detection function to stop motors
|
|
void distanceStop() {
|
|
distance = distanceUltra();
|
|
Serial.println(distance); // Check if it works
|
|
if (distance <= 12) {
|
|
digitalWrite(A_Direction, HIGH);
|
|
analogWrite(A_Speed, 0);
|
|
digitalWrite(B_Direction, HIGH);
|
|
analogWrite(B_Speed, 0);
|
|
delay(3000); // Stop time
|
|
}
|
|
}
|
|
|
|
// Line following
|
|
void lineFollowing() {
|
|
int Center_Tra_Value = digitalRead(sensorPin);
|
|
|
|
if (isMoving){
|
|
if (Center_Tra_Value == Black) {
|
|
// Go slightly left when black
|
|
digitalWrite(A_Direction, HIGH);
|
|
analogWrite(A_Speed, 40);
|
|
digitalWrite(B_Direction, LOW);
|
|
analogWrite(B_Speed, 50);
|
|
} else {
|
|
|
|
// Go slightly right when white
|
|
digitalWrite(A_Direction, HIGH);
|
|
analogWrite(A_Speed, 50);
|
|
digitalWrite(B_Direction, LOW);
|
|
analogWrite(B_Speed, 40);
|
|
}
|
|
}
|
|
else{digitalWrite(A_Direction, HIGH);
|
|
analogWrite(A_Speed, 0);
|
|
digitalWrite(B_Direction, LOW);
|
|
analogWrite(B_Speed, 0); }
|
|
}
|
|
|
|
void setup() {
|
|
Serial.begin(9600);
|
|
|
|
pinMode(cablePin, INPUT_PULLUP);
|
|
pinMode(sensorPin, INPUT);
|
|
pinMode(sensorPin_2, INPUT);
|
|
pinMode(A_Direction, OUTPUT);
|
|
pinMode(A_Speed, OUTPUT);
|
|
pinMode(B_Direction, OUTPUT);
|
|
pinMode(B_Speed, OUTPUT);
|
|
pinMode(trigPin, OUTPUT);
|
|
pinMode(echoPin, INPUT);
|
|
|
|
Serial.println("Robot Ready");
|
|
}
|
|
|
|
void loop() {
|
|
int pinState = digitalRead(cablePin);
|
|
|
|
// Wait for tirette to start
|
|
if (pinState == HIGH) { // HIGH = no connection between D7 and GND
|
|
// Start
|
|
if (!timerStarted) {
|
|
startTime = millis();
|
|
timerStarted = true;
|
|
}
|
|
|
|
if (digitalRead(sensorPin_2) == Black){
|
|
digitalWrite(A_Direction, HIGH);
|
|
analogWrite(A_Speed, 0);
|
|
digitalWrite(B_Direction, LOW);
|
|
analogWrite(B_Speed, 0);
|
|
isMoving = false;
|
|
//while (true) {/*stop the robot*/}
|
|
}
|
|
|
|
// Follow line
|
|
if (timerStarted && millis() - startTime >= 2000UL) { // 2 seconds delay
|
|
lineFollowing();
|
|
done = true ;
|
|
}
|
|
|
|
// Stop
|
|
if (done) {
|
|
digitalWrite(A_Direction, HIGH);
|
|
analogWrite(A_Speed, 0);
|
|
digitalWrite(B_Direction, LOW);
|
|
analogWrite(B_Speed, 0);
|
|
}
|
|
|
|
// Obstacle detection
|
|
if ( millis() - lastDistanceCheck >= 500UL) {
|
|
lastDistanceCheck = millis();
|
|
distanceStop();
|
|
}
|
|
} else {
|
|
timerStarted = false;
|
|
done = false;
|
|
}
|
|
}
|
|
|