Skip to content

Commit 44e832b

Browse files
committed
testing
1 parent 996cd1e commit 44e832b

File tree

8 files changed

+154
-80
lines changed

8 files changed

+154
-80
lines changed

src/main/java/frc/robot/Modules/AimFire.java

Lines changed: 35 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,8 @@
11
package frc.robot.Modules;
22

33
import frc.robot.Robot;
4+
import frc.robot.Modules.RobotInformation.FieldData.ValidTargets;
5+
46
import com.ctre.phoenix.motorcontrol.ControlMode;
57

68
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -22,32 +24,54 @@ public static void shooter() {
2224
*
2325
* @param target The name of the target to aim at. ("top_hub","ball")
2426
*/
25-
public static void centerAim(String target) {
27+
public static void centerAim(ValidTargets target) {
2628
//https://i.kym-cdn.com/entries/icons/original/000/039/393/cover2.jpg
27-
switch(target.toLowerCase()) {
28-
case "top_hub": // TODO: check PID on Triclops
29+
30+
switch(target) {
31+
case top_hub:
2932
Limelight.setPipeline(0);
3033
Limelight.getLimelightData();
3134
if(Math.abs(Limelight.nonZeroLimelightHorAng)>RobotInformation.deadbandAngle) { //Is the number within the deadband range?
3235
if(Limelight.nonZeroLimelightHorAng>0) { //Positive
3336
// Turn right
3437
double motorSpeed = (Math.abs(Limelight.nonZeroLimelightHorAng * .9)/59.6)+.05;
3538
motorSpeed = Math.round(motorSpeed * 100) / 100.0;
36-
DriveCode.oldDriveTrain(motorSpeed, motorSpeed);
39+
MotorControl.DriveCode.oldDriveTrain(motorSpeed, -motorSpeed);
3740
}
3841
if(Limelight.nonZeroLimelightHorAng<0) { //Negetive
3942
// Turn left
4043
double motorSpeed = (Math.abs(Limelight.nonZeroLimelightHorAng * .9)/59.6)+.05;
4144
motorSpeed = Math.round(motorSpeed * 100) / 100.0;
42-
DriveCode.oldDriveTrain(-motorSpeed, -motorSpeed);
45+
MotorControl.DriveCode.oldDriveTrain(-motorSpeed, motorSpeed);
46+
}
47+
}
48+
// else {
49+
// DriveCode.oldDriveTrain(0, 0);
50+
// }
51+
break;
52+
case lower_hub:
53+
Limelight.setPipeline(0);
54+
Limelight.getLimelightData();
55+
if(Math.abs(Limelight.nonZeroLimelightHorAng)>RobotInformation.deadbandAngle) { //Is the number within the deadband range?
56+
if(Limelight.nonZeroLimelightHorAng>0) { //Positive
57+
// Turn right
58+
double motorSpeed = (Math.abs(Limelight.nonZeroLimelightHorAng * .9)/59.6)+.05;
59+
motorSpeed = Math.round(motorSpeed * 100) / 100.0;
60+
MotorControl.DriveCode.oldDriveTrain(motorSpeed, -motorSpeed);
61+
}
62+
if(Limelight.nonZeroLimelightHorAng<0) { //Negetive
63+
// Turn left
64+
double motorSpeed = (Math.abs(Limelight.nonZeroLimelightHorAng * .9)/59.6)+.05;
65+
motorSpeed = Math.round(motorSpeed * 100) / 100.0;
66+
MotorControl.DriveCode.oldDriveTrain(-motorSpeed, motorSpeed);
4367
}
4468
}
4569
// else {
4670
// DriveCode.oldDriveTrain(0, 0);
4771
// }
4872
break;
4973

50-
case "ball":
74+
case ball:
5175

5276
break;
5377
}
@@ -57,8 +81,12 @@ public static void centerAim(String target) {
5781
/**
5882
* One click fire function
5983
*/
60-
public static void fire() { //TODO: One click fire
84+
public static void fire(ValidTargets target) { //TODO: One click fire
85+
if(Math.abs(Limelight.nonZeroLimelightHorAng)>RobotInformation.deadbandAngle) {
86+
centerAim(target);
87+
} else if(Math.abs(Limelight.nonZeroLimelightHorAng)<RobotInformation.deadbandAngle) {
6188

89+
}
6290
}
6391

6492
}

src/main/java/frc/robot/Modules/DriveCode.java

Lines changed: 0 additions & 28 deletions
This file was deleted.
Lines changed: 19 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,24 @@
11
package frc.robot.Modules;
22

3+
import frc.robot.Modules.Limelight.Limelight_Light_States;
4+
35
public class FunStuff {
4-
public static void doThings() {
5-
Limelight.setLEDS("blink");
6+
public static enum Music {
7+
MrKrabs,
8+
9+
}
10+
11+
public static void RAVE_MODE() {
12+
Limelight.setLEDS(Limelight_Light_States.blink);
13+
MotorControl.DriveCode.oldDriveTrain(0.6, -0.6);
14+
}
15+
16+
public static void playMusic(Music sound) {
17+
switch(sound) {
18+
case MrKrabs:
19+
// Play MrKrabs Music
20+
break;
21+
22+
}
623
}
724
}

src/main/java/frc/robot/Modules/Intake.java

Lines changed: 19 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -20,9 +20,7 @@ public static void rollers() { // Had absolute value of getRightY, would never b
2020
}
2121
}
2222

23-
/** The stupid thing that looks cool but doesnt really work // edit: it works apparently */
24-
25-
/**
23+
/** The stupid thing that looks cool but doesnt really work // edit: it works apparently
2624
* This function is called when the left joystick is moved in the Y direction.
2725
* If the joystick is moved up, the wrist is set to the output of the joystick multiplied by the
2826
* transfer percentage.
@@ -36,4 +34,22 @@ public static void wrist() {
3634
}
3735
}
3836

37+
public static Boolean wristInside() {
38+
double wrist_position = MotorControl.getCurrentPosition(Robot.wrist);
39+
if(0 < wrist_position && wrist_position < 1) { //TODO: find wrist sensor positions that constitute the wrist is inside
40+
return true;
41+
} else {
42+
return false;
43+
}
44+
}
45+
46+
public static Boolean wristDown() {
47+
double wrist_position = MotorControl.getCurrentPosition(Robot.wrist);
48+
if(0 < wrist_position && wrist_position < 1) { //TODO: find wrist sensor positions that constitute the wrist is down
49+
return true;
50+
} else {
51+
return false;
52+
}
53+
}
54+
3955
}

src/main/java/frc/robot/Modules/Limelight.java

Lines changed: 15 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,12 @@ public class Limelight {
88
/** The last angle seen by the limelight that wasn't zero */
99
public static double nonZeroLimelightHorAng; // Used to orient bot
1010

11+
public static enum Limelight_Light_States {
12+
on,
13+
off,
14+
blink
15+
}
16+
1117
/**
1218
* This function is used to update the SmartDashboard with the current values of the Limelight
1319
*/
@@ -81,7 +87,7 @@ public static boolean hubPresent() {
8187
* Initialises the Limelight for Top Hub tracking: Sets the LEDS to on, sets the pipeline to 0, and sets the Limelight as a Vision Processor
8288
*/
8389
public static void init() {
84-
setLEDS("on");
90+
setLEDS(Limelight_Light_States.on);
8591
NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").setNumber(0); //Sets the pipeline to 0
8692
NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0); //Sets the Limelight as a Vision Proccesor
8793
}
@@ -99,26 +105,26 @@ public static void setPipeline(double pipeline) {
99105
* Code run for limelight when robot is in a disabled state
100106
*/
101107
public static void disabled() {
102-
setLEDS("off"); // Turns off the god damm limelight cause im going to go blind and gouge my eyes out because wtf does it need to be so bright like holy hell
108+
setLEDS(Limelight_Light_States.off); // Turns off the god damm limelight cause im going to go blind and gouge my eyes out because wtf does it need to be so bright like holy hell
103109
}
104110

105111
/**
106112
* This function sets the LED mode of the limelight
107113
*
108114
* @param mode The LED mode. ("off", "blink", "on")
109115
*/
110-
public static void setLEDS(String mode) {
116+
public static void setLEDS(Limelight_Light_States mode) {
111117
switch (mode) {
112-
case "off":
113-
NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(1); // light off
118+
case on:
119+
NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3); // light on
114120
break;
115121

116-
case "blink":
117-
NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(2); //light blinking
122+
case off:
123+
NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(1); //light off
118124
break;
119125

120-
case "on":
121-
NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3); // light on
126+
case blink:
127+
NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(2); //light blinking
122128
break;
123129

124130
default:

src/main/java/frc/robot/Modules/MotorControl.java

Lines changed: 42 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -16,23 +16,23 @@
1616

1717

1818
public class MotorControl {
19-
/**
20-
* Given the ideal velocity and the transfer percentage, return the RPM
21-
*
22-
* @param idealVelocity the velocity you want to achieve in meters per second
23-
* @param transferPercent the percentage of the ideal velocity that is transferred to the wheel.
24-
* @return The RPM of the motor.
25-
*/
19+
/**
20+
* Given the ideal velocity and the transfer percentage, return the RPM
21+
*
22+
* @param idealVelocity the velocity you want to achieve in meters per second
23+
* @param transferPercent the percentage of the ideal velocity that is transferred to the wheel.
24+
* @return The RPM of the motor.
25+
*/
2626
public static double getRPM(double idealVelocity, double transferPercent) {
2727
return (((idealVelocity*(1/transferPercent))/(Math.PI*0.1016))*60); // rudimentary calculation that's 90% wrong
2828
}
2929

30-
/**
31-
* This function returns the current velocity of the motor. The motor must be a Falcon500
32-
*
33-
* @param motor The motor you want to get the velocity of.
34-
* @return The integrated sensor velocity of the motor.
35-
*/
30+
/**
31+
* This function returns the current velocity of the motor. The motor must be a Falcon500
32+
*
33+
* @param motor The motor you want to get the velocity of.
34+
* @return The integrated sensor velocity of the motor.
35+
*/
3636
public static double getCurrentVelocity(WPI_TalonFX motor) { // If using a TalonFX motor only
3737
return (motor.getSensorCollection().getIntegratedSensorVelocity());
3838
}
@@ -44,7 +44,7 @@ public static double getCurrentVelocity(WPI_TalonFX motor) { // If using a Talon
4444
* @return The current position of the motor.
4545
*/
4646
public static double getCurrentPosition(WPI_TalonFX motor) { // If using a TalonFX motor only
47-
return (motor.getSensorCollection().getIntegratedSensorPosition());
47+
return (motor.getSensorCollection().getIntegratedSensorAbsolutePosition());
4848
}
4949

5050
/**
@@ -71,6 +71,8 @@ public static void motor_init() {
7171
Robot.leftSlave.configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, 40, 40, .5));
7272
Robot.leftSlave.configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, 40, 40, .5));
7373

74+
Robot.rightMaster.setInverted(true);
75+
7476
// Initialize Climber's and their Motor Brakes
7577
Robot.climbRotation.configStatorCurrentLimit(new StatorCurrentLimitConfiguration(true, 80, 80, .5));
7678
Robot.climbExtension.configStatorCurrentLimit(new StatorCurrentLimitConfiguration(true, 80, 80, .5));
@@ -107,7 +109,7 @@ public static void setRPM(MotorPositions position, int RPM) {
107109
WPI_TalonFX flywheel_motor = RobotInformation.RobotData.MotorData.Shooter.Flywheel.motor;
108110
double current_velocity = getCurrentVelocity(flywheel_motor);
109111
double current_RPM = (60 * current_velocity) / (2 * Math.PI);
110-
112+
111113
break;
112114
case Rollers:
113115
TalonSRX roller_motor = RobotInformation.RobotData.MotorData.Intake.Rollers.motor;
@@ -124,4 +126,29 @@ public static void setRPM(MotorPositions position, int RPM) {
124126
break;
125127
}
126128
}
129+
130+
public static class DriveCode {
131+
/** Main Drive Call */
132+
public static void tankDrive() {
133+
if ((Math.abs(Robot.rightJoy.getY()) > 0.2) || Math.abs(Robot.leftJoy.getY()) > 0.2) {
134+
Robot.drive.tankDrive((-Robot.rightJoy.getY() * RobotInformation.DriveTeamInfo.driverPercentage), (Robot.leftJoy.getY() * RobotInformation.DriveTeamInfo.driverPercentage));// TODO: adjust deadzones
135+
}
136+
}
137+
138+
/** Move the Robot Backwards at -0.1 speed (should be a PID loop imo) */
139+
public static void moveBack() {
140+
Robot.drive.tankDrive(-0.1, -0.1);
141+
}
142+
143+
/**
144+
* Drive the robot using the left and right master motors at the given motor speeds.*
145+
*
146+
* @param motorSpeedLeft The speed that you want the left motor to go.
147+
* @param motorSpeedRight The speed that you want the right side to go.
148+
*/
149+
public static void oldDriveTrain(double motorSpeedLeft, double motorSpeedRight) {
150+
Robot.leftMaster.set(ControlMode.PercentOutput, motorSpeedLeft);
151+
Robot.rightMaster.set(ControlMode.PercentOutput, -motorSpeedRight);
152+
}
153+
}
127154
}

src/main/java/frc/robot/Modules/RobotInformation.java

Lines changed: 7 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,7 @@ public class RobotInformation {
99
// General Information
1010

1111
/** Minimum Angle that wont trigger the robot moving */
12-
public static final double deadbandAngle = 0.5;
12+
public static final double deadbandAngle = 0.3; // Last Known Working = 0.5
1313
/** Our Alliance Color */
1414
public static String allianceColor = "red"; // not final because it is not final
1515

@@ -81,6 +81,12 @@ public static class FieldData {
8181
public static final double tarmacWidthInches = 153;
8282
/** Tarmac width in meters */
8383
public static final double tarmacWidthMeters = (tarmacWidthInches/39.37);
84+
85+
public static enum ValidTargets {
86+
top_hub,
87+
lower_hub,
88+
ball
89+
}
8490
}
8591

8692
/**

0 commit comments

Comments
 (0)