Electropepper/Compete_car
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Fixing up phaseTwo()

Browse Source
This commit is contained in:
Ricardo
2026-02-03 07:07:57 +00:00
parent 7cd3ad3a87
commit a2c397d6ca
1 changed files with 345 additions and 345 deletions
Download Patch File Download Diff File Expand all files Collapse all files

690
Firmware/micro_motors/micro_motors.ino
View File

@@ -1,54 +1,54 @@
#include <WiFi.h> #include <WiFi.h>
#include <ArduinoOTA.h> #include <ArduinoOTA.h>
const char* ssid = "CIA"; const char* ssid = "CIA";
const char* password = "hi123456"; const char* password = "hi123456";
#define motorAttach ledcAttach // Get more appropriate names #define motorAttach ledcAttach // Get more appropriate names
#define motorSpeed ledcWrite // same " #define motorSpeed ledcWrite // same "
// Motor FR (Front Right) // Motor FR (Front Right)
#define FR_IN1 27 #define FR_IN1 27
#define FR_IN2 14 #define FR_IN2 14
#define FR_PWM 0 #define FR_PWM 0
// Motor FL (Front Left)
#define FL_IN1 12
#define FL_IN2 13
#define FL_PWM 2
// Motor BR (Back Right) // Motor FL (Front Left)
#define BR_IN1 33 #define FL_IN1 12
#define BR_IN2 32 #define FL_IN2 13
#define BR_PWM 23 #define FL_PWM 2
// Motor BL (Back Left) // Motor BR (Back Right)
#define BL_IN1 26 #define BR_IN1 33
#define BL_IN2 25 #define BR_IN2 32
#define BL_PWM 22 #define BR_PWM 23
// Motor BL (Back Left)
#define BL_IN1 26
#define BL_IN2 25
#define BL_PWM 22
// PWM settings // PWM settings
#define PWM_FREQ 1000 #define PWM_FREQ 1000
#define PWM_RESOLUTION 8 // 0-255 #define PWM_RESOLUTION 8 // 0-255
#define SERIAL_RX 21 #define SERIAL_RX 21
// Sensor data received from micro sensors // Sensor data received from micro sensors
int sens_FL = -1; int sens_FL = -1;
int sens_FR = -1; int sens_FR = -1;
int sens_BL = -1; int sens_BL = -1;
int sens_BR = -1; int sens_BR = -1;
int sens_LF = -1; int sens_LF = -1;
int sens_LB = -1; int sens_LB = -1;
int sens_RF = -1; int sens_RF = -1;
int sens_RB = -1; int sens_RB = -1;
// Trim values to calibrate wheel speeds // Trim values to calibrate wheel speeds
int trimFL = 0; int trimFL = 0;
int trimFR = 0; int trimFR = 0;
int trimBL = 0; int trimBL = 0;
int trimBR = 0; int trimBR = 0;
#define BUFFER_SIZE 7 #define BUFFER_SIZE 7
int buffer_FL[BUFFER_SIZE]; int buffer_FL[BUFFER_SIZE];
@@ -62,28 +62,28 @@ const char* password = "hi123456";
int bufferIndex = 0; int bufferIndex = 0;
bool bufferFilled = false; bool bufferFilled = false;
bool ph1 = false; // Phase 1 complete flag bool ph1 = false; // Phase 1 complete flag
bool ph2 = false; //phase 2 complete flag bool ph2 = false; //phase 2 complete flag
bool ph3 = false; //phase 3 complete flag bool ph3 = false; //phase 3 complete flag
bool p2_stop = false; // Flag when its passed the top of the hill bool p2_stop = false; // Flag when its passed the top of the hill
// Telnet server to watch serial // Telnet server to watch serial
WiFiServer telnetServer(23); WiFiServer telnetServer(23);
WiFiClient telnetClient; WiFiClient telnetClient;
// Function declaritons // Function declaritons
void otaTask(void *parameter); // Get micro ready for OTA void otaTask(void *parameter); // Get micro ready for OTA
void logPrint(const char* msg); // Print function for serial and telnet, strings void logPrint(const char* msg); // Print function for serial and telnet, strings
void logPrint(int val); // Print function for serial and telnet, integers void logPrint(int val); // Print function for serial and telnet, integers
void logPrint(float val); // Print function for serial and telnet, floats void logPrint(float val); // Print function for serial and telnet, floats
void logPrintln(const char* msg); // Print line function for serial and telnet void logPrintln(const char* msg); // Print line function for serial and telnet
void logPrintln(int val); // Print line function for serial and telnet, integers void logPrintln(int val); // Print line function for serial and telnet, integers
void logPrintln(float val); // Print line function for serial and telnet, floats void logPrintln(float val); // Print line function for serial and telnet, floats
void telnetTask(void *parameter); // Configure telnet serial void telnetTask(void *parameter); // Configure telnet serial
void motorFR(int speed); // Front right motor, speed from -255 to 255, positive goes forward, negative goes backward void motorFR(int speed); // Front right motor, speed from -255 to 255, positive goes forward, negative goes backward
void motorFL(int speed); // Front left motor, speed from -255 to 255, positive goes forward, negative goes backward void motorFL(int speed); // Front left motor, speed from -255 to 255, positive goes forward, negative goes backward
void motorBR(int speed); // Back right motor, speed from -255 to 255, positive goes forward, negative goes backward void motorBR(int speed); // Back right motor, speed from -255 to 255, positive goes forward, negative goes backward
void motorBL(int speed); // Back left motor, speed from -255 to 255, positive goes forward, negative goes backward void motorBL(int speed); // Back left motor, speed from -255 to 255, positive goes forward, negative goes backward
void motorsStop(); // Stop all motors void motorsStop(); // Stop all motors
void motorsGo(int speed); // All motors move the same speed, from -255 to 255, positive goes forward, negative goes backward void motorsGo(int speed); // All motors move the same speed, from -255 to 255, positive goes forward, negative goes backward
void Go45right(); void Go45right();
@@ -102,103 +102,103 @@ void setup() {
Serial.begin(115200); Serial.begin(115200);
// Configure WIFI // Configure WIFI
WiFi.mode(WIFI_STA); WiFi.mode(WIFI_STA);
WiFi.begin(ssid, password); WiFi.begin(ssid, password);
int wifiAttempts = 0;
while (WiFi.waitForConnectResult() != WL_CONNECTED && wifiAttempts < 10) {
Serial.println("WiFi connecting...");
wifiAttempts++;
delay(500);
}
if (WiFi.status() == WL_CONNECTED) {
Serial.print("IP Address: ");
Serial.println(WiFi.localIP());
} else {
Serial.println("WiFi not available - running without network");
}
int wifiAttempts = 0;
if (WiFi.status() == WL_CONNECTED) { while (WiFi.waitForConnectResult() != WL_CONNECTED && wifiAttempts < 10) {
ArduinoOTA.setHostname("Motors"); Serial.println("WiFi connecting...");
ArduinoOTA.onStart([]() { wifiAttempts++;
Serial.println("OTA Update starting..."); delay(500);
});
ArduinoOTA.onEnd([]() {
Serial.println("\nOTA Update complete!");
});
ArduinoOTA.onError([](ota_error_t error) {
Serial.printf("OTA Error [%u]\n", error);
});
ArduinoOTA.begin();
telnetServer.begin();
} }
Serial2.begin(115200, SERIAL_8N1, SERIAL_RX, -1); if (WiFi.status() == WL_CONNECTED) {
Serial.print("IP Address: ");
Serial.println(WiFi.localIP());
} else {
Serial.println("WiFi not available - running without network");
}
// Motor direction pins
pinMode(FR_IN1, OUTPUT); if (WiFi.status() == WL_CONNECTED) {
pinMode(FR_IN2, OUTPUT); ArduinoOTA.setHostname("Motors");
pinMode(FL_IN1, OUTPUT); ArduinoOTA.onStart([]() {
pinMode(FL_IN2, OUTPUT); Serial.println("OTA Update starting...");
pinMode(BR_IN1, OUTPUT); });
pinMode(BR_IN2, OUTPUT); ArduinoOTA.onEnd([]() {
pinMode(BL_IN1, OUTPUT); Serial.println("\nOTA Update complete!");
pinMode(BL_IN2, OUTPUT); });
ArduinoOTA.onError([](ota_error_t error) {
// PWM setup for ESP32 Serial.printf("OTA Error [%u]\n", error);
motorAttach(FR_PWM, PWM_FREQ, PWM_RESOLUTION); });
ArduinoOTA.begin();
telnetServer.begin();
}
Serial2.begin(115200, SERIAL_8N1, SERIAL_RX, -1);
// Motor direction pins
pinMode(FR_IN1, OUTPUT);
pinMode(FR_IN2, OUTPUT);
pinMode(FL_IN1, OUTPUT);
pinMode(FL_IN2, OUTPUT);
pinMode(BR_IN1, OUTPUT);
pinMode(BR_IN2, OUTPUT);
pinMode(BL_IN1, OUTPUT);
pinMode(BL_IN2, OUTPUT);
// PWM setup for ESP32
motorAttach(FR_PWM, PWM_FREQ, PWM_RESOLUTION);
motorAttach(FL_PWM, PWM_FREQ, PWM_RESOLUTION); motorAttach(FL_PWM, PWM_FREQ, PWM_RESOLUTION);
motorAttach(BR_PWM, PWM_FREQ, PWM_RESOLUTION); motorAttach(BR_PWM, PWM_FREQ, PWM_RESOLUTION);
motorAttach(BL_PWM, PWM_FREQ, PWM_RESOLUTION); motorAttach(BL_PWM, PWM_FREQ, PWM_RESOLUTION);
// Start with motors stopped // Start with motors stopped
digitalWrite(FR_IN1, LOW); digitalWrite(FR_IN1, LOW);
digitalWrite(FR_IN2, LOW); digitalWrite(FR_IN2, LOW);
digitalWrite(FL_IN1, LOW); digitalWrite(FL_IN1, LOW);
digitalWrite(FL_IN2, LOW); digitalWrite(FL_IN2, LOW);
motorSpeed(FR_PWM, 0); motorSpeed(FR_PWM, 0);
motorSpeed(FL_PWM, 0); motorSpeed(FL_PWM, 0);
digitalWrite(BR_IN1, LOW); digitalWrite(BR_IN1, LOW);
digitalWrite(BR_IN2, LOW); digitalWrite(BR_IN2, LOW);
digitalWrite(BL_IN1, LOW); digitalWrite(BL_IN1, LOW);
digitalWrite(BL_IN2, LOW); digitalWrite(BL_IN2, LOW);
motorSpeed(BR_PWM, 0); motorSpeed(BR_PWM, 0);
motorSpeed(BL_PWM, 0); motorSpeed(BL_PWM, 0);
// FreeRTOS tasks // FreeRTOS tasks
if (WiFi.status() == WL_CONNECTED) { if (WiFi.status() == WL_CONNECTED) {
xTaskCreate(otaTask, "OTA", 4096, NULL, 1, NULL); xTaskCreate(otaTask, "OTA", 4096, NULL, 1, NULL);
xTaskCreate(telnetTask, "Telnet", 4096, NULL, 1, NULL); xTaskCreate(telnetTask, "Telnet", 4096, NULL, 1, NULL);
} }
} }
void loop() { void loop() {
// ph1 = true; // ph1 = true;
// ph2 = true; // ph2 = true;
// ph3 = true; ph3 = true;
if (!ph1) { if (!ph1) {
phaseOne(); phaseOne();
ph1 = true; ph1 = true;
} }
if (!ph2) { if (!ph2) {
phaseTwo(); phaseTwo();
ph2 = true; ph2 = true;
} }
if (!ph3) { if (!ph3) {
phaseThree(); phaseThree();
ph3 = true; ph3 = true;
} }
// motorsTest(); // motorsTest();
sensorsTest(); // sensorsTest();
} }
@@ -235,10 +235,10 @@ void phaseOne() {
motorsGo(80); motorsGo(80);
delay(1250); // Go into the pit 1800 miliseconds, delay(1250); // Go into the pit 1800 miliseconds,
motorsStop(); motorsStop();
delay(3000); //how long to pick balls? dunno delay(3000); //how long to pick balls? dunno
motorsGo(-150); motorsGo(-150);
delay(1200); // Go out of the pit 1800 miliseconds delay(1200); // Go out of the pit 1800 miliseconds
motorsStop(); motorsStop();
delay(500); delay(500);
slideLeft(); slideLeft();
delay(1450); delay(1450);
@@ -256,14 +256,14 @@ void phaseTwo() {
receiveSensorData(); receiveSensorData();
while (!p2_stop) { // while (!p2_stop) {
while ((sens_FL + sens_FR) / 2 > 8000) { while ((sens_FL + sens_FR) / 2 > 8000) {
motorsGo(-255); motorsGo(-230);
delay(5); // Wait for new sensor data to arrive delay(5); // Wait for new sensor data to arrive
receiveSensorData(); // Get fresh reading each iteration receiveSensorData(); // Get fresh reading each iteration
} }
while ((sens_FL + sens_FR) / 2 < 680) { // Used to be 680 while ((sens_FL + sens_FR) / 2 < 800) { // Used to be 680
motorsGo(-180); motorsGo(-250);
delay(5); // Wait for new sensor data to arrive delay(5); // Wait for new sensor data to arrive
receiveSensorData(); // Get fresh reading each iteration receiveSensorData(); // Get fresh reading each iteration
} }
@@ -272,9 +272,9 @@ void phaseTwo() {
delay(5); // Wait for new sensor data to arrive delay(5); // Wait for new sensor data to arrive
receiveSensorData(); // Get fresh reading each iteration receiveSensorData(); // Get fresh reading each iteration
} }
p2_stop = true; // p2_stop = true;
receiveSensorData(); // receiveSensorData();
} // }
motorsStop(); motorsStop();
@@ -286,147 +286,147 @@ void phaseThree() {
} }
void otaTask(void *parameter) { void otaTask(void *parameter) {
for (;;) { for (;;) {
ArduinoOTA.handle(); ArduinoOTA.handle();
vTaskDelay(10 / portTICK_PERIOD_MS); vTaskDelay(10 / portTICK_PERIOD_MS);
} }
} }
void telnetTask(void *parameter) { void telnetTask(void *parameter) {
for (;;) { for (;;) {
if (telnetServer.hasClient()) { if (telnetServer.hasClient()) {
if (telnetClient && telnetClient.connected()) { if (telnetClient && telnetClient.connected()) {
telnetClient.stop(); telnetClient.stop();
} }
telnetClient = telnetServer.available(); telnetClient = telnetServer.available();
telnetClient.println("Connected to Motors"); telnetClient.println("Connected to Motors");
} }
vTaskDelay(100 / portTICK_PERIOD_MS); vTaskDelay(100 / portTICK_PERIOD_MS);
} }
}
void logPrint(const char* msg) {
Serial.print(msg);
if (telnetClient && telnetClient.connected()) {
telnetClient.print(msg);
}
}
void logPrint(int val) {
Serial.print(val);
if (telnetClient && telnetClient.connected()) {
telnetClient.print(val);
}
}
void logPrint(float val) {
Serial.print(val);
if (telnetClient && telnetClient.connected()) {
telnetClient.print(val);
}
}
void logPrintln(const char* msg) {
Serial.println(msg);
if (telnetClient && telnetClient.connected()) {
telnetClient.println(msg);
}
}
void logPrintln(int val) {
Serial.println(val);
if (telnetClient && telnetClient.connected()) {
telnetClient.println(val);
}
}
void logPrintln(float val) {
Serial.println(val);
if (telnetClient && telnetClient.connected()) {
telnetClient.println(val);
}
} }
void motorFR(int speed) { void logPrint(const char* msg) {
if (speed > 0) { Serial.print(msg);
digitalWrite(FR_IN1, HIGH); if (telnetClient && telnetClient.connected()) {
digitalWrite(FR_IN2, LOW); telnetClient.print(msg);
motorSpeed(FR_PWM, constrain(speed + trimFR, 0, 255)); }
} else if (speed < 0) { }
digitalWrite(FR_IN1, LOW);
digitalWrite(FR_IN2, HIGH);
motorSpeed(FR_PWM, constrain(-speed + trimFR, 0, 255));
} else {
digitalWrite(FR_IN1, LOW);
digitalWrite(FR_IN2, LOW);
motorSpeed(FR_PWM, 0);
}
}
void motorFL(int speed) {
if (speed > 0) {
digitalWrite(FL_IN1, HIGH);
digitalWrite(FL_IN2, LOW);
motorSpeed(FL_PWM, constrain(speed + trimFL, 0, 255));
} else if (speed < 0) {
digitalWrite(FL_IN1, LOW);
digitalWrite(FL_IN2, HIGH);
motorSpeed(FL_PWM, constrain(-speed + trimFL, 0, 255));
} else {
digitalWrite(FL_IN1, LOW);
digitalWrite(FL_IN2, LOW);
motorSpeed(FL_PWM, 0);
}
}
void motorBR(int speed) { void logPrint(int val) {
if (speed > 0) { Serial.print(val);
digitalWrite(BR_IN1, HIGH); if (telnetClient && telnetClient.connected()) {
digitalWrite(BR_IN2, LOW); telnetClient.print(val);
motorSpeed(BR_PWM, constrain(speed + trimBR, 0, 255)); }
} else if (speed < 0) { }
digitalWrite(BR_IN1, LOW);
digitalWrite(BR_IN2, HIGH);
motorSpeed(BR_PWM, constrain(-speed + trimBR, 0, 255));
} else {
digitalWrite(BR_IN1, LOW);
digitalWrite(BR_IN2, LOW);
motorSpeed(BR_PWM, 0);
}
}
void motorBL(int speed) {
if (speed > 0) {
digitalWrite(BL_IN1, HIGH);
digitalWrite(BL_IN2, LOW);
motorSpeed(BL_PWM, constrain(speed + trimBL, 0, 255));
} else if (speed < 0) {
digitalWrite(BL_IN1, LOW);
digitalWrite(BL_IN2, HIGH);
motorSpeed(BL_PWM, constrain(-speed + trimBL, 0, 255));
} else {
digitalWrite(BL_IN1, LOW);
digitalWrite(BL_IN2, LOW);
motorSpeed(BL_PWM, 0);
}
}
void motorsStop() { void logPrint(float val) {
motorFR(0); Serial.print(val);
motorFL(0); if (telnetClient && telnetClient.connected()) {
motorBR(0); telnetClient.print(val);
motorBL(0); }
}
void logPrintln(const char* msg) {
Serial.println(msg);
if (telnetClient && telnetClient.connected()) {
telnetClient.println(msg);
}
}
void logPrintln(int val) {
Serial.println(val);
if (telnetClient && telnetClient.connected()) {
telnetClient.println(val);
}
}
void logPrintln(float val) {
Serial.println(val);
if (telnetClient && telnetClient.connected()) {
telnetClient.println(val);
}
}
void motorFR(int speed) {
if (speed > 0) {
digitalWrite(FR_IN1, HIGH);
digitalWrite(FR_IN2, LOW);
motorSpeed(FR_PWM, constrain(speed + trimFR, 0, 255));
} else if (speed < 0) {
digitalWrite(FR_IN1, LOW);
digitalWrite(FR_IN2, HIGH);
motorSpeed(FR_PWM, constrain(-speed + trimFR, 0, 255));
} else {
digitalWrite(FR_IN1, LOW);
digitalWrite(FR_IN2, LOW);
motorSpeed(FR_PWM, 0);
}
}
void motorFL(int speed) {
if (speed > 0) {
digitalWrite(FL_IN1, HIGH);
digitalWrite(FL_IN2, LOW);
motorSpeed(FL_PWM, constrain(speed + trimFL, 0, 255));
} else if (speed < 0) {
digitalWrite(FL_IN1, LOW);
digitalWrite(FL_IN2, HIGH);
motorSpeed(FL_PWM, constrain(-speed + trimFL, 0, 255));
} else {
digitalWrite(FL_IN1, LOW);
digitalWrite(FL_IN2, LOW);
motorSpeed(FL_PWM, 0);
}
}
void motorBR(int speed) {
if (speed > 0) {
digitalWrite(BR_IN1, HIGH);
digitalWrite(BR_IN2, LOW);
motorSpeed(BR_PWM, constrain(speed + trimBR, 0, 255));
} else if (speed < 0) {
digitalWrite(BR_IN1, LOW);
digitalWrite(BR_IN2, HIGH);
motorSpeed(BR_PWM, constrain(-speed + trimBR, 0, 255));
} else {
digitalWrite(BR_IN1, LOW);
digitalWrite(BR_IN2, LOW);
motorSpeed(BR_PWM, 0);
}
}
void motorBL(int speed) {
if (speed > 0) {
digitalWrite(BL_IN1, HIGH);
digitalWrite(BL_IN2, LOW);
motorSpeed(BL_PWM, constrain(speed + trimBL, 0, 255));
} else if (speed < 0) {
digitalWrite(BL_IN1, LOW);
digitalWrite(BL_IN2, HIGH);
motorSpeed(BL_PWM, constrain(-speed + trimBL, 0, 255));
} else {
digitalWrite(BL_IN1, LOW);
digitalWrite(BL_IN2, LOW);
motorSpeed(BL_PWM, 0);
}
}
void motorsStop() {
motorFR(0);
motorFL(0);
motorBR(0);
motorBL(0);
} }
int rollingAverage(int buffer[]) { int rollingAverage(int buffer[]) {
int sum = 0; int sum = 0;
int count = bufferFilled ? BUFFER_SIZE : (bufferIndex + 1); int count = bufferFilled ? BUFFER_SIZE : (bufferIndex + 1);
for (int i = 0; i < count; i++) { for (int i = 0; i < count; i++) {
sum += buffer[i]; sum += buffer[i];
} }
return sum / count; return sum / count;
} }
@@ -460,155 +460,155 @@ void motorBR(int speed) {
sens_LB = values[5]; sens_LB = values[5];
sens_RF = values[6]; sens_RF = values[6];
sens_RB = values[7]; sens_RB = values[7];
} }
} }
void sensorsTest() { void sensorsTest() {
receiveSensorData(); receiveSensorData();
logPrint("FL:"); logPrint(sens_FL); logPrint("FL:"); logPrint(sens_FL);
logPrint(" FR:"); logPrint(sens_FR); logPrint(" FR:"); logPrint(sens_FR);
logPrint(" BL:"); logPrint(sens_BL); logPrint(" BL:"); logPrint(sens_BL);
logPrint(" BR:"); logPrint(sens_BR); logPrint(" BR:"); logPrint(sens_BR);
logPrint(" LF:"); logPrint(sens_LF); logPrint(" LF:"); logPrint(sens_LF);
logPrint(" LB:"); logPrint(sens_LB); logPrint(" LB:"); logPrint(sens_LB);
logPrint(" RF:"); logPrint(sens_RF); logPrint(" RF:"); logPrint(sens_RF);
logPrint(" RB:"); logPrintln(sens_RB); logPrint(" RB:"); logPrintln(sens_RB);
delay(100); delay(100);
} }
void motorsTest() { void motorsTest() {
logPrintln("Right forward..."); logPrintln("Right forward...");
motorBR(80); motorBR(80);
motorFR(80);
delay(2000);
logPrintln("Right stop...");
motorBR(0);
motorFR(0);
delay(1000);
logPrintln("Right forward...");
motorBR(250);
motorFR(250);
delay(2000);
logPrintln("Right stop...");
motorBR(0);
motorFR(0);
delay(1000);
logPrintln("Left forward...");
motorBL(80);
motorFL(80);
delay(2000);
logPrintln("Left stop...");
motorsStop();
delay(1000);
logPrintln("Left forward...");
motorBL(250);
motorFL(250);
delay(2000);
logPrintln("Left stop...");
motorsStop();
delay(1000);
logPrintln("Front forward...");
motorFR(80); motorFR(80);
motorFL(80); delay(2000);
delay(2000);
logPrintln("Front stop...");
motorsStop();
delay(1000);
logPrintln("Front forward..."); logPrintln("Right stop...");
motorFR(250); motorBR(0);
motorFL(250); motorFR(0);
delay(2000); delay(1000);
logPrintln("Front stop..."); logPrintln("Right forward...");
motorsStop();
delay(1000);
logPrintln("Back forward...");
motorBR(80);
motorBL(80);
delay(2000);
logPrintln("Back stop...");
motorsStop();
delay(1000);
logPrintln("Back forward...");
motorBR(250); motorBR(250);
motorBL(250); motorFR(250);
delay(2000); delay(2000);
logPrintln("Back stop..."); logPrintln("Right stop...");
motorBR(0);
motorFR(0);
delay(1000);
logPrintln("Left forward...");
motorBL(80);
motorFL(80);
delay(2000);
logPrintln("Left stop...");
motorsStop(); motorsStop();
delay(1000); delay(1000);
logPrintln("Left forward...");
motorBL(250);
motorFL(250);
delay(2000);
logPrintln("Left stop...");
motorsStop();
delay(1000);
logPrintln("Front forward...");
motorFR(80);
motorFL(80);
delay(2000);
logPrintln("Front stop...");
motorsStop();
delay(1000);
logPrintln("Front forward...");
motorFR(250);
motorFL(250);
delay(2000);
logPrintln("Front stop...");
motorsStop();
delay(1000);
logPrintln("Back forward...");
motorBR(80);
motorBL(80);
delay(2000);
logPrintln("Back stop...");
motorsStop();
delay(1000);
logPrintln("Back forward...");
motorBR(250);
motorBL(250);
delay(2000);
logPrintln("Back stop...");
motorsStop();
delay(1000);
} }
void motorsGo(int speed) { void motorsGo(int speed) {
motorFR(speed); motorFR(speed);
motorFL(speed); motorFL(speed);
motorBR(speed); motorBR(speed);
motorBL(speed); motorBL(speed);
} }
void Go45right() { void Go45right() {
motorFL(150); motorFL(150);
motorFR(-150); motorFR(-150);
motorBL(150); motorBL(150);
motorBR(-150); motorBR(-150);
delay(875); delay(875);
motorsStop(); motorsStop();
} }
void Go45left() { void Go45left() {
motorFL(-150); motorFL(-150);
motorFR(150); motorFR(150);
motorBL(-150); motorBL(-150);
motorBR(150); motorBR(150);
delay(875); delay(875);
motorsStop(); motorsStop();
} }
void Go90right() { void Go90right() {
motorFL(150); motorFL(150);
motorFR(-150); motorFR(-150);
motorBL(150); motorBL(150);
motorBR(-150); motorBR(-150);
delay(1250); delay(1250);
motorsStop(); motorsStop();
} }
void Go90left() { void Go90left() {
motorFL(-150); motorFL(-150);
motorFR(150); motorFR(150);
motorBL(-150); motorBL(-150);
motorBR(150); motorBR(150);
delay(1250); delay(1250);
motorsStop(); motorsStop();
} }
void slideRight() { void slideRight() {
motorFL(-150); motorFL(-150);
motorFR(150); motorFR(150);
motorBL(150); motorBL(150);
motorBR(-150); motorBR(-150);
} }
void slideLeft() { void slideLeft() {
motorFL(150); motorFL(150);
motorFR(-150); motorFR(-150);
motorBL(-150); motorBL(-150);
motorBR(150); motorBR(150);
} }