From 2fb4f081d43f443ce2e2e2144587ac99cba4f8bd Mon Sep 17 00:00:00 2001 From: Udunen Date: Tue, 9 Apr 2024 12:23:03 -0500 Subject: [PATCH] rivera is dumb --- Ri3D 2024.v5code | 2 +- include/joystick.cpp | 96 ++++++++++++++++---------- src/main.cpp | 158 +++++++++++++++++++++++-------------------- 3 files changed, 148 insertions(+), 108 deletions(-) diff --git a/Ri3D 2024.v5code b/Ri3D 2024.v5code index 3663ce2..04d4b56 100644 --- a/Ri3D 2024.v5code +++ b/Ri3D 2024.v5code @@ -1 +1 @@ -{"title":"Ri3D 2024","description":"Empty V5 C++ Project","icon":"USER921x.bmp","version":"23.09.1216","sdk":"20220726_10_00_00","language":"cpp","competition":false,"files":[{"name":"include/robot-config.h","type":"File","specialType":"device_config"},{"name":"include/vex.h","type":"File","specialType":""},{"name":"include/joystick.cpp","type":"File","specialType":""},{"name":"makefile","type":"File","specialType":""},{"name":"src/main.cpp","type":"File","specialType":""},{"name":"src/robot-config.cpp","type":"File","specialType":"device_config"},{"name":"vex/mkenv.mk","type":"File","specialType":""},{"name":"vex/mkrules.mk","type":"File","specialType":""},{"name":"include","type":"Directory"},{"name":"src","type":"Directory"},{"name":"vex","type":"Directory"}],"device":{"slot":1,"uid":"276-4810","options":{}},"isVexFileImport":false,"robotconfig":[]} \ No newline at end of file +{"title":"Ri3D 2024","description":"Empty V5 C++ Project","icon":"USER921x.bmp","version":"23.09.1216","sdk":"20220726_10_00_00","language":"cpp","competition":false,"files":[{"name":"include/robot-config.h","type":"File","specialType":"device_config"},{"name":"include/vex.h","type":"File","specialType":""},{"name":"include/joystick.cpp","type":"File","specialType":""},{"name":"makefile","type":"File","specialType":""},{"name":"src/main.cpp","type":"File","specialType":""},{"name":"src/robot-config.cpp","type":"File","specialType":"device_config"},{"name":"vex/mkenv.mk","type":"File","specialType":""},{"name":"vex/mkrules.mk","type":"File","specialType":""},{"name":"include","type":"Directory"},{"name":"src","type":"Directory"},{"name":"vex","type":"Directory"}],"device":{"slot":4,"uid":"276-4810","options":{}},"isExpertMode":false,"isExpertModeRC":false,"isVexFileImport":false,"robotconfig":[],"neverUpdate":null} \ No newline at end of file diff --git a/include/joystick.cpp b/include/joystick.cpp index f3fe729..b09d956 100644 --- a/include/joystick.cpp +++ b/include/joystick.cpp @@ -2,8 +2,9 @@ #include + /* - this class was made by Cole Huffine class of 2024 + this class was made by Cole Huffine, class of 2024 lets hope rivera gives this to future classes bc i think it would be useful for everyone */ @@ -14,21 +15,16 @@ Usage: If you need to get the value of a joystick fixed to account for the physical limitations - getLeftX(); - getLeftY(); - getRightX(); - getRightY(); + getLeftX(con, deadband); + getLeftY(con, deadband); + getRightX(con, deadband); + getRightY(con, deadband); If you need to get the REAL value of a joystick, as inputted into the brain - getTrueLeftX(); - getTrueLeftY(); - getTrueRightX(); - getTrueRightY(); -*/ - -/* - If you want to use a second controller, or name your controller something else other than Controller1, - then go to all of the getTrue methods, and change the name to what you have as your config + getTrueLeftX(con, deadband); + getTrueLeftY(con, deadband); + getTrueRightX(con, deadband); + getTrueRightY(con, deadband); */ @@ -39,22 +35,37 @@ int signum(double val) { return (val >= 0) ? 1 : -1; } - //use these if you need to get the true value of any joystick -double getTrueLeftX() { - return Controller1.Axis4.position(pct); +double getTrueLeftX(vex::controller con, int deadband) { + if (abs(con.Axis4.position(pct)) > deadband) { + return con.Axis4.position(pct); + } else { + return 0; + } } -double getTrueLeftY() { - return Controller1.Axis3.position(pct); +double getTrueLeftY(vex::controller con, int deadband) { + if (abs(con.Axis3.position(pct)) > deadband) { + return con.Axis3.position(pct); + } else { + return 0; + } } -double getTrueRightX() { - return Controller1.Axis1.position(pct); +double getTrueRightX(vex::controller con, int deadband) { + if (abs(con.Axis1.position(pct)) > deadband) { + return con.Axis1.position(pct); + } else { + return 0; + } } -double getTrueRightY() { - return Controller1.Axis2.position(pct); +double getTrueRightY(vex::controller con, int deadband) { + if (abs(con.Axis2.position(pct)) > deadband) { + return con.Axis2.position(pct); + } else { + return 0; + } } /* @@ -63,10 +74,10 @@ double getTrueRightY() { * forces the joystick into a circle, its impossible to reach the the corner of the box. * This method fixes that issue. */ -double joystickFix(bool isLeft, bool isX) { +double joystickFix(bool isLeft, bool isX, vex::controller con) { double root2 = sqrt(2); - double x = isLeft ? getTrueLeftX() : getTrueRightX(); - double y = isLeft ? getTrueLeftY() : getTrueRightY(); + double x = isLeft ? getTrueLeftX(con, 0) : getTrueRightX(con, 0); + double y = isLeft ? getTrueLeftY(con, 0) : getTrueRightY(con, 0); double magnitude = sqrt(x*x + y*y); double values[2] = { signum(x) * fmin(fabs(x*root2), magnitude), @@ -77,20 +88,37 @@ double joystickFix(bool isLeft, bool isX) { // these return the fixed value of all of the joysticks -double getLeftX() { - return joystickFix(true, true); +double getLeftX(vex::controller con, int deadband) { + if(fabs(joystickFix(true, true, con)) > deadband){ + return joystickFix(true, true, con); + } else { + return 0; + } } -double getLeftY() { - return joystickFix(true, false); +double getLeftY(vex::controller con, int deadband) { + if(fabs(joystickFix(true, false, con)) > deadband){ + return joystickFix(true, false, con); + } else { + return 0; + } } -double getRightX() { - return joystickFix(false, true); +double getRightX(vex::controller con, int deadband) { + if(fabs(joystickFix(false, true, con)) > deadband){ + return joystickFix(false, true, con); + } else { + return 0; + } } -double getRightY() { - return joystickFix(false, false); +double getRightY(vex::controller con, int deadband) { + if(fabs(joystickFix(false, false, con)) > deadband){ + return joystickFix(false, false, con); + } else { + return 0; + } } + diff --git a/src/main.cpp b/src/main.cpp index ff4dc4c..8d4d4f6 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -20,114 +20,126 @@ using namespace vex; competition Competition; -motor LeftDriveMotor = motor(1, ratio18_1, false); -motor RightDriveMotor = motor(2, ratio18_1, false); -motor FrontDriveMotor = motor(3, ratio18_1, false); -motor BackDriveMotor = motor(4, ratio18_1, false); -inertial Inertial = inertial(5); -motor arm = motor(7, ratio36_1, false); -motor deploy = motor(8, ratio18_1, false); -motor climbMotorA = motor(9, ratio18_1, false); -motor climbMotorB = motor(10, ratio18_1, true); +motor leftFrontDriveMotor = motor(PORT16, ratio18_1, false); +motor leftBackDriveMotor = motor(PORT11, ratio18_1, false); +motor rightFrontDriveMotor = motor(PORT6, ratio18_1, false); +motor rightBackDriveMotor = motor(PORT12, ratio18_1, false); +motor frontDriveMotor = motor(PORT3, ratio18_1, true); +motor backDriveMotor = motor(PORT4, ratio18_1, false); +inertial inertialSens = inertial(PORT21); +motor arm = motor(PORT7, ratio36_1, false); +motor deploy = motor(PORT8, ratio18_1, false); +motor climbMotorA = motor(PORT9, ratio18_1, false); +motor climbMotorB = motor(PORT10, ratio36_1, true); motor_group climb = motor_group(climbMotorA, climbMotorB); -controller Controller1 = controller(primary); +motor_group leftDrive = motor_group(leftFrontDriveMotor, leftBackDriveMotor); +motor_group rightDrive = motor_group(rightFrontDriveMotor, rightBackDriveMotor); +//using the climb group to run the new arm group to have two different motor carts run in tandem +controller con1 = controller(primary); class Drives { public: - void static robotOirented() { - double yInput = getLeftY(); - double xInput = getLeftX(); - double turning = getTrueRightX(); - double leftMotor = yInput + turning; - double rightMotor = yInput - turning; - double frontMotor = xInput + turning; - double backMotor = xInput - turning; - - LeftDriveMotor.spin(fwd, leftMotor, velocityUnits::pct); - RightDriveMotor.spin(fwd, rightMotor, velocityUnits::pct); - FrontDriveMotor.spin(fwd, frontMotor, velocityUnits::pct); - BackDriveMotor.spin(fwd, backMotor, velocityUnits::pct); - } - - void static autonDrive(double xIn, double yIn, double turnIn) { - double yInput = yIn; - double xInput = xIn; - double turning = turnIn; - double leftMotor = yInput + turning; - double rightMotor = yInput - turning; - double frontMotor = xInput + turning; - double backMotor = xInput - turning; - - LeftDriveMotor.spin(fwd, leftMotor, velocityUnits::pct); - RightDriveMotor.spin(fwd, rightMotor, velocityUnits::pct); - FrontDriveMotor.spin(fwd, frontMotor, velocityUnits::pct); - BackDriveMotor.spin(fwd, backMotor, velocityUnits::pct); + void static robotOriented(double xIn, double yIn, double turnIn) { + double leftMotor = yIn + turnIn; + double rightMotor = -yIn + turnIn; + double frontMotor = xIn + turnIn; + double backMotor = -xIn + turnIn; + + leftDrive.spin(fwd, leftMotor, velocityUnits::pct); + rightDrive.spin(fwd, rightMotor, velocityUnits::pct); + frontDriveMotor.spin(fwd, frontMotor, velocityUnits::pct); + backDriveMotor.spin(fwd, backMotor, velocityUnits::pct); } void static fieldOriented() { - double headingRadians = Inertial.heading() * 3.141592/180; - double yInput = getLeftY(); - double xInput = getLeftX(); + double headingRadians = inertialSens.heading() * 3.141592/180; + double yInput = getLeftY(con1, 5); + double xInput = getLeftX(con1, 5); double sineHeading = sin(headingRadians); double cosHeading = cos(headingRadians); double rotatedYInput = xInput * sineHeading + yInput * cosHeading; double rotatedXInput = xInput * cosHeading - yInput * sineHeading; - double turning = getTrueRightX(); + double turning = getTrueRightX(con1, 5); double leftMotor = rotatedYInput + turning; double rightMotor = -rotatedYInput + turning; double FrontMotor = rotatedXInput + turning; double backMotor = -rotatedXInput + turning; - LeftDriveMotor.spin(fwd, leftMotor, velocityUnits::pct); - RightDriveMotor.spin(fwd, rightMotor, velocityUnits::pct); - FrontDriveMotor.spin(fwd, FrontMotor, velocityUnits::pct); - BackDriveMotor.spin(fwd, backMotor, velocityUnits::pct); + leftDrive.spin(fwd, leftMotor, velocityUnits::pct); + rightDrive.spin(fwd, rightMotor, velocityUnits::pct); + frontDriveMotor.spin(fwd, FrontMotor, velocityUnits::pct); + backDriveMotor.spin(fwd, backMotor, velocityUnits::pct); } }; -void pre_auton(void) { - Inertial.setHeading(0.0, degrees); - Inertial.setRotation(0.0, degrees); - Inertial.startCalibration(); - while(Inertial.isCalibrating()) { +void usercontrol(void) { + inertialSens.setHeading(0.0, degrees); + inertialSens.setRotation(0.0, degrees); + inertialSens.startCalibration(); + while(inertialSens.isCalibrating()) { task::sleep(10); } - Inertial.setHeading(0.0, degrees); - Inertial.setRotation(0.0, degrees); - - // Initializing Robot Configuration. DO NOT REMOVE! - vexcodeInit(); -} - -void usercontrol(void) { + inertialSens.setHeading(0.0, degrees); + inertialSens.setRotation(0.0, degrees); while(true) { Drives::fieldOriented(); - //zeroes inertial sensor - if(Controller1.ButtonY.pressing()) { - Inertial.setHeading(0.0, degrees); - Inertial.setRotation(0.0, degrees); - Inertial.startCalibration(); - while (Inertial.isCalibrating()) { + if(con1.ButtonY.pressing()) { + inertialSens.setHeading(0.0, degrees); + inertialSens.setRotation(0.0, degrees); + inertialSens.startCalibration(); + while (inertialSens.isCalibrating()) { task::sleep(10); } - Inertial.setHeading(0.0, degrees); - Inertial.setRotation(0.0, degrees); + inertialSens.setHeading(0.0, degrees); + inertialSens.setRotation(0.0, degrees); + } + if(con1.ButtonLeft.pressing()) { + climbMotorA.setStopping(brake); + climbMotorB.setStopping(brake); + } + if(con1.ButtonRight.pressing()) { + climbMotorA.setStopping(hold); + climbMotorB.setStopping(hold); + } + if(con1.ButtonUp.pressing()) { + climb.setStopping(hold); + } + if(con1.ButtonDown.pressing()) { + climb.setStopping(coast); + } + if (con1.ButtonR1.pressing()) { + climb.spin(forward); + climb.setStopping(hold); + } else if (con1.ButtonR2.pressing()) { + climb.spin(reverse); + climb.setStopping(hold); + } else { + climb.stop(); + } + wait(20,msec); } - } -} + void auton(void) { - Drives::autonDrive(0.0, 0.5, 0); + Drives::robotOriented(0.0, 50, 0); task::sleep(4000); - Drives::autonDrive(0.0, 0.0, 0); + Drives::robotOriented(0.0, 0.0, 0); } int main() { - pre_auton(); - + climb.setMaxTorque(100, pct); + climb.setVelocity(100, pct); + climbMotorA.setVelocity(100, rpm); + climbMotorB.setVelocity(100, rpm); + climbMotorA.setMaxTorque(100, pct); + climbMotorB.setMaxTorque(100, pct); + climb.setStopping(coast); + + // Initializing Robot Configuration. DO NOT REMOVE! + vexcodeInit(); Competition.autonomous(auton); Competition.drivercontrol(usercontrol);