From 8e0e0338192b9811bf74253affb6a9efc9b08648 Mon Sep 17 00:00:00 2001 From: littlebro12 Date: Tue, 14 Sep 2021 17:12:41 +0300 Subject: [PATCH 1/8] Coded Turret and Gunner classes These files don't include the documentation --- .../executionHistory/executionHistory.bin | Bin 86049 -> 86049 bytes .../executionHistory/executionHistory.lock | Bin 17 -> 17 bytes .gradle/6.0.1/fileHashes/fileHashes.bin | Bin 21897 -> 22497 bytes .gradle/6.0.1/fileHashes/fileHashes.lock | Bin 17 -> 17 bytes .gradle/6.0.1/javaCompile/javaCompile.lock | Bin 17 -> 17 bytes .gradle/6.0.1/javaCompile/taskHistory.bin | Bin 24977 -> 29133 bytes .../buildOutputCleanup.lock | Bin 17 -> 17 bytes .gradle/buildOutputCleanup/outputFiles.bin | Bin 18803 -> 19019 bytes .idea/misc.xml | 4 +- src/main/java/frc/robot/Constants.java | 12 +++++ src/main/java/frc/robot/Ports.java | 8 +++ .../java/frc/robot/subsystems/UnitModel.java | 3 +- .../frc/robot/subsystems/turret/Turret.java | 47 ++++++++++++++++++ .../subsystems/turret/commands/Gunner.java | 35 +++++++++++++ 14 files changed, 107 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc/robot/Ports.java create mode 100644 src/main/java/frc/robot/subsystems/turret/Turret.java create mode 100644 src/main/java/frc/robot/subsystems/turret/commands/Gunner.java diff --git a/.gradle/6.0.1/executionHistory/executionHistory.bin b/.gradle/6.0.1/executionHistory/executionHistory.bin index ce2bb9ef7d708ce481f521159cffe8727aae42d4..fb49644b1e477247e005046f4a2166c7e5bc1dbf 100644 GIT binary patch delta 2941 zcmb7_ZA?>F7{~WWaFG^!@40hSy{xosiQ6(PbX^#mBC(Mf6@qay!IlZNhz80_DRc|l zAX|bGKP*0T$3>zr_F}RRfFuJxI7^n`YswgyNvhB&s1+BKEY59i*}cqVnfKiGdcWQK z|NqZ(o^yWZ+|G`~&W^;M?{BAncEs)T+5+y~WsUaI2HT$9wUsXiYTcEdS1PKWSa<*n zhc@&T4lPR{mlv(ialYyE)cKsgdjBquw;qe`93|HjpAYrILFC#6GR2azaLI5rJ7vd{ z!!_hCJ5_zUg+vq*y=w&F$)VOw&8=b0=b}&vG!^fX54d< z+e!(_SJ9vYeE9P*fl(DD9h-kWK&F{dNitE_2H1?<_XTo8;i02;IE>*CPkvNrIP^^m z>VFr=C8Y%At(X=P$WX$gyCk)mjprrWRP@=X#xuZ_DWW&kP z9c(F!uGMzIQ5=aBvn83mH<0UAOUnEev)S9n=I%pW(^HeG55l7- z%ju|g`>Ujj(1)3~jCg7$8RvSE`G>dT|5f@n2Pyb^uZ}HC;S`*m){7Hys!#(Dt8lK@ zt~yRS)|{}iRca#eEZ3B6$~WCu*%r3g_m|Btp20s)nAQEMXb2>sw{Ni_x4bBK@`FId zg)@QU+pmOagg}~jG2VQ;1+d`Gq7?DE2Img(fd-cikK8nJv)l8w^u4vzq-*rPJK|qy zrX5eg8gW`fJ{?P<9M{z_^W_U$f7@$>^`A^^(Tn(g^f~demiUr-e_frY9-tkYqs!7C zzQyHZD!P>0X1QlQFqSYJ-bykD$3Hfq@mrC(_L<_j*i%_8hGH0K%X0a@*w3JGK@!K~ zgkOCIAg%|K2N*od1#&PvU_gI^5OYKRb*@k@fh;-brsW`@Yfz<#15c_+IoRBoOjoY} zUW~-)RJQ8Gs0LS}bQJiP@*3>(-~Bb#-@Zmp1b;+~R4)@AtpUY+=vjdHyr9bQB5Iy$Hs~4Vm4Qr9R)BrXJ80)zL*Jsfc#J zWnp}Xb&4pz7OY%Xfpy@$`y=P`*Nyi0EA&sJwK7l1Bi7CqiZ^4-Rx%XeYgH^%GtE#2 LU-g%oA(-(uw~vVV delta 360 zcmZ3ufOX*l)(wZ>FrJydP?1rQi794sz(Lo|{s*UVPd{kNxMK3{bApo_?@XLLbD`XH zX%j}>$qoy7C!af;I$gkm@zCV^iv&0CKYxg4`z|*|8TQG38xl7Eef*tCNIte>z2vs$ zg6$z$2lHJl4l~Z1e9&KVv;6ZZj6xe#osK*WU3dRb@SnQxEgMA`_e^ehDLQ$>R)Ohv zMHs~rRi!Gc^)BJV?|H@CU-aA#GWDmV7MEn^C6;97=ecASabKLiP@OSwa>A<+ z#;YJ^C^I9J!FU_Qj|TD~493?We#~?QNk;zZ+a(xzZJ_& diff --git a/.gradle/6.0.1/executionHistory/executionHistory.lock b/.gradle/6.0.1/executionHistory/executionHistory.lock index f1467d8f6bd656d233d0a50d51712daa8cb56de3..7b5b969d5393cedba159d3db0439d0df58992b52 100644 GIT binary patch literal 17 UcmZSX2%KEGr{m;Q1_($805{kK@&Et; literal 17 UcmZSX2%KEGr{m;Q1_($505{SE>;M1& diff --git a/.gradle/6.0.1/fileHashes/fileHashes.bin b/.gradle/6.0.1/fileHashes/fileHashes.bin index e082f8f259a0ec8121a245d6056f87eee7d8f4cc..4549d02e106aa306243c6b6316ca830584481eed 100644 GIT binary patch delta 570 zcmeBN&G>LV;|3E6#(>GD5*`8*Zx}b1e4V$20Sv;LCzncC2+W@J(pR_`vjlGZG&$&3y^$Jul@IMsn3p@XD z^2hFr5ba|4ICG+6Mfly@hoNeCA!^t^ z18F^DAfXAwiXbfTI^5BBa~PjIcX?jb`BG`1AlOuB$TN3g>R4R%bg5R*&nyW>=P5UR zd1R1vurM$%nM@A!RR#LM&4_EVVIa^4K$g(tzz{taAVYleeP1aF$1>AT`~F?t-fm^l krS!O_5@ZBWQfl&6h$`91&qMSCfFklh>w$oo4M^w#0LZt$ZvX%Q delta 72 zcmaF3p0RT^;|3E6MwiK^5+0MMN*GMO3&g6Dl9R(E<0n6roUnPRR0Z>7Q@II~@5&`k c4wDz3+$wLy#8$Xb@rU?EgB60C9X&jF0n0%eD*ylh diff --git a/.gradle/6.0.1/fileHashes/fileHashes.lock b/.gradle/6.0.1/fileHashes/fileHashes.lock index 5e203abfd30da9f0760e5a85d91f9877736a7fd6..f663ba8a573722d07c33ebcd2b2e8cd838070e02 100644 GIT binary patch literal 17 UcmZSP4k`IDfpz_61_+n}04;n3S^xk5 literal 17 UcmZSP4k`IDfpz_61_)>a04;6=OaK4? diff --git a/.gradle/6.0.1/javaCompile/javaCompile.lock b/.gradle/6.0.1/javaCompile/javaCompile.lock index 06eb0196f03a7f45d1916662a2dbb9907b2323bc..cbf2ddcf1bdcd7f633de11b5bb8dfe19bd15b01e 100644 GIT binary patch literal 17 VcmZQh)cDS>`_X?N0~m0u1pqC91Zw~Q literal 17 VcmZQh)cDS>`_X?N0~m0u0st+31ZV&N diff --git a/.gradle/6.0.1/javaCompile/taskHistory.bin b/.gradle/6.0.1/javaCompile/taskHistory.bin index c2dfa6cefe0f53b834b08c3b062b223246e6aea0..b85a07d2e6f75b405942af562e800d7bb12cdb30 100644 GIT binary patch delta 747 zcmbPunDOjm#t9;_OblS);mE+iX$@fq)-W&#hyvNV8x4gdm=Y&$c9rn))HDW)g0MCS zpJH^jiU}=FEh>&FE-6h(%`1s<$xklL1u}{!uL*P(y2FI7VDiCejmhdkB|?uer5Zzx zCf^Mz6uOUHYC>=>E?xE^g+d>(>-rQ@FZ2ML)B>STn8Jo(n}n`o3WHpx8lEb22Ah=1 z5Gnb|*1p1%eIg@-p5W3gKlyiLtk84p(i5WUg)U)} zGMyX}9V>JLo74=UgUlEa1M=hSn0|bI%!!Q>I)`asV<;pDIO1aP>F)C9nY<~^51$gJ c_$b`c8A2y;+g+6qCUgZ;8WhGHi5-j<0Ds*YOaK4? delta 45 wcmX^6m~rA^#t9;Vj0|Al;mE+iX$@p5ZZv!$KGA_^vZRC-Q((l0qU^=U;sVRlgD@e#PVYB854VttGkyK2;?jP<`w3 z5UQkg@?HrG4IYKxMnB$_!SwBh>65$q`N8}P{54R;_b0!Vun@3WDL3QFzlJ=h<(89W zB{KwqHu?BB87S?Asw|#dE2$t*pcU{dXsOFrsNl!RQ-Ok|Jm+Kk?-cBV3QnKAR#JEuSmAL1Jg0z@}EdKfWI_V-koyx+rla)O7-WIqpI04kJ<)Bpeg delta 55 zcmX>-h4J$w#tkMCjDnNbN=%p>DrqozD-i#cOqg6MB{6v^5MPy2Vq#pmQSpcPMuP&e J&5j;Mi~v;76jJ~I diff --git a/.idea/misc.xml b/.idea/misc.xml index 6b60455..411ab32 100644 --- a/.idea/misc.xml +++ b/.idea/misc.xml @@ -4,5 +4,7 @@ - + + + \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index cf0c82e..5785403 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -16,4 +16,16 @@ * constants are needed, to reduce verbosity. */ public final class Constants { + + public static class Turret{ + public static double MAX_ANGLE = 0; + public static double MIN_ANGLE = 0; + public static double CYCLE_TIME = 0.2; + public static double TICKS_PER_DEGREE = 4096 / 360; + public static double RADIUS = 19.0; + public static double TURRET_CIRCUMFERENCE = Math.PI * 2 * RADIUS; + public static double kP = 0; + public static double kI = 0; + public static double kD = 0; + } } diff --git a/src/main/java/frc/robot/Ports.java b/src/main/java/frc/robot/Ports.java new file mode 100644 index 0000000..960a010 --- /dev/null +++ b/src/main/java/frc/robot/Ports.java @@ -0,0 +1,8 @@ +package frc.robot; + +public final class Ports { + + public static class Turret{ + public static int motorPort = 0; + } +} diff --git a/src/main/java/frc/robot/subsystems/UnitModel.java b/src/main/java/frc/robot/subsystems/UnitModel.java index d1c75b0..ed62295 100644 --- a/src/main/java/frc/robot/subsystems/UnitModel.java +++ b/src/main/java/frc/robot/subsystems/UnitModel.java @@ -1,5 +1,7 @@ package frc.robot.subsystems; +import static frc.robot.Constants.Turret.*; + public class UnitModel { double ticksPerUnit; @@ -22,5 +24,4 @@ public double toVelocity(double ticks100ms){ public int toTicks100ms(double velocity){ return (int) (velocity * ticksPerUnit / 10); } - } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java new file mode 100644 index 0000000..0fe031e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -0,0 +1,47 @@ +package frc.robot.subsystems.turret; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import edu.wpi.first.wpilibj.controller.PIDController; +import frc.robot.subsystems.UnitModel; + +import static frc.robot.Constants.Turret.*; +import static frc.robot.Ports.Turret.*; + +public class Turret { + private double currAngle = 0; + private double targetAngle; + private UnitModel unitMan = new UnitModel(TICKS_PER_DEGREE); + private TalonSRX motor = new TalonSRX(motorPort); + + public Turret(){ + motor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative); + motor.config_kP(0, kP); + motor.config_kI(0, kI); + motor.config_kD(0, kD); + motor.setSensorPhase(true); + } + + public void setTargetAngle(double targetAngle) { + if(targetAngle > MAX_ANGLE) + this.targetAngle = MAX_ANGLE; + else if(targetAngle < MIN_ANGLE) + this.targetAngle = MIN_ANGLE; + else + this.targetAngle = targetAngle; + } + + public void setCurrAngle(double currAngle){ + this.currAngle = currAngle; + } + + public void setAngle(){ + motor.set(ControlMode.Position, unitMan.toTicks(targetAngle)); + } + + public void terminate(){ + targetAngle = 0; + setAngle(); + } +} diff --git a/src/main/java/frc/robot/subsystems/turret/commands/Gunner.java b/src/main/java/frc/robot/subsystems/turret/commands/Gunner.java new file mode 100644 index 0000000..3c1b5ae --- /dev/null +++ b/src/main/java/frc/robot/subsystems/turret/commands/Gunner.java @@ -0,0 +1,35 @@ +package frc.robot.subsystems.turret.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.turret.Turret; + +public class Gunner extends CommandBase { + private double targetAngle; + private Turret gunnerMan = new Turret(); + + public Gunner(double targetAngle){ + this.targetAngle = targetAngle; + } + + @Override + public void initialize() { + + } + + @Override + public void execute() { + gunnerMan.setTargetAngle(targetAngle); + gunnerMan.setAngle(); + gunnerMan.setCurrAngle(targetAngle); + } + + @Override + public void end(boolean interrupted) { + gunnerMan.terminate(); + } + + @Override + public boolean isFinished() { + return false; + } +} From c4b3f01879b2a1e133011e1dc93d2fa06539be4d Mon Sep 17 00:00:00 2001 From: littlebro12 Date: Tue, 14 Sep 2021 18:59:45 +0300 Subject: [PATCH 2/8] Documentation Documentation for the Gunner and Turret classes --- .../buildOutputCleanup.lock | Bin 17 -> 17 bytes .gradle/buildOutputCleanup/outputFiles.bin | Bin 19019 -> 0 bytes .../frc/robot/subsystems/turret/Turret.java | 25 ++++++++++++++---- .../subsystems/turret/commands/Gunner.java | 13 +++++---- 4 files changed, 26 insertions(+), 12 deletions(-) delete mode 100644 .gradle/buildOutputCleanup/outputFiles.bin diff --git a/.gradle/buildOutputCleanup/buildOutputCleanup.lock b/.gradle/buildOutputCleanup/buildOutputCleanup.lock index e53d20e53a55f4f830f199100b441ddfb31fa52b..f4731f0e2629224a8345f07a08429af3d5901ee7 100644 GIT binary patch literal 17 UcmZR6wPBy2WV-wZ1_;On05l;4X#fBK literal 17 UcmZR6wPBy2WV-wZ1_;Oj05l*3XaE2J diff --git a/.gradle/buildOutputCleanup/outputFiles.bin b/.gradle/buildOutputCleanup/outputFiles.bin deleted file mode 100644 index 9f0635463715774be6d6c08e483cd405bb5bbe49..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 19019 zcmeI%Ur1A77{~E9Gt8VBfiaDAg^_876bP1GND}WE1I6;9Aj%?w3>)Pi2$3j8MO4J1 zSr(BIIch}F6!TBf3M+{eG+b!9f|79ku|`rcJE!+~lmU6zLB#OFfKv zS%h&CS1ClMNk3WOcEuL0iYoJ}udPe?{XxT>mNQG|SN)n6!aAM1@2t~Y+};sZ^V(DH z53m2oN$Gm@g4T)W-nnhV#%p_e-qF09d$)XBXF2cpsV-}|`?E3*SA5(`_oEeaAJ}y1 zWpr@H6PjPoeXyX!edrLq`;Z~-ACmpfoAiXO1C5|%c>D*%yU(NNncbBU!ySSSy-&c&jui)ox=AP8^dGKOqTotV|z}*rU zP4(D@YG@q~_q6P+6Au;|KasJ?O71!Cq?+#Vfl}42+1zuTr!F5c4_{DS-sLV6+t>Ct zxl|Tf6nAasJn!mVt;=ctDEGqCzTT@t$H`yjj`v%({DYZ`TGhO^k9$eo^VDMr>*#+X zcW}3-6?`kak{6=+Pu%x*%)HU|@nYw$P%*vCQnL5Kidi9^^Z MAX_ANGLE) this.targetAngle = MAX_ANGLE; @@ -32,14 +37,24 @@ else if(targetAngle < MIN_ANGLE) this.targetAngle = targetAngle; } + /** + * Sets the current angle of the turret. + * @param currAngle is the current angle of the turret. + */ public void setCurrAngle(double currAngle){ this.currAngle = currAngle; } + /** + * Changes the position of the motor, hence changing the position of the turret. + */ public void setAngle(){ - motor.set(ControlMode.Position, unitMan.toTicks(targetAngle)); + motor.set(ControlMode.Position, unitMan.toTicks(targetAngle - currAngle)); } + /** + * Sets the turret back to 0. + */ public void terminate(){ targetAngle = 0; setAngle(); diff --git a/src/main/java/frc/robot/subsystems/turret/commands/Gunner.java b/src/main/java/frc/robot/subsystems/turret/commands/Gunner.java index 3c1b5ae..61817c4 100644 --- a/src/main/java/frc/robot/subsystems/turret/commands/Gunner.java +++ b/src/main/java/frc/robot/subsystems/turret/commands/Gunner.java @@ -4,18 +4,15 @@ import frc.robot.subsystems.turret.Turret; public class Gunner extends CommandBase { - private double targetAngle; - private Turret gunnerMan = new Turret(); + private double targetAngle; // Angle the turret needs to be at. + private Turret gunnerMan = new Turret(); // Turret class object (in order to move the motor). + // Constructor. public Gunner(double targetAngle){ this.targetAngle = targetAngle; } - @Override - public void initialize() { - - } - + // Executes all the commands in a loop. @Override public void execute() { gunnerMan.setTargetAngle(targetAngle); @@ -23,11 +20,13 @@ public void execute() { gunnerMan.setCurrAngle(targetAngle); } + // Resets the turret. @Override public void end(boolean interrupted) { gunnerMan.terminate(); } + // Continues running the program until interrupted. @Override public boolean isFinished() { return false; From 8afb36a6abf4e7f21f919121377b8e62ae000d12 Mon Sep 17 00:00:00 2001 From: littlebro12 Date: Fri, 17 Sep 2021 10:51:37 +0300 Subject: [PATCH 3/8] Set up buttons Setting up the buttons for Gunner class. --- src/main/java/frc/robot/RobotContainer.java | 73 +++++++++++-------- .../subsystems/turret/commands/Gunner.java | 33 +++++++-- 2 files changed, 68 insertions(+), 38 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ed3b7fc..3e91352 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,9 +9,15 @@ import com.revrobotics.ColorSensorV3; import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.subsystems.ExampleSubsystem.ExampleSubsystem; +import frc.robot.subsystems.turret.*; +import frc.robot.subsystems.turret.commands.Gunner; + /** * This class is where the bulk of the robot should be declared. Since Command-based is a * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} @@ -19,35 +25,40 @@ * (including subsystems, commands, and button mappings) should be declared here. */ public class RobotContainer { - // The robot's subsystems and commands are defined here... - - - /** - * The container for the robot. Contains subsystems, OI devices, and commands. - */ - public RobotContainer() { - // Configure the button bindings - configureButtonBindings(); - } - - /** - * Use this method to define your button->command mappings. Buttons can be created by - * instantiating a {@link GenericHID} or one of its subclasses ({@link - * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a - * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. - */ - private void configureButtonBindings() { - } - - - /** - * Use this to pass the autonomous command to the main {@link Robot} class. - * - * @return the command to run in autonomous - */ - public Command getAutonomousCommand() { - - // An ExampleCommand will run in autonomous - return null; - } + private final XboxController xbox = new XboxController(1); + Trigger LT = new Trigger(() -> xbox.getRawAxis(XboxController.Axis.kLeftTrigger.value) > 0.3); + private final Turret gunnerMan = new Turret(); + // The robot's subsystems and commands are defined here... + + + /** + * The container for the robot. Contains subsystems, OI devices, and commands. + */ + public RobotContainer() { + gunnerMan.setDefaultCommand(new Gunner(gunnerMan)); + // Configure the button bindings + configureButtonBindings(); + } + + /** + * Use this method to define your button->command mappings. Buttons can be created by + * instantiating a {@link GenericHID} or one of its subclasses ({@link + * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a + * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. + */ + private void configureButtonBindings() { + + } + + + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ + public Command getAutonomousCommand() { + + // An ExampleCommand will run in autonomous + return null; + } } diff --git a/src/main/java/frc/robot/subsystems/turret/commands/Gunner.java b/src/main/java/frc/robot/subsystems/turret/commands/Gunner.java index 61817c4..acbfddd 100644 --- a/src/main/java/frc/robot/subsystems/turret/commands/Gunner.java +++ b/src/main/java/frc/robot/subsystems/turret/commands/Gunner.java @@ -1,23 +1,39 @@ package frc.robot.subsystems.turret.commands; +import edu.wpi.first.wpilibj.*; import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.button.*; import frc.robot.subsystems.turret.Turret; public class Gunner extends CommandBase { + private double X; + private double Y; private double targetAngle; // Angle the turret needs to be at. - private Turret gunnerMan = new Turret(); // Turret class object (in order to move the motor). + private Turret gunnerMan; // Turret class object (in order to move the motor). + private final XboxController xbox = new XboxController(1); + Trigger LT = new Trigger(() -> xbox.getRawAxis(XboxController.Axis.kLeftTrigger.value) > 0.3); - // Constructor. - public Gunner(double targetAngle){ - this.targetAngle = targetAngle; + public Gunner(Turret gunnerMan){ + this.gunnerMan = gunnerMan; + addRequirements(gunnerMan); } // Executes all the commands in a loop. @Override public void execute() { + X = xbox.getX(GenericHID.Hand.kRight); + Y = xbox.getY(GenericHID.Hand.kRight); + targetAngle = Math.atan2(Y, X); + + gunnerMan.setTargetAngle(targetAngle); - gunnerMan.setAngle(); - gunnerMan.setCurrAngle(targetAngle); + + if(LT.get()) + gunnerMan.setAngle(); + + if (isFinished()) { + gunnerMan.setCurrAngle(targetAngle); + } } // Resets the turret. @@ -29,6 +45,9 @@ public void end(boolean interrupted) { // Continues running the program until interrupted. @Override public boolean isFinished() { - return false; + if (!gunnerMan.anglesEqual()) + return false; + else + return true; } } From 2bb8d16032b68ffb42f45441f7d0949b3e8f95f3 Mon Sep 17 00:00:00 2001 From: littlebro12 Date: Fri, 17 Sep 2021 10:52:13 +0300 Subject: [PATCH 4/8] Refine the program Some details were worked out in this version of the code. --- src/main/java/frc/robot/Constants.java | 4 +-- .../frc/robot/subsystems/turret/Turret.java | 33 +++++++++++++++++-- 2 files changed, 31 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 5785403..8e6e33d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -18,12 +18,10 @@ public final class Constants { public static class Turret{ + public static double ERROR_RANGE = 2; public static double MAX_ANGLE = 0; public static double MIN_ANGLE = 0; - public static double CYCLE_TIME = 0.2; public static double TICKS_PER_DEGREE = 4096 / 360; - public static double RADIUS = 19.0; - public static double TURRET_CIRCUMFERENCE = Math.PI * 2 * RADIUS; public static double kP = 0; public static double kI = 0; public static double kD = 0; diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index d20ca6b..5e5bd2e 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -3,14 +3,14 @@ import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.can.TalonSRX; -import edu.wpi.first.wpilibj.controller.PIDController; +import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.subsystems.UnitModel; import static frc.robot.Constants.Turret.*; import static frc.robot.Ports.Turret.*; -public class Turret { - private double currAngle = 0; // The current angle of the turret. +public class Turret extends SubsystemBase { + private double currAngle = 90; // The current angle of the turret. private double targetAngle; // The angle the turret needs to get to. private UnitModel unitMan = new UnitModel(TICKS_PER_DEGREE); // The unit conversion module. private TalonSRX motor = new TalonSRX(motorPort); // The motor used to spin the turret. @@ -37,6 +37,22 @@ else if(targetAngle < MIN_ANGLE) this.targetAngle = targetAngle; } + /** + * Gets the current angle of the turret + * @return the current angle + */ + public double getCurrAngle() { + return currAngle; + } + + /** + * Gets the target angle of the turret + * @return the target angle + */ + public double getTargetAngle() { + return targetAngle; + } + /** * Sets the current angle of the turret. * @param currAngle is the current angle of the turret. @@ -52,6 +68,17 @@ public void setAngle(){ motor.set(ControlMode.Position, unitMan.toTicks(targetAngle - currAngle)); } + /** + * Checks whether or not the target angle and the current angle are equal in a certain error range. + * @return whether the angles are equal. + */ + public boolean anglesEqual(){ + if(currAngle == targetAngle + ERROR_RANGE || currAngle == targetAngle - ERROR_RANGE){ + return true; + } + return false; + } + /** * Sets the turret back to 0. */ From d051491db8a08b3c22bb041dbb8906391e582b21 Mon Sep 17 00:00:00 2001 From: littlebro12 Date: Fri, 17 Sep 2021 13:20:58 +0300 Subject: [PATCH 5/8] Barrel's pull request changes --- src/main/java/frc/robot/Constants.java | 14 ++++---- src/main/java/frc/robot/Ports.java | 4 ++- src/main/java/frc/robot/RobotContainer.java | 2 -- .../java/frc/robot/subsystems/UnitModel.java | 2 -- .../frc/robot/subsystems/turret/Turret.java | 34 +++++++++++-------- .../subsystems/turret/commands/Gunner.java | 11 ++---- 6 files changed, 31 insertions(+), 36 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8e6e33d..5e22a04 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -18,12 +18,12 @@ public final class Constants { public static class Turret{ - public static double ERROR_RANGE = 2; - public static double MAX_ANGLE = 0; - public static double MIN_ANGLE = 0; - public static double TICKS_PER_DEGREE = 4096 / 360; - public static double kP = 0; - public static double kI = 0; - public static double kD = 0; + public static final double ERROR_RANGE = 2; // degrees + public static final double MAX_ANGLE = 0; // degrees + public static final double MIN_ANGLE = 0; // degrees + public static final double TICKS_PER_DEGREE = 4096 / 360; + public static final double kP = 0; + public static final double kI = 0; + public static final double kD = 0; } } diff --git a/src/main/java/frc/robot/Ports.java b/src/main/java/frc/robot/Ports.java index 960a010..2ce0f9c 100644 --- a/src/main/java/frc/robot/Ports.java +++ b/src/main/java/frc/robot/Ports.java @@ -3,6 +3,8 @@ public final class Ports { public static class Turret{ - public static int motorPort = 0; + public final static boolean INVERTED = false; + public final static boolean SENSOR_PHASE = false; + public final static int MOTOR = 0; } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3e91352..dbf7bc6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -25,8 +25,6 @@ * (including subsystems, commands, and button mappings) should be declared here. */ public class RobotContainer { - private final XboxController xbox = new XboxController(1); - Trigger LT = new Trigger(() -> xbox.getRawAxis(XboxController.Axis.kLeftTrigger.value) > 0.3); private final Turret gunnerMan = new Turret(); // The robot's subsystems and commands are defined here... diff --git a/src/main/java/frc/robot/subsystems/UnitModel.java b/src/main/java/frc/robot/subsystems/UnitModel.java index ed62295..d1711d8 100644 --- a/src/main/java/frc/robot/subsystems/UnitModel.java +++ b/src/main/java/frc/robot/subsystems/UnitModel.java @@ -1,7 +1,5 @@ package frc.robot.subsystems; -import static frc.robot.Constants.Turret.*; - public class UnitModel { double ticksPerUnit; diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 5e5bd2e..ae8db12 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -10,23 +10,27 @@ import static frc.robot.Ports.Turret.*; public class Turret extends SubsystemBase { - private double currAngle = 90; // The current angle of the turret. - private double targetAngle; // The angle the turret needs to get to. + private double currAngle = 90; // The current angle of the turret. [degrees] + private double targetAngle; // The angle the turret needs to get to. [degrees] private UnitModel unitMan = new UnitModel(TICKS_PER_DEGREE); // The unit conversion module. - private TalonSRX motor = new TalonSRX(motorPort); // The motor used to spin the turret. + private TalonSRX motor = new TalonSRX(MOTOR); // The motor used to spin the turret. //Constructor. public Turret(){ motor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative); + motor.config_kP(0, kP); motor.config_kI(0, kI); motor.config_kD(0, kD); - motor.setSensorPhase(true); + + motor.setInverted(INVERTED); + + motor.setSensorPhase(SENSOR_PHASE); } /** - * This function sets the angle the turret needs to be at. - * @param targetAngle is the angle the turret needs to be at. + * This function sets the angle the turret needs to be at. [degrees] + * @param targetAngle is the angle the turret needs to be at. [degrees] */ public void setTargetAngle(double targetAngle) { if(targetAngle > MAX_ANGLE) @@ -38,24 +42,24 @@ else if(targetAngle < MIN_ANGLE) } /** - * Gets the current angle of the turret - * @return the current angle + * Gets the current angle of the turret. [degrees] + * @return the current angle. [degrees] */ public double getCurrAngle() { return currAngle; } /** - * Gets the target angle of the turret - * @return the target angle + * Gets the target angle of the turret. [degrees] + * @return the target angle. [degrees] */ public double getTargetAngle() { return targetAngle; } /** - * Sets the current angle of the turret. - * @param currAngle is the current angle of the turret. + * Sets the current angle of the turret. [degrees] + * @param currAngle is the current angle of the turret. [degrees] */ public void setCurrAngle(double currAngle){ this.currAngle = currAngle; @@ -64,7 +68,7 @@ public void setCurrAngle(double currAngle){ /** * Changes the position of the motor, hence changing the position of the turret. */ - public void setAngle(){ + public void setPosition(){ motor.set(ControlMode.Position, unitMan.toTicks(targetAngle - currAngle)); } @@ -73,7 +77,7 @@ public void setAngle(){ * @return whether the angles are equal. */ public boolean anglesEqual(){ - if(currAngle == targetAngle + ERROR_RANGE || currAngle == targetAngle - ERROR_RANGE){ + if(currAngle <= targetAngle + ERROR_RANGE || currAngle >= targetAngle - ERROR_RANGE){ return true; } return false; @@ -84,6 +88,6 @@ public boolean anglesEqual(){ */ public void terminate(){ targetAngle = 0; - setAngle(); + setPosition(); } } diff --git a/src/main/java/frc/robot/subsystems/turret/commands/Gunner.java b/src/main/java/frc/robot/subsystems/turret/commands/Gunner.java index acbfddd..f2ce9ff 100644 --- a/src/main/java/frc/robot/subsystems/turret/commands/Gunner.java +++ b/src/main/java/frc/robot/subsystems/turret/commands/Gunner.java @@ -29,11 +29,7 @@ public void execute() { gunnerMan.setTargetAngle(targetAngle); if(LT.get()) - gunnerMan.setAngle(); - - if (isFinished()) { - gunnerMan.setCurrAngle(targetAngle); - } + gunnerMan.setPosition(); } // Resets the turret. @@ -45,9 +41,6 @@ public void end(boolean interrupted) { // Continues running the program until interrupted. @Override public boolean isFinished() { - if (!gunnerMan.anglesEqual()) - return false; - else - return true; + return !gunnerMan.anglesEqual(); } } From d80e27fcc056b874d4b1674cd3a1b2d27a7a0d83 Mon Sep 17 00:00:00 2001 From: littlebro12 Date: Fri, 17 Sep 2021 13:31:21 +0300 Subject: [PATCH 6/8] Set the max and min angles --- src/main/java/frc/robot/Constants.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 5e22a04..c3b32f0 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -19,8 +19,8 @@ public final class Constants { public static class Turret{ public static final double ERROR_RANGE = 2; // degrees - public static final double MAX_ANGLE = 0; // degrees - public static final double MIN_ANGLE = 0; // degrees + public static final double MAX_ANGLE = 135; // degrees + public static final double MIN_ANGLE = 45; // degrees public static final double TICKS_PER_DEGREE = 4096 / 360; public static final double kP = 0; public static final double kI = 0; From f010ae81c71adff78ebc75e5add710a1d7c9f738 Mon Sep 17 00:00:00 2001 From: littlebro123 Date: Sun, 19 Sep 2021 19:25:25 +0300 Subject: [PATCH 7/8] Bug fixes --- src/main/java/frc/robot/Constants.java | 10 ++--- src/main/java/frc/robot/Ports.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 4 +- .../frc/robot/subsystems/turret/Turret.java | 33 ++++++++------ .../subsystems/turret/commands/Gunner.java | 3 +- .../subsystems/turret/commands/Manual.java | 43 +++++++++++++++++++ src/test/java/TestClass.java | 19 ++++++++ 7 files changed, 93 insertions(+), 21 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/turret/commands/Manual.java create mode 100644 src/test/java/TestClass.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c3b32f0..ac4bfac 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -19,11 +19,11 @@ public final class Constants { public static class Turret{ public static final double ERROR_RANGE = 2; // degrees - public static final double MAX_ANGLE = 135; // degrees - public static final double MIN_ANGLE = 45; // degrees - public static final double TICKS_PER_DEGREE = 4096 / 360; - public static final double kP = 0; + public static final double MAX_ANGLE = 180; // degrees + public static final double MIN_ANGLE = 0; // degrees + public static final double TICKS_PER_DEGREE = 4096.0 / 360.0; + public static final double kP = 0.2; public static final double kI = 0; - public static final double kD = 0; + public static final double kD = 0.2; } } diff --git a/src/main/java/frc/robot/Ports.java b/src/main/java/frc/robot/Ports.java index 2ce0f9c..ffafba3 100644 --- a/src/main/java/frc/robot/Ports.java +++ b/src/main/java/frc/robot/Ports.java @@ -5,6 +5,6 @@ public final class Ports { public static class Turret{ public final static boolean INVERTED = false; public final static boolean SENSOR_PHASE = false; - public final static int MOTOR = 0; + public final static int MOTOR = 22; } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index dbf7bc6..9fdc72a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,6 +17,7 @@ import frc.robot.subsystems.ExampleSubsystem.ExampleSubsystem; import frc.robot.subsystems.turret.*; import frc.robot.subsystems.turret.commands.Gunner; +import frc.robot.subsystems.turret.commands.Manual; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -33,7 +34,8 @@ public class RobotContainer { * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { - gunnerMan.setDefaultCommand(new Gunner(gunnerMan)); +// gunnerMan.setDefaultCommand(new Gunner(gunnerMan)); + gunnerMan.setDefaultCommand(new Manual(gunnerMan)); // Configure the button bindings configureButtonBindings(); } diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index ae8db12..110dbbd 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -16,7 +16,7 @@ public class Turret extends SubsystemBase { private TalonSRX motor = new TalonSRX(MOTOR); // The motor used to spin the turret. //Constructor. - public Turret(){ + public Turret() { motor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative); motor.config_kP(0, kP); @@ -30,19 +30,17 @@ public Turret(){ /** * This function sets the angle the turret needs to be at. [degrees] + * * @param targetAngle is the angle the turret needs to be at. [degrees] */ public void setTargetAngle(double targetAngle) { - if(targetAngle > MAX_ANGLE) - this.targetAngle = MAX_ANGLE; - else if(targetAngle < MIN_ANGLE) - this.targetAngle = MIN_ANGLE; - else - this.targetAngle = targetAngle; + targetAngle = targetAngle < 0 ? targetAngle + 360 : targetAngle; + this.targetAngle = (targetAngle > 180 && targetAngle < 270) ? MAX_ANGLE : ((targetAngle > 270 && targetAngle < 360) ? MIN_ANGLE : targetAngle); } /** * Gets the current angle of the turret. [degrees] + * * @return the current angle. [degrees] */ public double getCurrAngle() { @@ -51,6 +49,7 @@ public double getCurrAngle() { /** * Gets the target angle of the turret. [degrees] + * * @return the target angle. [degrees] */ public double getTargetAngle() { @@ -59,25 +58,35 @@ public double getTargetAngle() { /** * Sets the current angle of the turret. [degrees] + * * @param currAngle is the current angle of the turret. [degrees] */ - public void setCurrAngle(double currAngle){ + public void setCurrAngle(double currAngle) { this.currAngle = currAngle; } /** * Changes the position of the motor, hence changing the position of the turret. */ - public void setPosition(){ + public void setPosition() { motor.set(ControlMode.Position, unitMan.toTicks(targetAngle - currAngle)); } + public void setPower(double power) { + motor.set(ControlMode.PercentOutput, power); + } + + public int getTicks() { + return motor.getSelectedSensorPosition(); + } + /** * Checks whether or not the target angle and the current angle are equal in a certain error range. + * * @return whether the angles are equal. */ - public boolean anglesEqual(){ - if(currAngle <= targetAngle + ERROR_RANGE || currAngle >= targetAngle - ERROR_RANGE){ + public boolean anglesEqual() { + if (currAngle <= targetAngle + ERROR_RANGE || currAngle >= targetAngle - ERROR_RANGE) { return true; } return false; @@ -86,7 +95,7 @@ public boolean anglesEqual(){ /** * Sets the turret back to 0. */ - public void terminate(){ + public void terminate() { targetAngle = 0; setPosition(); } diff --git a/src/main/java/frc/robot/subsystems/turret/commands/Gunner.java b/src/main/java/frc/robot/subsystems/turret/commands/Gunner.java index f2ce9ff..73465a5 100644 --- a/src/main/java/frc/robot/subsystems/turret/commands/Gunner.java +++ b/src/main/java/frc/robot/subsystems/turret/commands/Gunner.java @@ -23,8 +23,7 @@ public Gunner(Turret gunnerMan){ public void execute() { X = xbox.getX(GenericHID.Hand.kRight); Y = xbox.getY(GenericHID.Hand.kRight); - targetAngle = Math.atan2(Y, X); - + targetAngle = Math.toDegrees(Math.atan2(Y, X)); gunnerMan.setTargetAngle(targetAngle); diff --git a/src/main/java/frc/robot/subsystems/turret/commands/Manual.java b/src/main/java/frc/robot/subsystems/turret/commands/Manual.java new file mode 100644 index 0000000..d99e746 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/turret/commands/Manual.java @@ -0,0 +1,43 @@ +package frc.robot.subsystems.turret.commands; + +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.Ports; +import frc.robot.RobotContainer; +import frc.robot.subsystems.turret.Turret; + +public class Manual extends CommandBase { + private final Turret turret; + private final XboxController xbox = new XboxController(1); + + + public Manual(Turret turret) { + this.turret = turret; + addRequirements(turret); + } + + @Override + public void initialize() { + + } + + @Override + public void execute() { + double x = xbox.getX(GenericHID.Hand.kRight); + if (Math.abs(x) < 0.01) x = 0; + x = x / 2; + turret.setPower(x); + System.out.println(turret.getTicks()); + } + + @Override + public void end(boolean interrupted) { + turret.setPower(0); + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/test/java/TestClass.java b/src/test/java/TestClass.java new file mode 100644 index 0000000..358f4b7 --- /dev/null +++ b/src/test/java/TestClass.java @@ -0,0 +1,19 @@ +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import org.junit.Test; +import frc.robot.subsystems.turret.*; + +import static frc.robot.Constants.Turret.MAX_ANGLE; +import static frc.robot.Constants.Turret.MIN_ANGLE; + +public class TestClass { + @Test + public void testFunction2() { + + double targetAngle = 195; + targetAngle = targetAngle < 0 ? targetAngle + 360 : targetAngle; + targetAngle = (targetAngle > 180 && targetAngle < 270) ? MAX_ANGLE : ((targetAngle > 270 && targetAngle < 360) ? MIN_ANGLE : targetAngle); + System.out.println("The corrected angle is: " + targetAngle); + } +} From 801aa01c80046dbad8126e77f406a91fafd76b57 Mon Sep 17 00:00:00 2001 From: Saar Date: Tue, 21 Sep 2021 15:41:24 +0300 Subject: [PATCH 8/8] Turret is working now :) --- src/main/java/frc/robot/Constants.java | 15 +++++++---- src/main/java/frc/robot/Ports.java | 4 +-- src/main/java/frc/robot/RobotContainer.java | 4 +-- .../frc/robot/subsystems/turret/Turret.java | 26 +++++++++++++++++-- .../subsystems/turret/commands/Gunner.java | 14 ++++++---- .../subsystems/turret/commands/Manual.java | 9 +++++-- src/test/java/TestClass.java | 10 +++---- 7 files changed, 58 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ac4bfac..dfdd58b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -17,13 +17,18 @@ */ public final class Constants { - public static class Turret{ + public static class Turret { public static final double ERROR_RANGE = 2; // degrees - public static final double MAX_ANGLE = 180; // degrees - public static final double MIN_ANGLE = 0; // degrees + public static final double MAX_TICKS = 2048; // degrees + public static final double MIN_TICKS = 0; // degrees public static final double TICKS_PER_DEGREE = 4096.0 / 360.0; - public static final double kP = 0.2; + public static final double kP = 0.4; + // public static final double kP = 3.5; public static final double kI = 0; - public static final double kD = 0.2; + // public static final double kI = 0.01; + public static final double kD = 0.04; +// public static final double kD = 150; + public static final double DEADBAND = 0.1 + ; } } diff --git a/src/main/java/frc/robot/Ports.java b/src/main/java/frc/robot/Ports.java index ffafba3..20d880c 100644 --- a/src/main/java/frc/robot/Ports.java +++ b/src/main/java/frc/robot/Ports.java @@ -2,9 +2,9 @@ public final class Ports { - public static class Turret{ + public static class Turret { public final static boolean INVERTED = false; - public final static boolean SENSOR_PHASE = false; + public final static boolean SENSOR_PHASE = true; public final static int MOTOR = 22; } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9fdc72a..8d3eae2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -34,8 +34,8 @@ public class RobotContainer { * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { -// gunnerMan.setDefaultCommand(new Gunner(gunnerMan)); - gunnerMan.setDefaultCommand(new Manual(gunnerMan)); +// gunnerMan.setDefaultCommand(new Manual(gunnerMan)); + gunnerMan.setDefaultCommand(new Gunner(gunnerMan)); // Configure the button bindings configureButtonBindings(); } diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 110dbbd..aa6b6ef 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -2,7 +2,10 @@ import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; +import com.ctre.phoenix.motorcontrol.LimitSwitchSource; import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.subsystems.UnitModel; @@ -17,7 +20,7 @@ public class Turret extends SubsystemBase { //Constructor. public Turret() { - motor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative); + motor.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); motor.config_kP(0, kP); motor.config_kI(0, kI); @@ -26,6 +29,13 @@ public Turret() { motor.setInverted(INVERTED); motor.setSensorPhase(SENSOR_PHASE); + + motor.configReverseLimitSwitchSource(LimitSwitchSource.Deactivated, LimitSwitchNormal.Disabled); + motor.configForwardLimitSwitchSource(LimitSwitchSource.Deactivated, LimitSwitchNormal.Disabled); + } + + private double map(double value, double minimumInput, double maximumInput, double minimumOutput, double maximumOutput) { + return (value - minimumInput) * (maximumOutput - minimumOutput) / (maximumInput - minimumInput) + minimumOutput; } /** @@ -34,8 +44,12 @@ public Turret() { * @param targetAngle is the angle the turret needs to be at. [degrees] */ public void setTargetAngle(double targetAngle) { +// targetAngle = targetAngle < 0 ? targetAngle + 360 : targetAngle; +// this.targetAngle = (targetAngle > 180 && targetAngle < 270) ? MAX_ANGLE : ((targetAngle > 270 && targetAngle < 360) ? MIN_ANGLE : targetAngle); targetAngle = targetAngle < 0 ? targetAngle + 360 : targetAngle; - this.targetAngle = (targetAngle > 180 && targetAngle < 270) ? MAX_ANGLE : ((targetAngle > 270 && targetAngle < 360) ? MIN_ANGLE : targetAngle); + targetAngle = targetAngle > 180 ? targetAngle < 270 ? 180 : 0 : targetAngle; + int ticks = (int) map(targetAngle, 0, 180, MIN_TICKS, MAX_TICKS); + setPosition(ticks); } /** @@ -72,10 +86,18 @@ public void setPosition() { motor.set(ControlMode.Position, unitMan.toTicks(targetAngle - currAngle)); } + public void setPosition(int position) { + motor.set(ControlMode.Position, position); + } + public void setPower(double power) { motor.set(ControlMode.PercentOutput, power); } + public void setVelocity(double velocity) { + motor.set(ControlMode.Velocity, velocity); + } + public int getTicks() { return motor.getSelectedSensorPosition(); } diff --git a/src/main/java/frc/robot/subsystems/turret/commands/Gunner.java b/src/main/java/frc/robot/subsystems/turret/commands/Gunner.java index 73465a5..a915911 100644 --- a/src/main/java/frc/robot/subsystems/turret/commands/Gunner.java +++ b/src/main/java/frc/robot/subsystems/turret/commands/Gunner.java @@ -5,6 +5,8 @@ import edu.wpi.first.wpilibj2.command.button.*; import frc.robot.subsystems.turret.Turret; +import static frc.robot.Constants.Turret.*; + public class Gunner extends CommandBase { private double X; private double Y; @@ -13,7 +15,7 @@ public class Gunner extends CommandBase { private final XboxController xbox = new XboxController(1); Trigger LT = new Trigger(() -> xbox.getRawAxis(XboxController.Axis.kLeftTrigger.value) > 0.3); - public Gunner(Turret gunnerMan){ + public Gunner(Turret gunnerMan) { this.gunnerMan = gunnerMan; addRequirements(gunnerMan); } @@ -22,13 +24,15 @@ public Gunner(Turret gunnerMan){ @Override public void execute() { X = xbox.getX(GenericHID.Hand.kRight); - Y = xbox.getY(GenericHID.Hand.kRight); + Y = -xbox.getY(GenericHID.Hand.kRight); + if(Math.abs(X) < DEADBAND) + X = 0; + if(Math.abs(Y) < DEADBAND) + Y = 0; targetAngle = Math.toDegrees(Math.atan2(Y, X)); - gunnerMan.setTargetAngle(targetAngle); - if(LT.get()) - gunnerMan.setPosition(); +// if(LT.get()) } // Resets the turret. diff --git a/src/main/java/frc/robot/subsystems/turret/commands/Manual.java b/src/main/java/frc/robot/subsystems/turret/commands/Manual.java index d99e746..d94b4ca 100644 --- a/src/main/java/frc/robot/subsystems/turret/commands/Manual.java +++ b/src/main/java/frc/robot/subsystems/turret/commands/Manual.java @@ -5,6 +5,7 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.Ports; import frc.robot.RobotContainer; +import frc.robot.subsystems.UnitModel; import frc.robot.subsystems.turret.Turret; public class Manual extends CommandBase { @@ -26,9 +27,13 @@ public void initialize() { public void execute() { double x = xbox.getX(GenericHID.Hand.kRight); if (Math.abs(x) < 0.01) x = 0; - x = x / 2; - turret.setPower(x); + x /= 4; System.out.println(turret.getTicks()); + System.out.println(x); +// turret.setPower(x); +// turret.setVelocity(x * 300); +// turret.setPosition(1600); + turret.setPosition(0); } @Override diff --git a/src/test/java/TestClass.java b/src/test/java/TestClass.java index 358f4b7..ff9a6b7 100644 --- a/src/test/java/TestClass.java +++ b/src/test/java/TestClass.java @@ -4,16 +4,14 @@ import org.junit.Test; import frc.robot.subsystems.turret.*; -import static frc.robot.Constants.Turret.MAX_ANGLE; -import static frc.robot.Constants.Turret.MIN_ANGLE; public class TestClass { @Test public void testFunction2() { - double targetAngle = 195; - targetAngle = targetAngle < 0 ? targetAngle + 360 : targetAngle; - targetAngle = (targetAngle > 180 && targetAngle < 270) ? MAX_ANGLE : ((targetAngle > 270 && targetAngle < 360) ? MIN_ANGLE : targetAngle); - System.out.println("The corrected angle is: " + targetAngle); +// double targetAngle = 195; +// targetAngle = targetAngle < 0 ? targetAngle + 360 : targetAngle; +// targetAngle = (targetAngle > 180 && targetAngle < 270) ? MAX_ANGLE : ((targetAngle > 270 && targetAngle < 360) ? MIN_ANGLE : targetAngle); +// System.out.println("The corrected angle is: " + targetAngle); } }