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