Comp day3 force fixes

src/sailboat_main/sailboat_main/trim_sail/trim_sail.py

@@ -55,14 +55,15 @@ def calculate_sail_angles(self, wind_dir):
         half_no_go = constants.WIND.NO_GO_WIDTH / 2
 
         # Set the jib on the port side of the boat if we're on the left side of the no-go zone.
-        jib_side_flag = constants.PHYSICAL.JIB_SIDE_PORT if wind_dir > constants.WIND.NO_GO_CENTER else constants.PHYSICAL.JIB_SIDE_STB
+        jib_side_flag = constants.PHYSICAL.JIB_SIDE_PORT if wind_dir < constants.WIND.NO_GO_CENTER else constants.PHYSICAL.JIB_SIDE_STB
 
         # No longer care about side: map angles on the left side of the no-go zone symmetrically to the right side.
         if 180 < wind_dir < 360: wind_dir = 360 - wind_dir
 
         if wind_dir > (constants.WIND.NO_GO_CENTER - half_no_go):  # In the no-go zone.
             mainsail_angle = constants.PHYSICAL.MAINSAIL_MIN_ANGLE
             jib_angle = constants.PHYSICAL.JIB_MIN_ANGLE
+            self.get_logger().info(f"=== BOAT IN IRONS. ===")
         else:  # Not in no-go zone: linearly map wind direction to sail angles.
             mainsail_angle = int(np.interp(wind_dir, [0, constants.WIND.NO_GO_CENTER - half_no_go],
                                     [constants.PHYSICAL.MAINSAIL_MAX_ANGLE, constants.PHYSICAL.MAINSAIL_MIN_ANGLE]))

teensy/src/Control Tasks/SerialControlTask.cpp

@@ -2,29 +2,24 @@
 
 SerialControlTask::SerialControlTask() : last_telemetry_send_time(0), current_time(0), send_telemetry(false) {}
 
-void SerialControlTask::execute()
-{
-    if (current_time - last_telemetry_send_time >= constants::serial::TX_PERIOD_MS)
-    {
-        send_telemetry = true;
-    }
-    if (send_telemetry)
-    {
-        uint8_t data[] = {
+void SerialControlTask::execute() {
+    if (current_time - last_telemetry_send_time >= constants::serial::TX_PERIOD_MS) send_telemetry = true;
+
+    if (send_telemetry) {
+        const uint8_t data[] = {
             constants::serial::TX_START_FLAG,
-            static_cast<uint8_t>((uint8_t)(sfr::anemometer::wind_angle >> 8)),
-            static_cast<uint8_t>((uint8_t)(sfr::anemometer::wind_angle & 0xFF)),
+            static_cast<uint8_t>(sfr::anemometer::wind_angle >> 8),
+            static_cast<uint8_t>(sfr::anemometer::wind_angle & 0xFF),
             sfr::servo::mainsail_angle,
             sfr::servo::rudder_angle,
             sfr::servo::jib_angle,
             sfr::servo::jib_side_flag,
             sfr::serial::dropped_packets,
             constants::serial::TX_END_FLAG
         };
-
         last_telemetry_send_time = millis();
         Serial.write(data, sizeof(data));
         send_telemetry = false;
     }
     current_time = millis();
-}
+}

teensy/src/Control Tasks/SerialControlTask.hpp

@@ -10,4 +10,4 @@ class SerialControlTask {
     uint32_t last_telemetry_send_time;
     uint32_t current_time;
     bool send_telemetry;
-};
+};

teensy/src/Control Tasks/ServoControlTask.cpp

@@ -12,59 +12,32 @@ ServoControlTask::ServoControlTask() {
     // Pre-set the rudder to center and all sail servos to "all-out" (for manual setup).
     actuate_servo(rudder_servo, constants::servo::RUDDER_MID_PULSE);
     actuate_servo(mainsail_servo, constants::servo::MAINSAIL_MIN_PULSE);
-    actuate_servo(jib_port_servo, constants::servo::JIB_PORT_MIN_PULSE);
-    actuate_servo(jib_stb_servo, constants::servo::JIB_STB_MIN_PULSE);
+    actuate_servo(jib_port_servo, constants::servo::JIB_PORT_MAX_PULSE);
+    actuate_servo(jib_stb_servo, constants::servo::JIB_STB_MAX_PULSE);
 }
 
 /**
- *  Updates the servos if valid serial data is ready and waiting.
- *  Radio mode (radio_flag != 0) takes priority over Jetson/ROS mode.
+ *  Update the servos if valid data is ready and waiting. <p>
+ *  Note that radio mode ( \code radio_flag != 0\endcode ) takes priority over Jetson/ROS mode. Flags differ between
+ *  modes, but the resultant servo behavior should be identical.
  *
- *  Radio buffer layout:   [radio_flag, mainsail_angle, rudder_angle, jib_angle, jib_side_flag]
- *  ROS buffer layout:     [mainsail_angle, rudder_angle, jib_angle, jib_side_flag]
+ *  - Radio buffer layout:   [radio_flag, mainsail_angle, rudder_angle, jib_angle, jib_side_flag]
+ *  - ROS buffer layout:     [mainsail_angle, rudder_angle, jib_angle, jib_side_flag]
  */
 void ServoControlTask::execute() {
-
-    // RADIO MODE
-    if (sfr::serial::radio_flag != 0) {
+    if (sfr::serial::radio_flag != 0) { // RADIO MODE.
         if (!sfr::serial::update_servos_radio) return;
-
-        const uint8_t mainsail_angle = sfr::serial::radio_buffer[1];
-        const uint8_t rudder_angle   = sfr::serial::radio_buffer[2];
-        const uint8_t jib_angle      = sfr::serial::radio_buffer[3];
-        const uint8_t jib_side_flag  = sfr::serial::radio_buffer[4];
-
-        if (mainsail_angle >= constants::servo::MAINSAIL_MIN_ANGLE && mainsail_angle <= constants::servo::MAINSAIL_MAX_ANGLE)
-            sfr::servo::radio_sail_angle = mainsail_angle;
-        if (rudder_angle >= constants::servo::RUDDER_MIN_ANGLE && rudder_angle <= constants::servo::RUDDER_MAX_ANGLE)
-            sfr::servo::radio_rudder_angle = rudder_angle;
-
-        apply_commands(mainsail_angle, rudder_angle, jib_angle, jib_side_flag);
+        apply_commands(sfr::serial::radio_buffer[1], sfr::serial::radio_buffer[2],
+                        sfr::serial::radio_buffer[3], sfr::serial::radio_buffer[4]);
         sfr::serial::update_servos_radio = false;
-
-    // Jetson (ROS) mode
-    } else {
-        if (!sfr::serial::update_servos_ros) return;
-
-        const uint8_t mainsail_angle = sfr::serial::ros_buffer[0];
-        const uint8_t rudder_angle   = sfr::serial::ros_buffer[1];
-        const uint8_t jib_angle      = sfr::serial::ros_buffer[2];
-        const uint8_t jib_side_flag  = sfr::serial::ros_buffer[3];
-
-        if (mainsail_angle >= constants::servo::MAINSAIL_MIN_ANGLE && mainsail_angle <= constants::servo::MAINSAIL_MAX_ANGLE)
-            sfr::servo::ros_sail_angle = mainsail_angle;
-        if (rudder_angle >= constants::servo::RUDDER_MIN_ANGLE && rudder_angle <= constants::servo::RUDDER_MAX_ANGLE)
-            sfr::servo::ros_rudder_angle = rudder_angle;
-
-        apply_commands(mainsail_angle, rudder_angle, jib_angle, jib_side_flag);
+    } else if (sfr::serial::update_servos_ros) { // JETSON (ROS) MODE.
+        apply_commands(sfr::serial::ros_buffer[0], sfr::serial::ros_buffer[1],
+                        sfr::serial::ros_buffer[2], sfr::serial::ros_buffer[3]);
         sfr::serial::update_servos_ros = false;
     }
 }
 
-/**
- *   Applies commands to servos based on flags inputted. Flags differ if reciving an input from ROS
- *   or from radio, but resultant servo behavior should be identical.
- **/
+/** A utility method to actually apply values to servos, based on the received data and if checks on the data pass. */
 void ServoControlTask::apply_commands(const uint8_t mainsail_angle, const uint8_t rudder_angle,
                                       const uint8_t jib_angle, const uint8_t jib_side_flag) {
     if (mainsail_angle >= constants::servo::MAINSAIL_MIN_ANGLE && mainsail_angle <= constants::servo::MAINSAIL_MAX_ANGLE) {
@@ -83,13 +56,13 @@ void ServoControlTask::apply_commands(const uint8_t mainsail_angle, const uint8_
         sfr::servo::jib_side_flag = jib_side_flag;
 
         if (jib_side_flag == constants::servo::JIB_SIDE_PORT) {
-            sfr::servo::jib_stb_pwm = constants::servo::JIB_STB_MIN_PULSE;
-            actuate_servo(jib_stb_servo, constants::servo::JIB_STB_MIN_PULSE);
+            sfr::servo::jib_stb_pwm = constants::servo::JIB_STB_MAX_PULSE;
+            actuate_servo(jib_stb_servo, constants::servo::JIB_STB_MAX_PULSE);
             sfr::servo::jib_port_pwm = jib_to_pwm(jib_angle, jib_side_flag);
             actuate_servo(jib_port_servo, sfr::servo::jib_port_pwm);
         } else {
-            sfr::servo::jib_port_pwm = constants::servo::JIB_PORT_MIN_PULSE;
-            actuate_servo(jib_port_servo, constants::servo::JIB_PORT_MIN_PULSE);
+            sfr::servo::jib_port_pwm = constants::servo::JIB_PORT_MAX_PULSE;
+            actuate_servo(jib_port_servo, constants::servo::JIB_PORT_MAX_PULSE);
             sfr::servo::jib_stb_pwm = jib_to_pwm(jib_angle, jib_side_flag);
             actuate_servo(jib_stb_servo, sfr::servo::jib_stb_pwm);
         }
@@ -112,15 +85,13 @@ uint32_t ServoControlTask::rudder_to_pwm(const uint8_t angle) {
  * @return the PWM to actuate \code mainsail_servo\endcode to.
  */
 uint32_t ServoControlTask::mainsail_to_pwm(const uint8_t angle) {
-    const uint32_t delta = law_of_cos_map(angle,
+    const uint32_t new_pwm = law_of_cos_map(angle,
         constants::servo::TWO_BOOM_LEN_SQD_CM,
         constants::servo::MAINSAIL_PULSE_PER_TURN,
         constants::servo::MAINSAIL_WHEEL_CIRCUM_CM,
         true
-    );
-    constexpr uint32_t range = constants::servo::MAINSAIL_MAX_PULSE - constants::servo::MAINSAIL_MIN_PULSE;
-    if (delta >= range) return constants::servo::MAINSAIL_MAX_PULSE;
-    return constants::servo::MAINSAIL_MIN_PULSE + delta;
+    ) + constants::servo::MAINSAIL_MIN_PULSE;
+    return new_pwm <= constants::servo::MAINSAIL_MAX_PULSE ? new_pwm : constants::servo::MAINSAIL_MAX_PULSE;
 }
 
 /** Maps a goal sail angle for the jib to a PWM value (for one of the two jib servos).
@@ -136,15 +107,13 @@ uint32_t ServoControlTask::jib_to_pwm(const uint8_t angle, const uint8_t jib_sid
                                 constants::servo::JIB_PORT_MIN_PULSE : constants::servo::JIB_STB_MIN_PULSE;
     const uint32_t max_pulse = (jib_side_flag == constants::servo::JIB_SIDE_PORT) ?
                                 constants::servo::JIB_PORT_MAX_PULSE : constants::servo::JIB_STB_MAX_PULSE;
-    const uint32_t delta = law_of_cos_map(angle,
+    const uint32_t new_pwm = law_of_cos_map(angle,
         constants::servo::TWO_JIB_FOOT_LEN_SQD_CM,
         constants::servo::JIB_PULSE_PER_TURN,
         wheel_circum,
         false
-    );
-    const uint32_t range = max_pulse - min_pulse;
-    if (delta >= range) return max_pulse;
-    return min_pulse + delta;
+    ) + min_pulse;
+    return new_pwm <= max_pulse ? new_pwm : max_pulse;
 }
 
 /** Send \code pwm\endcode to \code servo\endcode, thereby changing \code servo\endcode 's angle. */
@@ -169,11 +138,12 @@ void ServoControlTask::actuate_servo(Servo &servo, const uint32_t pwm) {
  *      apply the Pythagorean theorem on the right triangle formed by "c", the initial distance from the deck to the
  *      boom, and the actual mainsheet, to obtain the actual goal length of the mainsheet.
  * - We model similarly for the jib (note that this is less accurate as there is no "boom" for the jib, and it is
- *   therefore harder to quantify a triangle or angles in general).
+ *   therefore harder to quantify a triangle or angles in general. This is why we multiply by a slight fractional offset
+ *   when trimming the jib).
  *    - "angle" refers to the goal angle we want the jib to be set to.
  *    - We approximate "b" as the length of the foot of the jib.
  *    - Here, "c" on its own is approximately the length of the sheet, which is what we want to solve for.
- * - In either case, our final \code sheet_len\endcode value maps to a PWM value based on the specific servo and
+ * - In either case, this final \code sheet_len\endcode value will map to a PWM value based on the specific servo and
  *   the circumference of its wheel.
  *
  * Note that this method assumes that the max PWM value of the servo in question allows for the sail to be set to
@@ -183,7 +153,7 @@ uint32_t ServoControlTask::law_of_cos_map(const uint8_t angle, const uint32_t tw
                                           const float wheel_circum, const bool mainsail) {
     const float c_squared = static_cast<float>(two_b_sqd) * (1 - cosf(static_cast<float>(angle) * 0.017453f)); // 0.017453 = pi/180.
     const float sheet_len = mainsail ? sqrtf(c_squared + constants::servo::MAINSAIL_INITIAL_SQD_CM) - constants::servo::MAINSAIL_INITIAL_CM
-                                     : sqrtf(c_squared);
+                                     : sqrtf(c_squared) * 0.85f;
 
-    return static_cast<uint32_t>(PWM_per_turn * (sheet_len / wheel_circum));  // PWM: (PWM_per_turn * turns_needed).
+    return static_cast<uint32_t>(PWM_per_turn * (sheet_len / wheel_circum)); // PWM: (PWM_per_turn * turns_needed).
 }

teensy/src/Control Tasks/ServoControlTask.hpp

@@ -18,6 +18,6 @@ class ServoControlTask {
     static uint32_t jib_to_pwm(uint8_t angle, uint8_t jib_side_flag);
     static void actuate_servo(Servo &servo, uint32_t pwm);
 
-    static uint32_t law_of_cos_map(uint8_t angle, uint32_t two_b_sqd, float PWM_per_turn, float wheel_circum, bool mainsail);
     void apply_commands(uint8_t mainsail_angle, uint8_t rudder_angle, uint8_t jib_angle, uint8_t jib_side_flag);
-};
+    static uint32_t law_of_cos_map(uint8_t angle, uint32_t two_b_sqd, float PWM_per_turn, float wheel_circum, bool mainsail);
+};

teensy/src/MainControlLoop.cpp

@@ -1,13 +1,7 @@
 #include "MainControlLoop.hpp"
 
-MainControlLoop::MainControlLoop()
-    : anemometer_monitor(),
-      radio_serial_monitor(),
-      ros_serial_monitor(),
-      servo_control_task(),
-      serial_control_task()
-{
-
+MainControlLoop::MainControlLoop() : anemometer_monitor(), radio_serial_monitor(), ros_serial_monitor(),
+                                        servo_control_task(), serial_control_task() {
     delay(1000);
 }
 
@@ -17,4 +11,4 @@ void MainControlLoop::execute() {
     ros_serial_monitor.execute();
     servo_control_task.execute();
     serial_control_task.execute();
-}
+}

teensy/src/MainControlLoop.hpp

@@ -17,4 +17,4 @@ class MainControlLoop {
     ROSSerialMonitor ros_serial_monitor;
     ServoControlTask servo_control_task;
     SerialControlTask serial_control_task;
-};
+};

teensy/src/Monitors/AnemometerMonitor.cpp

@@ -8,4 +8,4 @@ void AnemometerMonitor::execute() {
     // Note that 0.35191 = 360/1023 -- maps analogRead() range of 0-1023 to 0-360 degrees.
     sfr::anemometer::wind_angle = static_cast<int>(0.35191 * analogRead(constants::anemometer::ANEMOMETER_PIN));
     //Serial.println(sfr::anemometer::wind_angle); // Print for testing.
-}
+}

teensy/src/Monitors/ROSSerialMonitor.cpp

@@ -30,8 +30,8 @@ void ROSSerialMonitor::execute()
 
             // Decode ROS packet: [sail, rudder]
             sfr::serial::update_servos_ros = true;
-            sfr::servo::ros_sail_angle   = sfr::serial::ros_buffer[0];
-            sfr::servo::ros_rudder_angle = sfr::serial::ros_buffer[1];
+            // sfr::servo::ros_sail_angle   = sfr::serial::ros_buffer[0]; //todo delete
+            // sfr::servo::ros_rudder_angle = sfr::serial::ros_buffer[1]; //todo delete
         }
         else if (packet_started)
         {

teensy/src/Monitors/RadioSerialMonitor.cpp

@@ -44,8 +44,8 @@ void RadioSerialMonitor::execute()
 
             // Decode RADIO packet: [radio_flag, mainsail_angle, rudder_angle, jib_angle, jib_side_flag]
             sfr::serial::radio_flag          = sfr::serial::radio_buffer[0];
-            sfr::servo::radio_sail_angle     = sfr::serial::radio_buffer[1];
-            sfr::servo::radio_rudder_angle   = sfr::serial::radio_buffer[2];
+            // sfr::servo::radio_sail_angle     = sfr::serial::radio_buffer[1]; //todo delete
+            // sfr::servo::radio_rudder_angle   = sfr::serial::radio_buffer[2]; //todo delete
             sfr::serial::update_servos_radio = true;
         }
         else if (packet_started)

teensy/src/Monitors/RadioSerialMonitor.hpp

@@ -1,11 +1,8 @@
 #pragma once
-
 #include <Arduino.h>
 #include "sfr.hpp"
-#include "constants.hpp"
 
-class RadioSerialMonitor
-{
+class RadioSerialMonitor {
 public:
     RadioSerialMonitor();
     void execute();