From bc981bcb07457c0cc42fc149f1b4fb853b0b4873 Mon Sep 17 00:00:00 2001 From: p-eanat Date: Mon, 24 Jun 2024 09:33:02 -0700 Subject: [PATCH] Failsafe testing, detach servo when parachute not enabled --- src/SUAS_Link_2024/src/main.cpp | 19 +++++++++++++++---- src/SUAS_Link_2024/src/params.h | 2 +- src/main/encoder_steering.cpp | 29 ++++++++++++++++++----------- src/main/encoder_steering.h | 3 +++ 4 files changed, 37 insertions(+), 16 deletions(-) diff --git a/src/SUAS_Link_2024/src/main.cpp b/src/SUAS_Link_2024/src/main.cpp index 37d804c..e2645c4 100644 --- a/src/SUAS_Link_2024/src/main.cpp +++ b/src/SUAS_Link_2024/src/main.cpp @@ -96,7 +96,7 @@ 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(0, 0, drop_data, &des_drop_data); // TODO: REMOVE LATER!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! + calc_des_drop_state(1, 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)); @@ -106,9 +106,9 @@ void loop() { Serial.print("Sent desired drop point to Pi: "); 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,6 +126,17 @@ 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); + } + des_drop_data.bottleID = 2; notDropped = false; Serial.println("Failsafe mode: Left bottle dropped."); diff --git a/src/SUAS_Link_2024/src/params.h b/src/SUAS_Link_2024/src/params.h index 0ab9454..6311b6f 100644 --- a/src/SUAS_Link_2024/src/params.h +++ b/src/SUAS_Link_2024/src/params.h @@ -20,7 +20,7 @@ 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 0.0 // TODO: find this from testing, it's arbitrary rn!! +#define DRIFT_FACTOR 10.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 diff --git a/src/main/encoder_steering.cpp b/src/main/encoder_steering.cpp index cac3408..f01264e 100644 --- a/src/main/encoder_steering.cpp +++ b/src/main/encoder_steering.cpp @@ -12,6 +12,7 @@ QueueHandle_t steeringQueue = NULL; #define SERVO_CONTROL_PERIOD 10 // ms volatile int PID_Control_Period = SERVO_CONTROL_PERIOD; +bool servo_enable = false; void steering_setup(double acquireRate, double kp, double ki, double kd) { @@ -31,16 +32,6 @@ void steering_setup(double acquireRate, double kp, double ki, double kd) { PID_Control_Period = (1.0/acquireRate)*1000; // pid.setDeadband(DIST_PER_TICK); - // Initialize servos - Serial.println("Initializing servos"); - servo_1.attach(SERVO_1); - servo_1.setPeriodHertz(SERVO_FREQ); - servo_2.attach(SERVO_2); - servo_2.setPeriodHertz(SERVO_FREQ); - // No spin - servo_1.write(90); - servo_2.write(90); - // Create queue steeringQueue = xQueueCreate(1, sizeof(AngleData)); // this queue will take the yaw value if (steeringQueue == NULL) { @@ -67,7 +58,19 @@ void steering_setup(double acquireRate, double kp, double ki, double kd) { } else { Serial.println("Servo control task created"); } +} + +void init_servos() { + // Initialize servos + Serial.println("Initializing servos"); + servo_1.attach(SERVO_1); + servo_1.setPeriodHertz(SERVO_FREQ); + servo_2.attach(SERVO_2); + servo_2.setPeriodHertz(SERVO_FREQ); + // No spin + servo_1.write(90); + servo_2.write(90); } @@ -91,10 +94,14 @@ double angle_diff(double angle1, double angle2) { void servo_control(AngleData data) { if (data.desiredForward == -1){ - do_nothing(); return; } + if (!servo_enable) { + servo_enable = true; + init_servos(); + } + SteeringData des_lengths; SteeringData current_lengths = {(double)DIST_PER_TICK*((int)count_1/2), (double)DIST_PER_TICK*((int)count_2/2)}; diff --git a/src/main/encoder_steering.h b/src/main/encoder_steering.h index 8df0577..2fea534 100644 --- a/src/main/encoder_steering.h +++ b/src/main/encoder_steering.h @@ -39,6 +39,9 @@ extern double current_heading; extern volatile bool forward_1; extern volatile bool forward_2; +extern bool servo_enable; + +void init_servos(); void steering_setup(double acquireRate, double kp, double ki, double kd); double angle_diff(double angle1, double angle2); void servo_control(AngleData data);