Advanced Sumo Robot with Arduino Nano ESP32

1#include <ESP32Servo.h>
2
3
4// Create Servo objects for each motor
5Servo escRight;
6Servo escLeft;
7Servo myServo;
8
9// Define ESC pins (use only PWM-capable pins!)
10const int pinRight = 2;
11const int pinLeft = 3;
12// Define servo motor pin
13const int servoPin = 12;
14
15// Individual sensor pins
16const int sensor1 = 4;
17const int sensor2 = 5;
18const int sensor3 = 6;
19const int sensor4 = 7;
20const int sensor5 = 8;
21const int sensor6 = 9;
22const int sensor7 = 10;
23const int sensor8 = 11;
24unsigned long attackTimer = 0;
25bool actionDone = false;  // Flag to ensure the action happens only once
26
27void setup() {
28  //Serial.begin(115200);
29  // Set PWM frequency to 80Hz for ESCs
30  escRight.setPeriodHertz(80);
31  escLeft.setPeriodHertz(80);
32
33  // Attach ESCs with min/max PWM signal
34  escRight.attach(pinRight, 1000, 2000);
35  escLeft.attach(pinLeft, 1000, 2000);
36
37  // Servo motor setup and single open/close action
38  myServo.setPeriodHertz(50);  // 50 Hz standard servo
39  myServo.attach(servoPin, 500, 2400);
40  delay(4000);
41
42  myServo.write(180);  // Open
43  delay(500);
44  myServo.write(15);  // Close
45  delay(500);         // Wait for 3.3 seconds
46
47
48  // Set all sensor pins as input
49  pinMode(sensor1, INPUT);
50  pinMode(sensor2, INPUT);
51  pinMode(sensor3, INPUT);
52  pinMode(sensor4, INPUT);
53  pinMode(sensor5, INPUT);
54  pinMode(sensor6, INPUT);
55  pinMode(sensor7, INPUT);
56  pinMode(sensor8, INPUT);
57
58
59
60
61
62
63  //myServo.detach();
64}
65
66void loop() {
67
68  if (!actionDone) {  // Check if the action has already occurred
69    writeAll(1500);
70    delay(200);
71    writeAll(2000);
72    delay(200);
73    writeAll(1500);
74    delay(600);
75
76    actionDone = true;  // Set the flag to true to prevent repeating
77  }
78
79  int s1 = digitalRead(sensor1);
80  int s2 = digitalRead(sensor2);
81  int s3 = digitalRead(sensor3);
82  int s4 = digitalRead(sensor4);
83  int s5 = digitalRead(sensor5);
84  int s6 = digitalRead(sensor6);
85  int s7 = digitalRead(sensor7);
86  int s8 = digitalRead(sensor8);
87
88  if (s3 == 1) {
89    writeAll(1100);  // Move forward to attack
90  }
91
92  else if (s7 == 1) {
93    writeAll(1900);  // Move forward to attack
94  }
95
96
97  else if ((s1 == 1) || (s5 == 1)) {
98    attackTimer = millis();
99    while (((!digitalRead(sensor3)) || (!digitalRead(sensor7))) && ((millis() - attackTimer) < 600)) {
100      escRight.writeMicroseconds(1800);
101      escLeft.writeMicroseconds(1200);
102
103      if ((digitalRead(sensor3) == 1) || (digitalRead(sensor7) == 1)) {
104        break;
105      }
106    }
107  }
108
109
110  else if ((s2 == 1) || (s6 == 1)) {
111    attackTimer = millis();
112    while (((!digitalRead(sensor3)) || (!digitalRead(sensor7))) && ((millis() - attackTimer) < 300)) {
113      escRight.writeMicroseconds(1800);
114      escLeft.writeMicroseconds(1200);
115
116      if ((digitalRead(sensor3) == 1) || (digitalRead(sensor7) == 1)) {
117        writeAll(1500);
118        break;
119      }
120    }
121  }
122
123
124  else if ((s4 == 1) || (s8 == 1)) {
125    attackTimer = millis();
126    while (((!digitalRead(sensor3)) || (!digitalRead(sensor7))) && ((millis() - attackTimer) < 300)) {
127      escRight.writeMicroseconds(1200);
128      escLeft.writeMicroseconds(1800);
129
130      if ((digitalRead(sensor3) == 1) || (digitalRead(sensor7) == 1)) {
131        writeAll(1500);
132        break;
133      }
134    }
135  }
136
137
138
139
140  else {
141    // If no enemy detected, stop
142    writeAll(1500);
143  }
144}
145
146// Helper function to write the same signal to all ESCs
147void writeAll(int pulse) {
148  escRight.writeMicroseconds(pulse);
149  escLeft.writeMicroseconds(pulse);
150}

1#include <ESP32Servo.h>
2// Create Servo objects for each motor
3Servo escRight;
4Servo escLeft;
5Servo myServo;
6
7
8// Define ESC pins (use only PWM-capable pins!)
9const int pinRight = 2;
10const int pinLeft = 3;
11// Define servo motor pin
12const int servoPin = 12;
13// Individual sensor pins
14const int sensor1 = 4;
15const int sensor2 = 5;
16const int sensor3 = 6;
17const int sensor4 = 7;
18const int sensor5 = 8;
19const int sensor6 = 9;
20const int sensor7 = 10;
21const int sensor8 = 11;
22unsigned long attackTimer = 0;
23bool actionDone = false;  // Flag to ensure the action happens only once
24
25// edge sensors
26int ferValue;
27int felValue;
28int berValue;
29int belValue;
30// Define pins for edge sensors, these edge sensors with output HIGH if a white color is detected
31const int FER_PIN = A1;  // Front Edge Right
32const int FEL_PIN = A2;  // Front Edge Left
33const int BER_PIN = A3;  // Back Edge Right
34const int BEL_PIN = A4;  // Back Edge Left
35// Variables to store the last edge speed for searching for the opponent
36int lastLeftEdgeSpeed = 1630;
37int lastRightEdgeSpeed = 1610;
38
39
40void setup() {
41  //Serial.begin(115200);
42  // Set PWM frequency to 80Hz for ESCs
43  escRight.setPeriodHertz(80);
44  escLeft.setPeriodHertz(80);
45  // Attach ESCs with min/max PWM signal
46  escRight.attach(pinRight, 1000, 2000);
47  escLeft.attach(pinLeft, 1000, 2000);
48  // Servo motor setup and single open/close action
49  myServo.setPeriodHertz(50);  // 50 Hz standard servo
50  myServo.attach(servoPin, 500, 2400);
51  delay(4000);
52  myServo.write(180);  // Open
53  delay(500);
54  myServo.write(15);  // Close
55  delay(500);         // Wait for 3.3 seconds
56  // Set all sensor pins as input
57  pinMode(sensor1, INPUT);
58  pinMode(sensor2, INPUT);
59  pinMode(sensor3, INPUT);
60  pinMode(sensor4, INPUT);
61  pinMode(sensor5, INPUT);
62  pinMode(sensor6, INPUT);
63  pinMode(sensor7, INPUT);
64  pinMode(sensor8, INPUT);
65  //Edge sensors
66  pinMode(FER_PIN, INPUT);
67  pinMode(FEL_PIN, INPUT);
68  pinMode(BER_PIN, INPUT);
69  pinMode(BEL_PIN, INPUT);
70}
71void loop() {
72  if (!actionDone) {  // Check if the action has already occurred
73    writeAll(1500);
74    delay(200);
75    writeAll(2000);
76    delay(200);
77    writeAll(1500);
78    delay(600);
79    actionDone = true;  // Set the flag to true to prevent repeating
80  }
81  int s1 = digitalRead(sensor1);
82  int s2 = digitalRead(sensor2);
83  int s3 = digitalRead(sensor3);
84  int s4 = digitalRead(sensor4);
85  int s5 = digitalRead(sensor5);
86  int s6 = digitalRead(sensor6);
87  int s7 = digitalRead(sensor7);
88  int s8 = digitalRead(sensor8);
89  //  Edge digital sensor  values
90  ferValue = digitalRead(FER_PIN);
91  felValue = digitalRead(FEL_PIN);
92  berValue = digitalRead(BER_PIN);
93  belValue = digitalRead(BEL_PIN);
94  if (s3 == 1) {
95    writeAll(1100);  // Move forward to attack
96  } else if (s7 == 1) {
97    writeAll(1900);  // Move forward to attack
98  }
99
100
101
102  else if (ferValue == 0) {  // Front Edge Right detected
103    lastRightEdgeSpeed = 1330;
104    lastLeftEdgeSpeed = 1350;
105    escRight.writeMicroseconds(1330);
106    escLeft.writeMicroseconds(1350);
107    delay(400);
108  } else if (felValue == 10) {  // Front Edge Left detected
109    lastRightEdgeSpeed = 1350;
110    lastLeftEdgeSpeed = 1330;
111    escRight.writeMicroseconds(1350);
112    escLeft.writeMicroseconds(1330);
113    delay(400);
114
115  } else if (berValue == 0) {  // Back Edge Right detected
116    lastRightEdgeSpeed = 1630;
117    lastLeftEdgeSpeed = 1590;
118    escRight.writeMicroseconds(1630);
119    escLeft.writeMicroseconds(1590);
120    delay(400);
121
122  } else if (belValue == 0) {  // Back Edge Left detected
123    lastRightEdgeSpeed = 1590;
124    lastLeftEdgeSpeed = 1630;
125    escRight.writeMicroseconds(1590);
126    escLeft.writeMicroseconds(1630);
127    delay(400);
128
129
130  }
131
132
133  else if ((s1 == 1) || (s5 == 1)) {
134    attackTimer = millis();
135    while (((!digitalRead(sensor3)) || (!digitalRead(sensor7))) && ((millis() - attackTimer) < 600)) {
136      escRight.writeMicroseconds(1800);
137      escLeft.writeMicroseconds(1200);
138      if ((digitalRead(sensor3) == 1) || (digitalRead(sensor7) == 1)) {
139        break;
140      }
141    }
142  } else if ((s2 == 1) || (s6 == 1)) {
143    attackTimer = millis();
144    while (((!digitalRead(sensor3)) || (!digitalRead(sensor7))) && ((millis() - attackTimer) < 300)) {
145      escRight.writeMicroseconds(1800);
146      escLeft.writeMicroseconds(1200);
147      if ((digitalRead(sensor3) == 1) || (digitalRead(sensor7) == 1)) {
148        writeAll(1500);
149        break;
150      }
151    }
152  } else if ((s4 == 1) || (s8 == 1)) {
153    attackTimer = millis();
154    while (((!digitalRead(sensor3)) || (!digitalRead(sensor7))) && ((millis() - attackTimer) < 300)) {
155      escRight.writeMicroseconds(1200);
156      escLeft.writeMicroseconds(1800);
157      if ((digitalRead(sensor3) == 1) || (digitalRead(sensor7) == 1)) {
158        writeAll(1500);
159        break;
160      }
161    }
162  } else {
163    escRight.writeMicroseconds(lastRightEdgeSpeed);
164    escLeft.writeMicroseconds(lastLeftEdgeSpeed);
165  }
166}
167// Helper function to write the same signal to all ESCs
168void writeAll(int pulse) {
169  escRight.writeMicroseconds(pulse);
170  escLeft.writeMicroseconds(pulse);
171}

1#include <ESP32Servo.h>
2
3// Create Servo objects for each motor
4Servo escRight;
5Servo escLeft;
6
7// Define pins (use only PWM-capable pins!)
8const int pinRight  = 2;
9const int pinLeft   = 3;
10
11void setup() {
12  // Set PWM frequency to 50Hz for ESCs
13  escRight.setPeriodHertz(80);
14  escLeft.setPeriodHertz(80);
15
16  // Attach ESCs with min/max PWM signal
17  escRight.attach(pinRight, 1000, 2000);
18  escLeft.attach(pinLeft, 1000, 2000);
19
20  // Arm ESCs with neutral signal
21  writeAll(1500);
22  delay(2000); // ESC arming delay
23}
24
25void loop() {
26  // Forward (1700us) for 0.9 sec
27  writeAll(1900);
28  delay(1600);
29
30  // Stop (1500us) for 0.12 sec
31  writeAll(1500);
32  delay(120);
33
34  // Reverse (1300us) for 0.9 sec
35  writeAll(1100);
36  delay(1600);
37
38  // Stop (1500us) for 0.12 sec
39  writeAll(1500);
40  delay(120);
41}
42
43// Helper function to write the same signal to all ESCs
44void writeAll(int pulse) {
45  escRight.writeMicroseconds(pulse);
46  escLeft.writeMicroseconds(pulse);
47}

1#include <ESP32Servo.h>
2
3Servo esc;
4int escPin = 4;  // Choose any PWM-capable pin
5
6void setup() {
7  esc.setPeriodHertz(80);          // Standard 50Hz for ESC
8  esc.attach(escPin, 1000, 2000);  // Min and Max pulse width in microseconds
9  esc.writeMicroseconds(1500);     // Neutral (Stop)
10  delay(2000);                     // Wait to arm ESC
11}
12
13void loop() {
14
15  // Forward (1800us) for 2 sec
16  esc.writeMicroseconds(2000);
17  delay(1000);
18
19  // Stop (1500us) for 1 sec
20  esc.writeMicroseconds(1500);
21  delay(120);
22
23  // Reverse (1200us) for 2 sec
24  esc.writeMicroseconds(1000);
25  delay(1000);
26
27  // Stop (1500us) for 1 sec
28  esc.writeMicroseconds(1500);
29  delay(120);
30}

1#include <SoftwareSerial.h>
2#include "TFMini.h"
3
4#define TIMER_THRESHOLD 300     // Timeout in milliseconds
5int rangeLimit = 40;            // Detection threshold in cm
6
7// Sensor 1: RX = 8 green, TX = 7
8SoftwareSerial tfSerial1(8, 7);
9TFMini tfmini1;
10
11// Sensor 2: RX = 10 green, TX = 9
12SoftwareSerial tfSerial2(10, 9);
13TFMini tfmini2;
14
15// Output pins
16const int output1 = 5;
17const int output2 = 6;
18
19void setup() {
20  Serial.begin(9600);
21
22  tfSerial1.begin(TFMINI_BAUDRATE);
23  tfSerial2.begin(TFMINI_BAUDRATE);
24
25  tfmini1.begin(&tfSerial1);
26  tfmini2.begin(&tfSerial2);
27
28  pinMode(output1, OUTPUT);
29  pinMode(output2, OUTPUT);
30}
31
32// Read and parse data frame from TFMini
33void getTFminiData(SoftwareSerial& serial, int* distance, int* strength, bool* validReading) {
34  static char i = 0;
35  char j = 0;
36  int checksum = 0;
37  static int rx[9];
38  if (serial.available()) {
39    rx[i] = serial.read();
40    if (rx[0] != 0x59) {
41      i = 0;
42    } else if (i == 1 && rx[1] != 0x59) {
43      i = 0;
44    } else if (i == 8) {
45      for (j = 0; j < 8; j++) {
46        checksum += rx[j];
47      }
48      if (rx[8] == (checksum % 256)) {
49        *distance = rx[2] + rx[3] * 256;
50        *strength = rx[4] + rx[5] * 256;
51        *validReading = true;
52      }
53      i = 0;
54    } else {
55      i++;
56    }
57  }
58}
59
60// Read one sensor and print status
61void readSensor(TFMini& sensor, SoftwareSerial& serial, int pinOut, const char* label) {
62  int distance = 0, strength = 0;
63  bool validReading = false;
64  unsigned long start = millis();
65
66  serial.listen();  // Activate listening on this serial port
67
68  // Try to read new data within timeout
69  while (!validReading && (millis() - start < TIMER_THRESHOLD)) {
70    getTFminiData(serial, &distance, &strength, &validReading);
71  }
72
73  if (validReading) {
74    if (distance <= rangeLimit) {
75      digitalWrite(pinOut, HIGH);
76      //Serial.print(label); Serial.println(": HIGH (Target Detected)");
77    } else {
78      digitalWrite(pinOut, LOW);
79      //Serial.print(label); Serial.println(": LOW (Target Out of Range)");
80    }
81
82    Serial.print(label); Serial.print(" Distance: ");
83    Serial.print(distance); Serial.println(" cm");
84  } else {
85    digitalWrite(pinOut, LOW);
86    //Serial.print(label); Serial.println(": TIMEOUT (No Response)");
87  }
88}
89
90void loop() {
91  readSensor(tfmini1, tfSerial1, output1, "Sensor 1");
92  readSensor(tfmini2, tfSerial2, output2, "Sensor 2");
93
94  //delay(100);
95}