Το Στέκι των Πληροφορικών

/*
 Λειτουργία Έξυπνου αυτοκίνητου με την εφαρμογή ARDicon.
 Έξυπνο αυτοκίνητο ελέγχου ρομποτικού βραχίονα.
*/
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x47);
#include <Servo.h>
//#include <IRremote.h>
//int RECV_PIN = A0;
int echoPin = 13;  // Ορισμός μονάδα υπερήχων ECHO στο D13
int trigPin = 12;  // Ορισμός μονάδα υπερήχων TRIG στο D12

#define IR_Go 70
#define IR_Back 21
#define IR_Left 68
#define IR_Right 67
#define IR_Stop 64

#define SensorLeft 6    // καρφίτσα εισαγωγής του αριστερού αισθητήρα
#define SensorMiddle 7  // καρφίτσα εισαγωγής του μεσαίου αισθητήρα
#define SensorRight 8  // καρφίτσα εισαγωγής του δεξιού αισθητήρα
unsigned char SL;      // κατάσταση αριστερού αισθητήρα
unsigned char SM;      // κατάσταση μεσαίου αισθητήρα
unsigned char SR;      // κατάσταση δεξιού αισθητήρα

Servo myservo1;
Servo myservo2;
Servo myservo3;

int k1 = 80, k2 = 120, k3 = 90;  // αρχικοποίηση της τιμής γωνίας των σερβομηχανισμών
int M1[20], M2[20], M3[20];
int i = 0, j = 0, t = 0, speed = 1500, turnSpeed = 2000;
char blue_val;

void setup() {
  Serial.begin(9600);  //set baud rate to 9600
  pwm.begin();
  pwm.setPWMFreq(60);
  myservo1.attach(11);
  myservo2.attach(10);
  myservo3.attach(9);
  myservo1.write(k1);
  delay(1000);
  myservo3.write(k3);
  delay(1000);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(SensorLeft, INPUT);
  pinMode(SensorMiddle, INPUT);
  pinMode(SensorRight, INPUT);
  stop();
  //IrReceiver.begin(RECV_PIN);  // ενεργοποίηση του δέκτη IR
}

void loop() {
  if (Serial.available() > 0) {  // Λήψη σημάτων Bluetooth
    blue_val = Serial.read();
    Serial.println(blue_val);
    switch (blue_val) {
      case 'F': advance(); break;  // Αν λάβει εντολή 'F', κάνει κίνηση μπροστά

      case 'B': back(); break;  // Αν λάβει εντολή 'B', κάνει κίνηση πίσω

      case 'L': turnL(); break;  // Αν λάβει εντολή 'L', στροφή αριστερά

      case 'R': turnR(); break;  // Αν λάβει εντολή 'R', στροφή δεξιά

      case 'S': stop(); break;  // Αν λάβει εντολή 'S', σταματάει

      case 'a': speeds_a(); break;  // Αν λάβει εντολή 'a', επιταχύνει

      case 'd': speeds_d(); break;  // Αν λάβει εντολή 'd', επιβραδύνει

      case 'f': servo2up(); break;  // // Ο βραχίονας ανυψώνεται

      case 'b': servo2down(); break;  // // Ο βραχίονας χαμηλώνει

      case 'l': servo3left(); break;  // Ο βραχίονας στρέφεται προς τα αριστερά

      case 'c': servo3center(); break;  // Ο βραχίονας παίρνει κεντρική θέση

      case 'r': servo3right(); break;  // Ο βραχίονας στρέφεται προς τα δεξιά

      case 'Q': servo1on(); break;  // Η δαγκάνα ανοίγει

      case 'E': servo1off(); break;  // Η δαγκάνα κλείνει

      case 't': read_servo(); break;  // Αν λάβει εντολή 't', καταγράφει κίνηση

      case 'i': do_servo(); break;  // Αν λάβει εντολή 'i', εκτελεί κίνηση

      case 'Y': avoid(); break;  // Αν λάβει εντολή 'Y' είσοδο στη λειτουργία αποφυγής εμποδίων

      case 'X': tracking(); break;  // Αν λάβει εντολή 'X' είσοδος σε λειτουργία παρακολούθησης γραμμής

      case 'U': follow(); break;  // Αν λάβει εντολή 'U' είσοδος στη λειτουργία παρακολούθησης υπερήχων
      case 'G': Fall(); break;  // Αν λάβει εντολή 'G' εισέλθει σε κατάσταση αντι-πτώσης
      default: break;
    }
  }
}

// Μέθοδοι κίνησης//

void advance()  // Κίνηση μπροστά
{
  pwm.setPWM(0, 0, speed);
  pwm.setPWM(1, 0, 0);
  pwm.setPWM(2, 0, speed);
  pwm.setPWM(3, 0, 0);
  pwm.setPWM(4, 0, speed);
  pwm.setPWM(5, 0, 0);
  pwm.setPWM(6, 0, speed);
  pwm.setPWM(7, 0, 0);
}

void turnR()  // Στροφή δεξιά
{
  pwm.setPWM(0, 0, turnSpeed);
  pwm.setPWM(1, 0, 0);
  pwm.setPWM(2, 0, turnSpeed);
  pwm.setPWM(3, 0, 0);
  pwm.setPWM(4, 0, 0);
  pwm.setPWM(5, 0, turnSpeed);
  pwm.setPWM(6, 0, 0);
  pwm.setPWM(7, 0, turnSpeed);
}

void turnL()  // Στροφή αριστερά
{
  pwm.setPWM(0, 0, 0);
  pwm.setPWM(1, 0, turnSpeed);
  pwm.setPWM(2, 0, 0);
  pwm.setPWM(3, 0, turnSpeed);
  pwm.setPWM(4, 0, turnSpeed);
  pwm.setPWM(5, 0, 0);
  pwm.setPWM(6, 0, turnSpeed);
  pwm.setPWM(7, 0, 0);
}

void stop()  // Σταματάει
{
  pwm.setPWM(0, 0, 0);
  pwm.setPWM(1, 0, 0);
  pwm.setPWM(2, 0, 0);
  pwm.setPWM(3, 0, 0);
  pwm.setPWM(4, 0, 0);
  pwm.setPWM(5, 0, 0);
  pwm.setPWM(6, 0, 0);
  pwm.setPWM(7, 0, 0);
}

void back()  // Κίνηση προς τα πίσω
{
  pwm.setPWM(0, 0, 0);
  pwm.setPWM(1, 0, speed);
  pwm.setPWM(2, 0, 0);
  pwm.setPWM(3, 0, speed);
  pwm.setPWM(4, 0, 0);
  pwm.setPWM(5, 0, speed);
  pwm.setPWM(6, 0, 0);
  pwm.setPWM(7, 0, speed);
}

//Μεθοδοι ελέγχου ταχύτητας//

//Επιτάχυνση
void speeds_a() {
 speed=1500;
}

//Επιβράδυνση
void speeds_d() {
  speed=750;
}

//Μέθοδοι ελέγχου βραχίωνα//
void servo1on() {  // Η δαγκάνα ανοίγει
  while (k1 != 20) {
    k1 -= 1;
    myservo1.write(k1);
    delay(5);
  }
}

void servo1off() {  // Η δαγκάνα κλείνει
  bool servo1off_flag = 1;
  while (k1 != 80) {
    k1 += 1;  // έλεγχος της ακρίβειας περιστροφής
    myservo1.write(k1);
    delay(5);  // έλεγχος της ταχύτητας περιστροφής του σερβο

  }
}

void servo2up() {  // ο ρομποτικός βραχίονας ανυψώνεται
  while (k2 != 120) {
    k2 += 1;  // έλεγχος της ακρίβειας περιστροφής
    myservo2.write(k2);
    delay(10);  // έλεγχος της ταχύτητας περιστροφής του σερβο
  
  }
}

void servo2down() {  // ο ρομποτικός βραχίονας χαμηλώνει
  while (k2 != 40) {
    k2 -= 1;  // έλεγχος της ακρίβειας περιστροφής
    myservo2.write(k2);
    delay(10);  // έλεγχος της ταχύτητας περιστροφής του σερβο
  
  }
}

void servo3left() {  // ο ρομποτικός βραχίονας στρέφεται αριστερόστροφα
  while (k3!=180) {
    k3 += 1;  // έλεγχος της ακρίβειας περιστροφής
    myservo3.write(k3);
    delay(10);  // έλεγχος της ταχύτητας περιστροφής του σερβο
  }
}

void servo3center() {  // ο ρομποτικός βραχίονας στέκεται στην κεντρική θέση 
  while (k3!=90) {
    if (k3 > 90) {
    k3 -= 1;
    }
    if (k3 < 90) 
    {k3 += 1;}
      // έλεγχος της ακρίβειας περιστροφής
    myservo3.write(k3);
    delay(10);  // έλεγχος της ταχύτητας περιστροφής του σερβο

  }
}

void servo3right() {  // ο ρομποτικός βραχίονας στρέφεται δεξιόστροφα

  while (k3!=10) {
    k3 -= 1;  // έλεγχος της ακρίβειας περιστροφής
    myservo3.write(k3);
    delay(10);  // έλεγχος της ταχύτητας περιστροφής του σερβο
  }
}

//Μεθόδοι Μνήμης//
void read_servo() {
  M1[i] = myservo1.read();  // αποθήκευση της τιμής της γωνίας των σερβομηχανισμών σε πίνακα
  delay(100);              // χρόνος καθυστέρησης για να αποθηκεύτεί η τιμή της γωνίας
  M2[i] = myservo2.read();
  delay(100);
  M3[i] = myservo3.read();
  delay(100);
  i++;    // i προσθέτει 1 κάθε φορά που αποθηκεύεται i
  j = i;  // ορίστε την τιμή του i στο j
  delay(200);
}

void do_servo() {
  i = 0;
  t = 1;
  k1 = myservo1.read();  // διαβάζει τις τιμές των γωνιών και τις θέτει στην ανάλογη μεταβλητή
  k2 = myservo2.read();
  k3 = myservo3.read();

  while (t) {
    for (int k = 0; k < j; k++) {  // επανάληψη j φορές
      if (k1 < M1[k]) {            // εάν η τρέχουσα τιμή γωνίας του σερβομηχανισμού 1 είναι μικρότερη από την τιμή του πίνακα 1
        while (k1 < M1[k]) {      // ο σερβομηχανισμός περιστρέφεται στη θέση όπου είναι αποθηκευμένος στον πίνακα
          myservo1.write(k1);      // servo 1 εκτελεί την κίνηση
          k1++;                    // K1 προσθέτει 1
          delay(10);              // καθυστέρηση για 10ms / έλεγχος της ταχύτητας περιστροφής του σερβομηχανισμού
        }
      } else {                // όταν η τιμή της γωνίας του σερβομηχανισμού 1 είναι μεγαλύτερη από την τιμή που έχει αποθηκευτεί στον πίνακα 1
        while (k1 > M1[k]) {  // ο σερβομηχανισμός περιστρέφεται στη θέση όπου είναι αποθηκευμένος στον πίνακα
          myservo1.write(k1);  // servo 1 εκτελεί την κίνηση
          k1--;                // k1 αφαιρεί 1
          delay(10);          // καθυστέρηση για 10ms / έλεγχος της ταχύτητας περιστροφής του σερβομηχανισμού
        }
      }
      // το ίδιο παρακάτω
      if (k2 < M2[k]) {
        while (k2 < M2[k]) {
          myservo2.write(k2);
          k2++;
          delay(10);
        }
      } else {
        while (k2 > M2[k]) {
          myservo2.write(k2);
          k2--;
          delay(10);
        }
      }

      if (k3 < M3[k]) {
        while (k3 < M3[k]) {
          myservo3.write(k3);
          k3++;
          delay(10);
        }
      } else {
        while (k3 > M3[k]) {
          myservo3.write(k3);
          k3--;
          delay(10);
        }
      }

      if (Serial.available() > 0) {  // για την έξοδο από τον βρόχο
        if (Serial.read() == 't') {  // λαμβάνει 'i' τότε 't'
          t = 0;                    // θέτει το 't' σε 0, έξοδος απο τον βρόχο
          break;
        }
      }
    }
  }
}

//Μεθοδος αποφυγής εμποδίων//

int Ultrasonic_Ranging() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  int distance = pulseIn(echoPin, HIGH);  // Ανάγνωση της διάρκειας των υψηλών επιπέδων
  distance = distance / 58;              // Μετατροπή του χρόνου παλμού σε απόσταση
  delay(50);
  return distance;
}

void avoid() {  //αυτόνομη κίνηση, αποφυγή εμποδίων με υπερήχους
  bool avoid_flag = 1;
  while (avoid_flag) {
    int distance = Ultrasonic_Ranging();
    Serial.print("distance=");
    Serial.println(distance);
    if (distance < 30) {    // υποθέτοντας ότι η μπροστινή απόσταση είναι μικρότερη από 30 cm
      if (distance < 15) {  // υποθέτοντας ότι η μπροστινή απόσταση είναι μικρότερη από 15 cm
        stop();
        delay(100);
        back();
        delay(300);
      } else {  // όταν 10 < απόσταση < 30, στροφή δεξιά
        stop();
        delay(100);
        turnR();
        delay(500);
      }
    } else {  // όταν η απόσταση > 30, κίνηση μπροστά
      advance();
    }
    blue_val = Serial.read();
    if (blue_val == 'S') {
      stop();
      avoid_flag = 0;
    }
  }
}

//Μέθοδος ακολούθησης γραμμής
void tracking(void) {
  bool tracking_flag = 1;
  while (tracking_flag) {
    SL = digitalRead(SensorLeft);
    SM = digitalRead(SensorMiddle);
    SR = digitalRead(SensorRight);
    if (SM == HIGH) {
      if (SL == LOW && SR == HIGH) {  // Αν ανιχνευτεί μαύρο στα δεξιά και λευκό στα αριστερά, στρίβει δεξιά
        turnR();
      } else if (SR == LOW && SL == HIGH) {  // αν ανιχνευτεί μαύρο στα αριστερά και λευκό στα δεξιά, στρίβει αριστερά
        turnL();
      } else {  // Αν ανιχνευτεί λευκό και στις δύο άκρες, κινείται ευθεία
        advance();
      }
    } else {
      if (SL == LOW && SR == HIGH) {  // Αν ανιχνευτεί μαύρο στα δεξιά και λευκό στα αριστερά, στρίβει δεξιά
        turnR();
      } else if (SR == LOW && SL == HIGH) {  // Αν ανιχνευτεί μαύρο στα αριστερά και λευκό στα δεξιά, στρίβει αριστερά
        turnL();
      } else {  // Αν όλοι οι σένσορες ανιχνεύουν λευκό, το αυτοκίνητο σταματάει
        stop();
      }
    }
    blue_val = Serial.read();
    if (blue_val == 'S') {
      stop();
      tracking_flag = 0;
    }
  }
}

//Μέθοδος αισθητήρα υπέρηχων
void follow() {
  bool follow_flag = 1;
  while (follow_flag) {
    int distance = Ultrasonic_Ranging();
    if (distance < 15) {  // Αν η απόσταση είναι μικρότερη απο 15 εκατοστά, κίνηση προς τα πίσω
      back();
    } else if (distance >= 15 && distance < 20) {  // Αν η απόσταση είναι μεταξύ 15 και 20 εκατοστά, σταμάτα
      stop();
    } else if (distance >= 20 && distance < 40) {  // Αν η απόσταση είναι μεταξύ 20 και 40 εκατοστά, κίνηση μπροστά
      advance();
    } else {
      stop();  // αλλιώς σταμάτα
    }
    blue_val = Serial.read();
    if (blue_val == 'S') {
      stop();
      follow_flag = 0;
    }
  }
}

//Μέθοδος αντι-πτώσης
void Fall(void) {
  bool Fall_flag = 1;
  while (Fall_flag) {
    SL = digitalRead(SensorLeft);
    SM = digitalRead(SensorMiddle);
    SR = digitalRead(SensorRight);
    if (SM == HIGH || SL == HIGH || SR == HIGH) {
      back();
      delay(500);
      turnL();
      delay(500);
    } else {
      advance();
    } 
    blue_val = Serial.read();
    if (blue_val == 'S') {
      stop();
      Fall_flag = 0;
    }
  }
}

#include <Servo.h>

// Create servo objects
Servo leftFront;
Servo leftRear;
Servo rightFront;
Servo rightRear;

// Define pins (change if needed)
const int LF_PIN = 3;  // PM1
const int LR_PIN = 5;  // PM2
const int RF_PIN = 6;  // PM3
const int RR_PIN = 9;  // PM4

void setup() {
  Serial.begin(9600);

  // Attach servos
  leftFront.attach(LF_PIN);
  leftRear.attach(LR_PIN);
  rightFront.attach(RF_PIN);
  rightRear.attach(RR_PIN);
}

void loop() {
  if (Serial.available()) {
    char command = Serial.read();
    handleCommand(command);
  }
}

void handleCommand(char cmd) {
  switch (cmd) {
    case 'F': // Forward
      setAll(180, 0);
      break;
    case 'B': // Backward
      setAll(0, 180);
      break;
    case 'L': // Turn left
      setAll(0, 0);
      break;
    case 'R': // Turn right
      setAll(180, 180);
      break;
    case 'S': // Stop
      setAll(90, 90);
      break;
  }
}

// Set speed for left and right side
void setAll(int leftSpeed, int rightSpeed) {
  leftFront.write(leftSpeed);
  leftRear.write(leftSpeed);
  rightFront.write(rightSpeed);
  rightRear.write(rightSpeed);
}