That-One-Nerd/ary-over-under
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

comments and shii

Browse Source
This commit is contained in:
ary 2023-12-07 22:47:45 -05:00
parent 0da3ed162d
commit 99e01877c2
2 changed files with 12 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
CLOUDS/src/ary-lib/drive/set_pid.cpp
View File

@ -114,23 +114,23 @@ void Drive::set_swing(e_swing type, double target, int speed) {
} }
void set_profiled_drive(Drive& chassis, double target, int endTimeout) { void set_profiled_drive(Drive& chassis, double target, int endTimeout) {
//kv: rpm -> voltage
//sf: in/ms -> rpm
int sign = ary::util::signum(target); int sign = ary::util::signum(target);
target = fabs(target); target = fabs(target);
std::vector<double> profile; std::vector<double> profile; // The vector that stores all the velocity points
// std::cout << "reached 1" << std::endl;
/*
Handle generation of motion profile, taking into account direction
*/
if(sign > 0) profile = ary::util::trapezoidalMotionProfile(target, LINEAR_MAX_VEL, LINEAR_FWD_ACCEL, LINEAR_FWD_DECEL); if(sign > 0) profile = ary::util::trapezoidalMotionProfile(target, LINEAR_MAX_VEL, LINEAR_FWD_ACCEL, LINEAR_FWD_DECEL);
else profile = ary::util::trapezoidalMotionProfile(target, LINEAR_MAX_VEL, LINEAR_REV_ACCEL, LINEAR_REV_DECEL); else profile = ary::util::trapezoidalMotionProfile(target, LINEAR_MAX_VEL, LINEAR_REV_ACCEL, LINEAR_REV_DECEL);
for (int i = 0; i < profile.size(); i++) for (int i = 0; i < profile.size(); i++)
{ {
chassis.set_tank(profile[i] * VELOCITY_TO_VOLTS_LIN * sign, profile[i] * VELOCITY_TO_VOLTS_LIN * sign); chassis.set_tank(profile[i] * VELOCITY_TO_VOLTS_LIN * sign, profile[i] * VELOCITY_TO_VOLTS_LIN * sign); // Convert to volts, respect direction
pros::delay(10); pros::delay(10); // Delay to prevent any resource errors
} }
chassis.set_tank(0, 0); // Stop the drive chassis.set_tank(0, 0); // Stop the drive
chassis.set_drive_brake(MOTOR_BRAKE_BRAKE); chassis.set_drive_brake(MOTOR_BRAKE_BRAKE);
pros::delay(endTimeout); pros::delay(endTimeout); // The amount of the time to wait after completing the movement
} }

8
CLOUDS/src/ary-lib/util.cpp
View File

@ -148,16 +148,16 @@ double clip_num(double input, double max, double min) {
} }
std::vector<double> trapezoidalMotionProfile(double dist, double maxVel, double accel, double decel) { std::vector<double> trapezoidalMotionProfile(double dist, double maxVel, double accel, double decel) {
double max = std::min(std::sqrt((2 * accel * decel * dist) / accel + decel), maxVel); double max = std::min(std::sqrt((2 * accel * decel * dist) / accel + decel), maxVel); // Utilize kinematic equations to determine a max velocity
double accelTime = max / accel; double accelTime = max / accel; // Calculate time required to accelerate and decelerate
double decelTime = max / decel; double decelTime = max / decel;
double coastDist = (dist / max) - (max / (2 * accel)) - (max / (2 * decel)); double coastDist = (dist / max) - (max / (2 * accel)) - (max / (2 * decel)); // Coast time is the moment from where motors go unpowered, and the remaining distance that the robot travels
double coastTime = coastDist / max; double coastTime = coastDist / max;
double totalTime = accelTime + decelTime + coastTime; double totalTime = accelTime + decelTime + coastTime;
double vel = 0; double vel = 0;
double diff;
std::vector<double> profile; std::vector<double> profile;
/* Add the points to the vector*/
for (int i = 0; i < std::ceil(totalTime); i++) for (int i = 0; i < std::ceil(totalTime); i++)
{ {
if (i < std::floor(accelTime)) if (i < std::floor(accelTime))