From 12f9a830fa2672101b4c99c949a3d6f0f6a14e1f Mon Sep 17 00:00:00 2001 From: p-eanat Date: Tue, 25 Jun 2024 06:50:15 -0700 Subject: [PATCH] What worked last night --- src/SUAS_Link_2024/src/main.cpp | 42 +++++++++++++++++++-------------- src/SUAS_Link_2024/src/params.h | 12 +++++----- src/main/encoder_steering.cpp | 13 +++++++--- src/main/gpsLowLevel.h | 2 +- src/main/main.ino | 3 ++- src/main/sensors.h | 2 +- 6 files changed, 44 insertions(+), 30 deletions(-) diff --git a/src/SUAS_Link_2024/src/main.cpp b/src/SUAS_Link_2024/src/main.cpp index e2645c4..66f0434 100644 --- a/src/SUAS_Link_2024/src/main.cpp +++ b/src/SUAS_Link_2024/src/main.cpp @@ -52,8 +52,13 @@ void setup() { peerInfo.encrypt = false; // Register parachute boards memcpy(peerInfo.peer_addr, ADDRESS_1, 6); + if (esp_now_add_peer(&peerInfo) != ESP_OK) { Serial.println("Failed to add peer"); } memcpy(peerInfo.peer_addr, ADDRESS_2, 6); if (esp_now_add_peer(&peerInfo) != ESP_OK) { Serial.println("Failed to add peer"); } + memcpy(peerInfo.peer_addr, ADDRESS_3, 6); + if (esp_now_add_peer(&peerInfo) != ESP_OK) { Serial.println("Failed to add peer"); } + memcpy(peerInfo.peer_addr, ADDRESS_4, 6); + if (esp_now_add_peer(&peerInfo) != ESP_OK) { Serial.println("Failed to add peer"); } digitalWrite(LED_RED, LOW); // Red LED off indicates comminucation established @@ -81,10 +86,11 @@ void loop() { // Read drop point and bottle number from Pi (blocking) // drop_data = recieveData(); - drop_data.lon = -123.072022; // For testing REMOVE LATER!!!!!!!!!!!!!!!!!!!!!!! - drop_data.lat = 49.244365; + + drop_data.lon = -76.5207332; // For testing REMOVE LATER!!!!!!!!!!!!!!!!!!!!!!! + drop_data.lat = 38.3022803; drop_data.heading = 0.0; - drop_data.bottleID = 2; + drop_data.bottleID = 4; bool notDropped = true; snprintf(buffer, sizeof(buffer), "Received data from Pi: %.8f,%.8f,%.2f,%d\n", drop_data.lat, drop_data.lon, drop_data.heading, drop_data.bottleID); Serial.print(buffer); @@ -96,19 +102,19 @@ void loop() { // Serial.println("Wind: " + String(windspeed) + ", " + String(wind_heading)); // calc_des_drop_state(windspeed, wind_heading, drop_data, &des_drop_data); - calc_des_drop_state(1, 0, drop_data, &des_drop_data); // TODO: REMOVE LATER!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! + calc_des_drop_state(0, 0, drop_data, &des_drop_data); // TODO: REMOVE LATER!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! // Serial.println(latitudeToMeters(des_drop_data.lat)-latitudeToMeters(drop_data.lat)); // Serial.println(latitudeToMeters(des_drop_data.lon)-latitudeToMeters(drop_data.lon)); // Send desired drop point to Pi snprintf(buffer, sizeof(buffer), "%.8f,%.8f,%.2f,%d\n", des_drop_data.lat, des_drop_data.lon, des_drop_data.heading, des_drop_data.bottleID); PiSerial.print(buffer); - Serial.print("Sent desired drop point to Pi: "); Serial.print(buffer); + Serial.print("Sent desired drop point to Pi (not): "); Serial.print(buffer); // Send message to parachutes (try 3 times; this is time sensitive so nothing we can do if it fails) - // for (int i = 0; i < 3; i++) { - // broadcastMessage(des_drop_data); - // } + for (int i = 0; i < 3; i++) { + broadcastMessage(des_drop_data); + } // Wait to get close enough to desired drop point digitalWrite(LED_RED, HIGH); @@ -126,16 +132,16 @@ void loop() { Serial.println("Failsafe mode: Right bottle dropped."); } if (msg.servo_output_raw.servo14_raw > 1500) { // If servo 14 is high, then drop the left bottle - drop_data.lon = msg.global_position_int.lon / 10000000.0; // For testing REMOVE LATER!!!!!!!!!!!!!!!!!!!!!!! - drop_data.lat = msg.global_position_int.lat / 10000000.0; - drop_data.heading = 0.0; - drop_data.bottleID = 2; - snprintf(buffer, sizeof(buffer), "Received data from failsafe activation: %.8f,%.8f,%.2f,%d\n", drop_data.lat, drop_data.lon, drop_data.heading, drop_data.bottleID); Serial.print(buffer); - calc_des_drop_state(1, 0, drop_data, &des_drop_data); - snprintf(buffer, sizeof(buffer), "Destination: %.8f,%.8f,%.2f,%d\n", des_drop_data.lat, des_drop_data.lon, des_drop_data.heading, des_drop_data.bottleID); Serial.print(buffer); - for (int i = 0; i < 3; i++) { - broadcastMessage(des_drop_data); - } + // drop_data.lon = msg.global_position_int.lon / 10000000.0; // For testing REMOVE LATER!!!!!!!!!!!!!!!!!!!!!!! + // drop_data.lat = msg.global_position_int.lat / 10000000.0; + // drop_data.heading = 0.0; + // drop_data.bottleID = 2; + // snprintf(buffer, sizeof(buffer), "Received data from failsafe activation: %.8f,%.8f,%.2f,%d\n", drop_data.lat, drop_data.lon, drop_data.heading, drop_data.bottleID); Serial.print(buffer); + // calc_des_drop_state(1, 0, drop_data, &des_drop_data); + // snprintf(buffer, sizeof(buffer), "Destination: %.8f,%.8f,%.2f,%d\n", des_drop_data.lat, des_drop_data.lon, des_drop_data.heading, des_drop_data.bottleID); Serial.print(buffer); + // for (int i = 0; i < 3; i++) { + // broadcastMessage(des_drop_data); + // } des_drop_data.bottleID = 2; notDropped = false; diff --git a/src/SUAS_Link_2024/src/params.h b/src/SUAS_Link_2024/src/params.h index 6311b6f..d5d0b9a 100644 --- a/src/SUAS_Link_2024/src/params.h +++ b/src/SUAS_Link_2024/src/params.h @@ -19,10 +19,10 @@ extern HardwareSerial CubeSerial; #define M_PER_LAT_DEG 111320.0 // 111320 meters per degree of latitude (short distance approx) -#define AIRCRAFT_SPEED 7.0 // m/s -#define DRIFT_FACTOR 10.0 // TODO: find this from testing, it's arbitrary rn!! +#define AIRCRAFT_SPEED 12.0 // m/s +#define DRIFT_FACTOR 1.0 // TODO: find this from testing, it's arbitrary rn!! #define RELEASE_DELAY 0.0 // s // TODO!!! -#define RELEASE_MARGIN 6.0 // Tolerance margin from desired drop point that we will trigger release +#define RELEASE_MARGIN 5.0 // Tolerance margin from desired drop point that we will trigger release #define WINDOW_SIZE 10 @@ -31,8 +31,8 @@ extern HardwareSerial CubeSerial; // MAC Address of responder - edit as required static const uint8_t ADDRESS_1[] = {0xD8, 0xBC, 0x38, 0xE4, 0x9E, 0x5C}; // Parachute #1 static const uint8_t ADDRESS_2[] = {0x94, 0xE6, 0x86, 0x92, 0xA7, 0xFC}; // Parachute #2 -// static const uint8_t ADDRESS_3[] = {0x, 0x, 0x, 0x, 0x, 0x}; -// static const uint8_t ADDRESS_4[] = {0x, 0x, 0x, 0x, 0x, 0x}; -// static const uint8_t ADDRESS_5[] = {0x, 0x, 0x, 0x, 0x, 0x}; +static const uint8_t ADDRESS_3[] = {0x94, 0xE6, 0x86, 0x92, 0xA7, 0xE4}; // Parachute #3 +static const uint8_t ADDRESS_4[] = {0x94, 0xE6, 0x86, 0x92, 0xA8, 0xCC}; // Parachute #4 +// static const uint8_t ADDRESS_ALL[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; #endif \ No newline at end of file diff --git a/src/main/encoder_steering.cpp b/src/main/encoder_steering.cpp index f01264e..20c2276 100644 --- a/src/main/encoder_steering.cpp +++ b/src/main/encoder_steering.cpp @@ -93,7 +93,14 @@ double angle_diff(double angle1, double angle2) { */ void servo_control(AngleData data) { - if (data.desiredForward == -1){ + // Serial,printf("Data Packet: %lf \t %lf \t %lf\n", data.currentYaw, data.desiredForward, data.desiredYaw); + + if (data.desiredForward == -1.0){ + if (servo_enable) { + servo_1.detach(); + servo_2.detach(); + servo_enable = false; + } return; } @@ -152,13 +159,13 @@ void servoControlTask(void *pvParameters) { // initialize the incoming data incomingData.desiredYaw = 0; incomingData.currentYaw = 0; - incomingData.desiredForward = 0; + incomingData.desiredForward = -1.0; while (true) { // Check if we have incoming data if (xQueueReceive(steeringQueue, &incomingData, 0) == pdTRUE) { // Successfully received data - Serial.printf("Desired yaw: %lf, Current yaw: %lf, Desired forward: %lf\n", incomingData.desiredYaw, incomingData.currentYaw, incomingData.desiredForward); + // Serial.printf("Desired yaw: %lf, Current yaw: %lf, Desired forward: %lf\n", incomingData.desiredYaw, incomingData.currentYaw, incomingData.desiredForward); } // Call servo_control function here servo_control(incomingData); // Modify parameters as needed diff --git a/src/main/gpsLowLevel.h b/src/main/gpsLowLevel.h index a6104f3..341fa7d 100644 --- a/src/main/gpsLowLevel.h +++ b/src/main/gpsLowLevel.h @@ -26,7 +26,7 @@ typedef struct{ class gpsLowLevel { public: - gpsLowLevel(HardwareSerial *gpsSerial = &Serial2, uint32_t GPSBaudRate = 115200, uint8_t GPSRate = 5, uint8_t RXPin = 2, uint8_t TXPin = 4); + gpsLowLevel(HardwareSerial *gpsSerial = &Serial2, uint32_t GPSBaudRate = 115200, uint8_t GPSRate = 5, uint8_t RXPin = 4, uint8_t TXPin = 2); enum GPS_Status { GPS_OK, diff --git a/src/main/main.ino b/src/main/main.ino index 39bc748..c362551 100644 --- a/src/main/main.ino +++ b/src/main/main.ino @@ -402,9 +402,10 @@ void ComputePID(){ // Send the data packet to the queue (i.e. activate steering), if detect that parachute has fallen below HEIGHT_THRESH (and if it was given a coordinate to go to lol) // double height = X_Zaxis(0, 0); double height = sensorData_inst.barometerData.Altitude - sensorData_inst.barometerData.AltitudeOffset; - // Serial.println("\nHeight: " + String(height) + "\n"); + Serial.println("\nHeight: " + String(height) + "\n"); if (height <= configData_inst.HEIGHT_THRESH && height >= 1.0 && target_lon != 1000.0 && target_lat != 1000.0){ // Make the Data packet + // Serial.printf("Height matched if condition. Value: %lf\n", height); AngleData data = {setpoint, sensorData_inst.imuData.EulerAngles.v2, 0}; sendSteeringData(data); } diff --git a/src/main/sensors.h b/src/main/sensors.h index d4023b5..5300e54 100644 --- a/src/main/sensors.h +++ b/src/main/sensors.h @@ -37,7 +37,7 @@ #define GPSBAUDRATE 115200 #define GPS_RX 4 #define GPS_TX 2 -#define GPS_RATE 5 +#define GPS_RATE 10 // BATTERY MEASUREMENT #define BATTERY_PIN 14