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) {
//kv: rpm -> voltage
//sf: in/ms -> rpm
int sign = ary::util::signum(target);
target = fabs(target);
std::vector<double> profile;
// std::cout << "reached 1" << std::endl;
std::vector<double> profile; // The vector that stores all the velocity points
/*
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);
else profile = ary::util::trapezoidalMotionProfile(target, LINEAR_MAX_VEL, LINEAR_REV_ACCEL, LINEAR_REV_DECEL);
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);
pros::delay(10);
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); // Delay to prevent any resource errors
}
chassis.set_tank(0, 0); // Stop the drive
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) {
double max = std::min(std::sqrt((2 * accel * decel * dist) / accel + decel), maxVel);
double accelTime = max / accel;
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; // Calculate time required to accelerate and decelerate
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 totalTime = accelTime + decelTime + coastTime;
double vel = 0;
double diff;
std::vector<double> profile;
/* Add the points to the vector*/
for (int i = 0; i < std::ceil(totalTime); i++)
{
if (i < std::floor(accelTime))