Added PAMi code

PAMI_LineFollowing_Stop.ino

@@ -0,0 +1,145 @@
+// 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;
+  }
+}
+