update superstructure
- implement new intake control (tasks) - update constants - add constant hold voltage to intake
This commit is contained in:
ary
parent
35cd4e24c4
commit
f10d0baccc
@ -3,6 +3,7 @@
|
||||
#define DRIVE_SPEED 127
|
||||
#define TURN_SPEED 120
|
||||
#define SWING_SPEED 120
|
||||
|
||||
// R1 -> WINGS, L1 -> CATA, L2 -> PTO, R2 -> INTAKE
|
||||
// Renu's control preferences
|
||||
#define RENU_PTO_TOGGLE DIGITAL_R1
|
||||
@ -30,6 +31,7 @@ namespace superstruct {
|
||||
void motorsCoast();
|
||||
void motorsHold();
|
||||
void motorsBrake();
|
||||
void controllerTelemtry();
|
||||
|
||||
void disableActiveBrake();
|
||||
|
||||
@ -51,7 +53,7 @@ namespace superstruct {
|
||||
/* Structure */
|
||||
void togglePto(bool toggle);
|
||||
void runCata(double inpt);
|
||||
void runIntake(double inpt);
|
||||
void setIntakeSpeed(double inpt);
|
||||
void subsysControl(pros::controller_digital_e_t ptoToggleButton, pros::controller_digital_e_t cataRunButton, pros::controller_digital_e_t intakeButton, pros::controller_digital_e_t outtakeButton);
|
||||
void wingsControl(pros::controller_digital_e_t wingControlButton);
|
||||
void actuateClimb();
|
||||
|
||||
@ -16,8 +16,12 @@ double speedScale = 1.0;
|
||||
double turnScale = 1.0;
|
||||
double swingScale = 1.0;
|
||||
|
||||
namespace superstruct {
|
||||
void chassisInit() {
|
||||
double intakeSpeed = 0;
|
||||
|
||||
namespace superstruct
|
||||
{
|
||||
void chassisInit()
|
||||
{
|
||||
/*
|
||||
When the robot first starts up we want to do a couple things:
|
||||
- Adjust the drivetrain curve bottons so it does not interfere with any of the driver controls.
|
||||
@ -37,21 +41,24 @@ namespace superstruct {
|
||||
chassis.set_joystick_turnscale(1.0);
|
||||
}
|
||||
|
||||
void opControlInit() {
|
||||
void opControlInit()
|
||||
{
|
||||
motorsCoast();
|
||||
disableActiveBrake();
|
||||
disableActiveBrake();
|
||||
}
|
||||
|
||||
// Adjust exit conditions to allow for quick movements
|
||||
void configureExitConditions() {
|
||||
chassis.set_exit_condition(chassis.turn_exit, 50, 2, 220, 3, 150, 500);
|
||||
chassis.set_exit_condition(chassis.swing_exit, 100, 3, 500, 7, 150, 500);
|
||||
chassis.set_exit_condition(chassis.drive_exit, 40, 80, 300, 150, 150, 500);
|
||||
void configureExitConditions()
|
||||
{
|
||||
chassis.set_exit_condition(chassis.turn_exit, 50, 2, 220, 3, 500, 500);
|
||||
chassis.set_exit_condition(chassis.swing_exit, 100, 3, 500, 7, 500, 500);
|
||||
chassis.set_exit_condition(chassis.drive_exit, 40, 80, 300, 150, 500, 500);
|
||||
}
|
||||
|
||||
// Adjust PID constants for accurate movements
|
||||
void configureConstants() {
|
||||
chassis.set_slew_min_power(100, 100);
|
||||
void configureConstants()
|
||||
{
|
||||
chassis.set_slew_min_power(80, 80);
|
||||
chassis.set_slew_distance(7, 7);
|
||||
chassis.set_pid_constants(&chassis.headingPID, 16, 0, 32, 0);
|
||||
chassis.set_pid_constants(&chassis.forward_drivePID, 0.5, 0, 5, 0);
|
||||
@ -61,69 +68,103 @@ namespace superstruct {
|
||||
}
|
||||
|
||||
// Prepare the bot for the autonomous period of a match
|
||||
void autonomousResets() {
|
||||
void autonomousResets()
|
||||
{
|
||||
chassis.reset_pid_targets();
|
||||
chassis.reset_gyro();
|
||||
chassis.reset_drive_sensor();
|
||||
chassis.reset_gyro();
|
||||
chassis.reset_drive_sensor();
|
||||
configureConstants();
|
||||
configureExitConditions();
|
||||
motorsBrake();
|
||||
|
||||
}
|
||||
|
||||
void motorsCoast() {
|
||||
void motorsCoast()
|
||||
{
|
||||
chassis.set_drive_brake(MOTOR_BRAKE_COAST);
|
||||
}
|
||||
|
||||
void motorsHold() {
|
||||
void motorsHold()
|
||||
{
|
||||
chassis.set_drive_brake(MOTOR_BRAKE_HOLD);
|
||||
}
|
||||
|
||||
void motorsBrake() {
|
||||
void motorsBrake()
|
||||
{
|
||||
chassis.set_drive_brake(MOTOR_BRAKE_BRAKE);
|
||||
}
|
||||
|
||||
// The chassis will not apply a constant voltage to prevent it from being moved
|
||||
void disableActiveBrake() {
|
||||
void disableActiveBrake()
|
||||
{
|
||||
chassis.set_active_brake(0.0);
|
||||
}
|
||||
|
||||
void handleIntake() {
|
||||
while (true) {
|
||||
intake_mtr.move_voltage(intakeSpeed);
|
||||
}
|
||||
}
|
||||
|
||||
pros::Task intakeManager(handleIntake);
|
||||
|
||||
// void controllerTelemetry()
|
||||
// {
|
||||
// while (1)
|
||||
// {
|
||||
// for (int i = 0; i < 4; i++)
|
||||
// {
|
||||
// double localavg_one = 0;
|
||||
// }
|
||||
|
||||
// for (int i = 0; i < 4; i++)
|
||||
// {
|
||||
// double
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
|
||||
// Drives forward, runs next commands WITHOUT waiting for the drive to complete
|
||||
void driveAsync(double dist, bool useHeadingCorrection) {
|
||||
void driveAsync(double dist, bool useHeadingCorrection)
|
||||
{
|
||||
chassis.set_drive(dist, DRIVE_SPEED * speedScale, (dist > 14.0) ? true : false, useHeadingCorrection);
|
||||
}
|
||||
|
||||
// Drives forward, runs next commands AFTER waiting for the drive to complete
|
||||
void driveSync(double dist, bool useHeadingCorrection) {
|
||||
void driveSync(double dist, bool useHeadingCorrection)
|
||||
{
|
||||
chassis.set_drive(dist, DRIVE_SPEED * speedScale, (dist > 14.0) ? true : false, useHeadingCorrection);
|
||||
chassis.wait_drive();
|
||||
}
|
||||
|
||||
// Drives forward, runs next commands AFTER reaching a certain measurement/error along the path
|
||||
void driveWithMD(double dist, bool useHeadingCorrection, double waitUntilDist) {
|
||||
void driveWithMD(double dist, bool useHeadingCorrection, double waitUntilDist)
|
||||
{
|
||||
chassis.set_drive(dist, DRIVE_SPEED * speedScale, (dist > 14.0) ? true : false, useHeadingCorrection);
|
||||
chassis.wait_until(waitUntilDist);
|
||||
}
|
||||
|
||||
// Turns the chassis, runs other commands after it has run.
|
||||
void turnSync(double theta) {
|
||||
void turnSync(double theta)
|
||||
{
|
||||
chassis.set_turn(theta, TURN_SPEED * turnScale);
|
||||
chassis.wait_drive();
|
||||
}
|
||||
|
||||
// Turns the chassis, runs other commands immediately after call
|
||||
void turnAsync(double theta) {
|
||||
void turnAsync(double theta)
|
||||
{
|
||||
chassis.set_turn(theta, TURN_SPEED * turnScale);
|
||||
}
|
||||
|
||||
// Moves only the right side of the chassis so it can make a left turn
|
||||
void leftSwing(double theta) {
|
||||
void leftSwing(double theta)
|
||||
{
|
||||
chassis.set_swing(LEFT_SWING, theta, SWING_SPEED * swingScale);
|
||||
}
|
||||
|
||||
// Moves only the left side of the chassis so it can make a right turn
|
||||
void rightSwing(double theta) {
|
||||
void rightSwing(double theta)
|
||||
{
|
||||
chassis.set_swing(RIGHT_SWING, theta, SWING_SPEED * swingScale);
|
||||
}
|
||||
|
||||
@ -131,66 +172,89 @@ namespace superstruct {
|
||||
Each of the scale values must be clamed between 0.1 - 1 (10% to 100%) to avoid saturation of motors.
|
||||
*/
|
||||
|
||||
void setDriveScale(double val) {
|
||||
void setDriveScale(double val)
|
||||
{
|
||||
speedScale = std::clamp(val, 0.1, 1.0);
|
||||
}
|
||||
|
||||
void setTurnScale(double val) {
|
||||
void setTurnScale(double val)
|
||||
{
|
||||
turnScale = std::clamp(val, 0.1, 1.0);
|
||||
}
|
||||
|
||||
void setSwingScale(double val) {
|
||||
void setSwingScale(double val)
|
||||
{
|
||||
swingScale = std::clamp(val, 0.1, 1.0); //
|
||||
}
|
||||
|
||||
// Structure methods
|
||||
void togglePto(bool toggle) {
|
||||
void togglePto(bool toggle)
|
||||
{
|
||||
ptoEnabled = toggle;
|
||||
chassis.pto_toggle({intake_mtr, cata_mtr}, toggle); // Configure the listed PTO motors to whatever value toggle is.
|
||||
pto_piston.set_value(toggle);
|
||||
|
||||
if (toggle) {
|
||||
if (toggle)
|
||||
{
|
||||
intake_mtr.set_brake_mode(MOTOR_BRAKE_COAST);
|
||||
cata_mtr.set_brake_mode(MOTOR_BRAKE_COAST);
|
||||
}
|
||||
}
|
||||
|
||||
void runCata(double inpt) {
|
||||
if (!ptoEnabled) return;
|
||||
void runCata(double inpt)
|
||||
{
|
||||
if (!ptoEnabled)
|
||||
return;
|
||||
cata_mtr.move_voltage(inpt);
|
||||
}
|
||||
|
||||
void runIntake(double inpt) {
|
||||
if (!ptoEnabled) return;
|
||||
intake_mtr.move_voltage(inpt);
|
||||
void setIntakeSpeed(double inpt)
|
||||
{
|
||||
if (!ptoEnabled)
|
||||
intakeSpeed = 0;
|
||||
intakeSpeed = inpt;
|
||||
}
|
||||
|
||||
int lock = 0;
|
||||
void subsysControl(pros::controller_digital_e_t ptoToggleButton, pros::controller_digital_e_t cataRunButton, pros::controller_digital_e_t intakeButton, pros::controller_digital_e_t outtakeButton) {
|
||||
if (globals::master.get_digital(ptoToggleButton) && lock == 0) { // If the PTO button has been pressed and the PTO is not engaged
|
||||
void subsysControl(pros::controller_digital_e_t ptoToggleButton, pros::controller_digital_e_t cataRunButton, pros::controller_digital_e_t intakeButton, pros::controller_digital_e_t outtakeButton)
|
||||
{
|
||||
if (globals::master.get_digital(ptoToggleButton) && lock == 0)
|
||||
{ // If the PTO button has been pressed and the PTO is not engaged
|
||||
togglePto(!ptoEnabled); // Toggle the PTO so that cataput is useable
|
||||
lock = 1;
|
||||
} else if(!globals::master.get_digital(ptoToggleButton)) {
|
||||
}
|
||||
else if (!globals::master.get_digital(ptoToggleButton))
|
||||
{
|
||||
lock = 0;
|
||||
}
|
||||
|
||||
if (globals::master.get_digital(cataRunButton)) {
|
||||
runCata(-12000);
|
||||
} else {
|
||||
runCata(0);
|
||||
}
|
||||
if (globals::master.get_digital(cataRunButton))
|
||||
{
|
||||
runCata(-12000);
|
||||
}
|
||||
else
|
||||
{
|
||||
runCata(0);
|
||||
}
|
||||
|
||||
if (globals::master.get_digital(intakeButton)) {
|
||||
runIntake(-12000);
|
||||
} else if (globals::master.get_digital(outtakeButton)) {
|
||||
runIntake(12000);
|
||||
} else {
|
||||
runIntake(0);
|
||||
if (globals::master.get_digital(intakeButton))
|
||||
{
|
||||
setIntakeSpeed(-12000);
|
||||
}
|
||||
else if (globals::master.get_digital(outtakeButton))
|
||||
{
|
||||
setIntakeSpeed(12000);
|
||||
}
|
||||
else
|
||||
{
|
||||
setIntakeSpeed(-1000);
|
||||
}
|
||||
}
|
||||
|
||||
void wingsControl(pros::controller_digital_e_t wingControlButton) {
|
||||
if (globals::master.get_digital_new_press(wingControlButton)) {
|
||||
void wingsControl(pros::controller_digital_e_t wingControlButton)
|
||||
{
|
||||
if (globals::master.get_digital_new_press(wingControlButton))
|
||||
{
|
||||
if (wings.getState() == 0) // A value of 0 indicates that both wings are closed
|
||||
wings.open();
|
||||
else if (wings.getState() == 3) // A value of 3 indicates that both wings are open
|
||||
@ -198,13 +262,16 @@ namespace superstruct {
|
||||
}
|
||||
}
|
||||
|
||||
void actuateClimb() {
|
||||
void actuateClimb()
|
||||
{
|
||||
climb_piston_one.set_value(1);
|
||||
climb_piston_two.set_value(1);
|
||||
}
|
||||
|
||||
void climbControl(pros::controller_digital_e_t but1, pros::controller_digital_e_t but2) {
|
||||
if (globals::master.get_digital(but1) && globals::master.get_digital(but2)) {
|
||||
void climbControl(pros::controller_digital_e_t but1, pros::controller_digital_e_t but2)
|
||||
{
|
||||
if (globals::master.get_digital(but1) && globals::master.get_digital(but2))
|
||||
{
|
||||
actuateClimb();
|
||||
}
|
||||
}
|
||||
@ -212,19 +279,22 @@ namespace superstruct {
|
||||
/*
|
||||
Controls -> For whoever is controlling the robot
|
||||
*/
|
||||
void renu_control() {
|
||||
void renu_control()
|
||||
{
|
||||
subsysControl(RENU_PTO_TOGGLE, RENU_CATA_CONTROL, RENU_INTAKE_CONTROL_INTAKE, RENU_INTAKE_CONTROL_OUTTAKE);
|
||||
wingsControl(RENU_WING_CONTROL);
|
||||
climbControl(RENU_CLIMB_CONTROL_ONE, RENU_CLIMB_CONTROL_TWO);
|
||||
}
|
||||
|
||||
void ria_control() {
|
||||
void ria_control()
|
||||
{
|
||||
subsysControl(RENU_PTO_TOGGLE, RENU_CATA_CONTROL, RENU_INTAKE_CONTROL_INTAKE, RENU_INTAKE_CONTROL_OUTTAKE);
|
||||
wingsControl(RENU_WING_CONTROL);
|
||||
climbControl(RENU_CLIMB_CONTROL_ONE, RENU_CLIMB_CONTROL_TWO);
|
||||
}
|
||||
|
||||
void chris_control() {
|
||||
void chris_control()
|
||||
{
|
||||
subsysControl(RENU_PTO_TOGGLE, RENU_CATA_CONTROL, RENU_INTAKE_CONTROL_INTAKE, RENU_INTAKE_CONTROL_OUTTAKE);
|
||||
wingsControl(RENU_WING_CONTROL);
|
||||
climbControl(RENU_CLIMB_CONTROL_ONE, RENU_CLIMB_CONTROL_TWO);
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user