Feature vtm comms

.gitignore

@@ -7,6 +7,7 @@
 # Ignore the build directory
 build
 build/
+.pio/
 
 # Ignore the output binaries
 *.elf

src/main.cpp

@@ -209,6 +209,7 @@ int main() {
             // hid_incoming.get_target_state_map(target_state_map);
             target_state_map.from_comms_packet(comms_layer.get_hive_data().target_state_data.state);
             last_feed = target_state_map[Cfg::StateName::Feeder].get_position();            
+
         }
 
         // override temp state if needed. Dont override in teensy mode so the sentry doesnt move during inspection

src/sensors/transmitter/dr16.cpp

@@ -322,30 +322,27 @@ DR16Data DR16::get_dr16_data(){
 
 void DR16::manual_controls(const RobotStateMap& estimated_state_map, RobotStateMap& target_state_map, bool not_safety_mode, float& feed, float& last_feed) {
 	float delta = control_input_timer.delta();
-	transmitter_pos_x += mouse_x * 0.05 * delta;
-	transmitter_pos_y += mouse_y * 0.05 * delta;
-
-	vtm_pos_x += ref.ref_data.kbm_interaction.mouse_speed_x * 0.05 * delta;
-	vtm_pos_y += ref.ref_data.kbm_interaction.mouse_speed_y * 0.05 * delta;
-
+	// Used for determing whether we are using vtm or usb to dr16
+	auto merge_input = [](auto primary_val, auto secondary_val) {
+		// If primary isn't zero, use it. Else use secondary.
+		return (primary_val != 0) ? primary_val : secondary_val;
+	};
+
+	pos_x = merge_input(mouse_x,ref.ref_data.kbm_interaction.mouse_speed_x) * 0.05 * delta;
+	pos_y = merge_input(mouse_y,ref.ref_data.kbm_interaction.mouse_speed_y) * 0.05 * delta;
+	l_mouse_button = merge_input(l_mouse_button,ref.ref_data.kbm_interaction.button_left);
 	float pitch_min = estimated_state_map[Cfg::StateName::GimbalPitch].config().reference_limits.position.min;
     float pitch_max = estimated_state_map[Cfg::StateName::GimbalPitch].config().reference_limits.position.max;
     float pitch_average = 0.5 * (pitch_min + pitch_max);
     pitch_min -= pitch_average;
     pitch_max -= pitch_average;
 
 	// clamp to pitch limits
-	if (transmitter_pos_y < pitch_min) {
-		transmitter_pos_y = pitch_min;
-	}
-	if (transmitter_pos_y > pitch_max) {
-		transmitter_pos_y = pitch_max;
-	}
-	if (vtm_pos_y < pitch_min) {
-		vtm_pos_y = pitch_min;
+	if (pos_y < pitch_min) {
+		pos_y = pitch_min;
 	}
-	if (vtm_pos_y > pitch_max) {
-		vtm_pos_y = pitch_max;
+	if (pos_y > pitch_max) {
+		pos_y = pitch_max;
 	}
 
 	float chassis_vel_x = 0;
@@ -355,30 +352,28 @@ void DR16::manual_controls(const RobotStateMap& estimated_state_map, RobotStateM
 
 	if (estimated_state_map[Cfg::StateName::ChassisX].config().governor_type == Cfg::StateOrder::Velocity) { // if we should be controlling velocity
 
-		chassis_vel_x = get_l_stick_y() * 5.4 +
-						(-ref.ref_data.kbm_interaction.key_w + ref.ref_data.kbm_interaction.key_s) * 2.5;
+		chassis_vel_x = get_l_stick_y() * 5.4;
+
+		chassis_vel_x += merge_input((-ref.ref_data.kbm_interaction.key_w + ref.ref_data.kbm_interaction.key_s), (-keys.w + keys.s)) * 2.5;
 
-		chassis_vel_x += (-keys.w + keys.s) * 2.5;
-
-		chassis_vel_y = -(get_l_stick_x() * 5.4) +
-						(ref.ref_data.kbm_interaction.key_d - ref.ref_data.kbm_interaction.key_a) * 2.5;
-
-		chassis_vel_y += (keys.d - keys.a) * 2.5;
+		chassis_vel_y = -(get_l_stick_x() * 5.4);
+
+		chassis_vel_y += merge_input((ref.ref_data.kbm_interaction.key_d - ref.ref_data.kbm_interaction.key_a),(keys.d - keys.a)) * 2.5;
 
 	} else if (estimated_state_map[Cfg::StateName::ChassisX].config().governor_type == Cfg::StateOrder::Position) { // if we should be controlling position
 		chassis_pos_x = get_l_stick_x() * 2 + pos_offset_x;
 		chassis_pos_y = get_l_stick_y() * 2 + pos_offset_y;
 	}
 
 	float chassis_spin = get_wheel() * 25;
-	float pitch_target = 1.57 + -get_r_stick_y() * 0.3 + vtm_pos_y;
-	float yaw_target = -get_r_stick_x() * 1.5 - vtm_pos_x;
+	float pitch_target = 1.57 + -get_r_stick_y() * 0.3 + pos_y;
+	float yaw_target = -get_r_stick_x() * 1.5 - pos_x; // This was setup with vtm_pos before which never functioned so, if bug its here
 
 	float fly_wheel_target =
 		(get_r_switch() == SwitchPos::FORWARD || get_r_switch() == SwitchPos::MIDDLE) ? 18 : 0; // m/s
 	// if the right switch is forward, and either the left mouse button is pressed or the right switch is not
 	// backward, set the feeder to something. Otherwise, set it to 0
-	float feeder_target = (((ref.ref_data.kbm_interaction.button_left) &&
+	float feeder_target = ((l_mouse_button &&
 							get_r_switch() != SwitchPos::BACKWARD) || get_r_switch() == SwitchPos::FORWARD) ? 10 : 0;
 	if (estimated_state_map[Cfg::StateName::Feeder].config().governor_type == Cfg::StateOrder::Position) {
 		float dt2 = timer.delta();
@@ -413,4 +408,4 @@ void DR16::manual_controls(const RobotStateMap& estimated_state_map, RobotStateM
 		pos_offset_y = estimated_state_map[Cfg::StateName::ChassisY].get_position();
 		feed = last_feed;
 	}
-}
+}

src/sensors/transmitter/dr16.hpp

@@ -217,14 +217,10 @@ class DR16 : public Transmitter {
 	Keys keys;
 
 	// manual controls
-	/// @brief Mouse x axis position
-	float transmitter_pos_x = 0;
+	/// @brief Mouse x axis position 
+	float pos_x = 0;
 	/// @brief Mouse y axis position
-	float transmitter_pos_y = 0;
-	/// @brief Mouse x axis position from ref
-	float vtm_pos_x = 0;
-	/// @brief Mouse y axis position from ref
-	float vtm_pos_y = 0;
+	float pos_y = 0;
 	/// @brief Position offset for chassis x (so the sentry doesn't drive to 0,0)
 	float pos_offset_x = 0;
 	/// @brief Position offset for chassis y (so the sentry doesn't drive to 0,0)