Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Added arcade vector scaling #185 #188

Merged
merged 1 commit into from
Nov 27, 2024
Merged

Added arcade vector scaling #185 #188

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 4 additions & 0 deletions include/EZ-Template/drive/drive.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3074,7 +3074,11 @@ class Drive {
double odom_ime_track_width_left = 0.0;
double odom_ime_track_width_right = 0.0;

void opcontrol_arcade_scaling(bool enable);
bool opcontrol_arcade_scaling_enabled();

private:
bool arcade_vector_scaling = false;
// odom privates
std::vector<odom> pp_movements;
std::vector<int> injected_pp_index;
Expand Down
39 changes: 31 additions & 8 deletions src/EZ-Template/drive/user_input.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,9 @@ file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "main.h"

void Drive::opcontrol_arcade_scaling(bool enable) { arcade_vector_scaling = enable; }
bool Drive::opcontrol_arcade_scaling_enabled() { return arcade_vector_scaling; }

// Set curve defaults
void Drive::opcontrol_curve_default_set(double left, double right) {
left_curve_scale = left;
Expand Down Expand Up @@ -230,20 +233,40 @@ void Drive::opcontrol_drive_reverse_set(bool toggle) { is_reversed = toggle; }
bool Drive::opcontrol_drive_reverse_get() { return is_reversed; }

void Drive::opcontrol_joystick_threshold_iterate(int l_stick, int r_stick) {
double l_out = 0.0, r_out = 0.0;

// Check the motors are being set to power
if (abs(l_stick) > 0 || abs(r_stick) > 0) {
if (practice_mode_is_on && (abs(l_stick) > 120 || abs(r_stick) > 120))
drive_set(0, 0);
else if (is_reversed == true)
drive_set(-r_stick, -l_stick);
else
drive_set(l_stick, r_stick);
if (active_brake_kp != 0) drive_sensor_reset();
if (active_brake_kp != 0) drive_sensor_reset(); // Reset sensor

if (practice_mode_is_on && (abs(l_stick) > 120 || abs(r_stick) > 120)) {
l_out = 0.0;
r_out = 0.0;
} else if (!is_reversed) {
l_out = l_stick;
r_out = r_stick;
} else {
l_out = -r_stick;
r_out = -l_stick;
}

}
// When joys are released, run active brake (P) on drive
else {
drive_set((0 - drive_sensor_left()) * active_brake_kp, (0 - drive_sensor_right()) * active_brake_kp);
l_out = (0 - drive_sensor_left()) * active_brake_kp;
r_out = (0 - drive_sensor_right()) * active_brake_kp;
}

// Constrain output between 127 and -127
if (arcade_vector_scaling) {
double faster_side = fmax(fabs(l_out), fabs(r_out));
if (faster_side > 127.0) {
l_out *= (127.0 / faster_side);
r_out *= (127.0 / faster_side);
}
}

drive_set(l_out, r_out);
}

// Clip joysticks based on joystick threshold
Expand Down