Skip to content

Commit 7ab2902

Browse files
srimanachantasrimanachanta
committed
push
1 parent 9a4ed19 commit 7ab2902

File tree

2 files changed

+123
-101
lines changed

2 files changed

+123
-101
lines changed

‎gradlew

100644100755
File mode changed.

‎src/main/java/frc/robot/Robot.java

+123-101
Original file line numberDiff line numberDiff line change
@@ -36,30 +36,32 @@ public class Robot extends TimedRobot {
3636
private boolean stage1; // auto staging
3737

3838
// CONSTANTS
39-
private static final double WHEEL_DIAMETER = 6; // inches
40-
//NEED TO FIGURE OUT ENCODER MODEL- changes CPR for drive
41-
private static final double cpr = 360; //if am-3132
42-
//private static final double cpr = 7/4; //if am-2861a
43-
// private static final double cpr = 5; //if am-3314a
44-
// private static final double cpr = 1024; //if am-3445
45-
// private static final double cpr = 64; //if am-4027
46-
private static final double drive_dpp = (Math.PI*WHEEL_DIAMETER/cpr); // Gives in inches per rev FOR DRIVETRAIN WHEELS ONLY
39+
// private static final double WHEEL_DIAMETER = 6; // inches
40+
// //NEED TO FIGURE OUT ENCODER MODEL- changes CPR for drive
41+
// private static final double cpr = 360; //if am-3132
42+
// //private static final double cpr = 7/4; //if am-2861a
43+
// // private static final double cpr = 5; //if am-3314a
44+
// // private static final double cpr = 1024; //if am-3445
45+
// // private static final double cpr = 64; //if am-4027
46+
// private static final double drive_dpp = (Math.PI*WHEEL_DIAMETER/cpr); // Gives in inches per rev FOR DRIVETRAIN WHEELS ONLY
4747

4848
// TODO: Check what motors should be inverted (.setInverted(false))
4949
// MOTOR VARIABLES
50-
private WPI_TalonFX leftSlave, rightSlave, leftMaster, rightMaster, climbRotation, climbExtension, shooterFly, wrist; //Falcon 500s
51-
private TalonSRX rollers, hood; // 775s or BAG motors
50+
private WPI_TalonFX leftSlave, rightSlave, leftMaster, rightMaster, climbExtension, shooterFly, wrist; //Falcon 500s
51+
//private WPI_TalonFX climbRotation
52+
private TalonSRX rollers
53+
//private TalonSRX hood; // 775s or BAG motors
5254

5355
//CONTROLLERS
5456
private Joystick leftJoy, rightJoy; //Used for tank drive
5557
private XboxController controller; // Used for button man mechanism controls
5658

5759
// SENSORS
58-
private AHRS gyro; //9-axis-> used mainly to orient shooter hood using roll
60+
//private AHRS gyro; //9-axis-> used mainly to orient shooter hood using roll
5961

60-
private Encoder rightEncoder, leftEncoder; // Drivetrain encoders (might just use integrated Falcon stuff who knows)
61-
private DigitalInput lowerShooterLimit, upperShooterLimit; // Limit switch to reset hood to its default position
62-
private DigitalInput intakeLimit; // Limit switch to know if intake is ready to kickup
62+
// private Encoder rightEncoder, leftEncoder; // Drivetrain encoders (might just use integrated Falcon stuff who knows)
63+
// private DigitalInput lowerShooterLimit, upperShooterLimit; // Limit switch to reset hood to its default position
64+
// private DigitalInput intakeLimit; // Limit switch to know if intake is ready to kickup
6365

6466
// MISCELLANEOUS
6567
private double nonZeroLimelightHorAng; // Used to orient bot
@@ -86,16 +88,16 @@ public void robotInit() {
8688
SmartDashboard.putData("Auto choices", m_chooser);
8789

8890
// SENSOR INITIALIZATIONS
89-
gyro = new AHRS(SerialPort.Port.kMXP); //SUBJECT TO CHANGE FROM ELECTRICAL COULD BE SOURCE OF ERROR
91+
//gyro = new AHRS(SerialPort.Port.kMXP); //SUBJECT TO CHANGE FROM ELECTRICAL COULD BE SOURCE OF ERROR
9092
// ALL Ports subject to change from Electrical
91-
leftEncoder = new Encoder(0, 1);
92-
leftEncoder.setDistancePerPulse(drive_dpp);
93-
rightEncoder = new Encoder(2, 3);
94-
rightEncoder.setDistancePerPulse(drive_dpp);
95-
// Limit switches- CHANGE PORTS BASED ON ELECTRICAL
96-
lowerShooterLimit = new DigitalInput(4);
97-
upperShooterLimit = new DigitalInput(5);
98-
intakeLimit = new DigitalInput(6);
93+
//leftEncoder = new Encoder(0, 1);
94+
// leftEncoder.setDistancePerPulse(drive_dpp);
95+
// rightEncoder = new Encoder(2, 3);
96+
// rightEncoder.setDistancePerPulse(drive_dpp);
97+
// // Limit switches- CHANGE PORTS BASED ON ELECTRICAL
98+
// lowerShooterLimit = new DigitalInput(4);
99+
// upperShooterLimit = new DigitalInput(5);
100+
// intakeLimit = new DigitalInput(6);
99101

100102
// MOTORS
101103
//Drivetrain motors and configuration
@@ -106,19 +108,19 @@ public void robotInit() {
106108
leftSlave = new WPI_TalonFX(RobotMap.DRIVETRAIN_BACKLEFT);
107109
leftSlave.follow(leftMaster);
108110
// Set integrated sensor position to 0 for encoder use
109-
leftMaster.getSensorCollection().setIntegratedSensorPosition(0, 10);
110-
rightMaster.getSensorCollection().setIntegratedSensorPosition(0, 10);
111+
// leftMaster.getSensorCollection().setIntegratedSensorPosition(0, 10);
112+
// rightMaster.getSensorCollection().setIntegratedSensorPosition(0, 10);
111113

112114
//Climb Motors
113-
climbRotation = new WPI_TalonFX(RobotMap.CLIMBROTATION);
115+
//climbRotation = new WPI_TalonFX(RobotMap.CLIMBROTATION);
114116
climbExtension = new WPI_TalonFX(RobotMap.CLIMBEXTENSION);
115117

116118
//Intake Motors
117119
wrist = new WPI_TalonFX(RobotMap.INTAKE_WRIST);
118120
rollers = new TalonSRX(RobotMap.INTAKE_ROLLERS);
119121

120122
//Hood Motor
121-
hood = new TalonSRX(RobotMap.SHOOTER_HOOD);
123+
//hood = new TalonSRX(RobotMap.SHOOTER_HOOD);
122124

123125
//Shooter Motors
124126
shooterFly = new WPI_TalonFX(RobotMap.SHOOTER_FLY);
@@ -130,18 +132,18 @@ public void robotInit() {
130132
shooterFly.setInverted(false);
131133
shooterFly.setSensorPhase(false);
132134
// config P,I,D,F values- start by doubling F, then P, then D, then I (middle values) then increase/decrease over time
133-
shooterFly.config_kF(0, 0.007, 1); // (F) Feed Forward Term
134-
shooterFly.config_kP(0, 0.8192, 1); // (P) Proportional Term
135-
shooterFly.config_kI(0, 0.0008, 1); // (I) Integral term
136-
shooterFly.config_kD(0, 0.0256, 1); // (D) Differentiable Term
135+
// shooterFly.config_kF(0, 0.007, 1); // (F) Feed Forward Term
136+
// shooterFly.config_kP(0, 0.8192, 1); // (P) Proportional Term
137+
// shooterFly.config_kI(0, 0.0008, 1); // (I) Integral term
138+
// shooterFly.config_kD(0, 0.0256, 1); // (D) Differentiable Term
137139

138140
// Used for tank and arcade drive respectively
139141
drive = new DifferentialDrive(leftMaster, rightMaster);
140142

141143
// TODO- make this choose from sendable chooser to set alliance color
142144
NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").setNumber(0); //Sets the pipeline to 0
143145
NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0); //Sets the Limelight as a Vision Proccesor
144-
NetworkTableInstance.getDefault().getTable("TalonPi").getEntry("alliance").setString("red");
146+
//NetworkTableInstance.getDefault().getTable("TalonPi").getEntry("alliance").setString("red");
145147
}
146148

147149
/** This function is called once when test mode is enabled. */
@@ -216,35 +218,52 @@ public void autonomousPeriodic() {
216218
SmartDashboard.putNumber("Current Angle", gyro.getRoll());
217219
}
218220

219-
// Resets hood to position 0 and uses that as the 0-angle.
220-
if (!lowerShooterLimit.get()) {
221-
hood.set(ControlMode.PercentOutput, -0.1);
222-
}
223-
gyro.reset(); //sets gyro value to 0
221+
if (shooterCalculations[0][0] < 2.3) { // Counter
222+
drive.tankDrive(-0.1, -0.1);
223+
ready = false;
224+
} else {
225+
drive.tankDrive(0,0);
226+
ready = true;
227+
// Spin flywheel to speed
228+
// Intake ball using wrist
229+
// PRAY()
224230

225-
if ((Math.abs(shooterCalculations[0][0]) < 1.5)) {
226-
drive.tankDrive(-0.8, -0.8); // backs up until 1.5 meters away
227-
}
228-
if ((Math.abs(shooterCalculations[0][0]) >= 1.5) && (Math.abs(shooterCalculations[1][0]) > 1)) { // TODO: adjust to reduce bouncing
229-
centerAim("top_hub"); // fires a shot into the hub
230-
}
231-
if (Math.abs(shooterCalculations[1][0]) < 1 && (Math.abs(shooterCalculations[0][0]) >= 1.5) && stage1) {
232-
if (counter < 1000) { // TODO: adjust based on how long it takes to shoot a ball
233-
fire(); // shoots ball-> changes stage1 to false after having shot a ball
234-
counter++;
235-
}
236-
else if (counter >= 1000) {
237-
stage1 = false;
238-
}
239231
}
240-
if (!stage1 && !ballSeen()) { // Locates ball if it can't see after shooting one
241-
centerAim("ball_tracking");
242-
rollers.set(ControlMode.PercentOutput, 0);
243-
}
244-
if (!stage1 && ballSeen()) { // Drives forward if it sees the ball
245-
drive.tankDrive(0.8, 0.8);
246-
rollers.set(ControlMode.PercentOutput, 1);
232+
233+
if(ready) {
234+
// Push ball into flywheel
235+
// flywheel should be running at speed
247236
}
237+
238+
// // Resets hood to position 0 and uses that as the 0-angle.
239+
// // if (!lowerShooterLimit.get()) {
240+
// // hood.set(ControlMode.PercentOutput, -0.1);
241+
// // }
242+
// gyro.reset(); //sets gyro value to 0
243+
244+
// if ((Math.abs(shooterCalculations[0][0]) < 1.5)) {
245+
// drive.tankDrive(-0.8, -0.8); // backs up until 1.5 meters away
246+
// }
247+
// if ((Math.abs(shooterCalculations[0][0]) >= 1.5) && (Math.abs(shooterCalculations[1][0]) > 1)) { // TODO: adjust to reduce bouncing
248+
// centerAim("top_hub"); // fires a shot into the hub
249+
// }
250+
// if (Math.abs(shooterCalculations[1][0]) < 1 && (Math.abs(shooterCalculations[0][0]) >= 1.5) && stage1) {
251+
// if (counter < 1000) { // TODO: adjust based on how long it takes to shoot a ball
252+
// fire(); // shoots ball-> changes stage1 to false after having shot a ball
253+
// counter++;
254+
// }
255+
// else if (counter >= 1000) {
256+
// stage1 = false;
257+
// }
258+
// }
259+
// if (!stage1 && !ballSeen()) { // Locates ball if it can't see after shooting one
260+
// centerAim("ball_tracking");
261+
// rollers.set(ControlMode.PercentOutput, 0);
262+
// }
263+
// if (!stage1 && ballSeen()) { // Drives forward if it sees the ball
264+
// drive.tankDrive(0.8, 0.8);
265+
// rollers.set(ControlMode.PercentOutput, 1);
266+
// }
248267
}
249268

250269
/**
@@ -268,9 +287,9 @@ public void teleopPeriodic() {
268287
if(leftJoy.getTrigger()) { //center robot on top hub (retro reflector)
269288
centerAim("top_hub");
270289
}
271-
if(rightJoy.getTrigger()) { //center robot on ball from Raspberry Pi Data
272-
centerAim("ball_tracking");
273-
}
290+
// if(rightJoy.getTrigger()) { //center robot on ball from Raspberry Pi Data
291+
// centerAim("ball_tracking");
292+
// }
274293

275294
// DRIVE CALL
276295
if ((Math.abs(rightJoy.getY()) > 0.2) || Math.abs(leftJoy.getY()) > 0.2) drive.tankDrive(-rightJoy.getY(), -leftJoy.getY());// TODO: adjust deadzones
@@ -292,10 +311,10 @@ public void teleopPeriodic() {
292311

293312
public double[][] getLimelightData() {
294313
NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight");
295-
double heightDifference = ((72/39.37)-(17/39.37)); //13.0 (photo room)
296-
// double heightDifference = ((104/39.37)-(24.025/39.37)); //14.0 https://www.desmos.com/calculator/zmzfln2j6v
297-
double fixedLLANGLE = 14.7734450937; //13.0 Angle
298-
// double fixedLLANGLE = 40; //14.0 Angle https://www.desmos.com/calculator/zmzfln2j6v
314+
// double heightDifference = ((72/39.37)-(17/39.37)); //13.0 (photo room)
315+
double heightDifference = ((104/39.37)-(22.76/39.37)); //14.0 https://www.desmos.com/calculator/zmzfln2j6v
316+
// double fixedLLANGLE = 14.7734450937; //13.0 Angle
317+
double fixedLLANGLE = 40; //14.0 Angle https://www.desmos.com/calculator/zmzfln2j6v
299318

300319
double[][] LimelightInfo = {{0,0,0},{0,0,0}};
301320

@@ -353,40 +372,43 @@ public void centerAim(String target) {
353372
}
354373
}
355374

356-
private boolean ballSeen() {// TODO: Sriman/Ayush code this so we know if there's a ball
357-
return true;
358-
}
359-
360-
public void fire() {
361-
double transferPercent = 0.35;
362-
double[][] limelightData = getLimelightData();
363-
double idealAngle = limelightData[0][1];
364-
double idealVelocity = limelightData[0][2]; // flywheel diameter in meters
365-
366-
double rpm = getRPM(idealVelocity, transferPercent);
367-
if (!intakeLimit.get()) {
368-
wrist.set(ControlMode.PercentOutput, 0.75);
369-
}
370-
shooterFly.set(ControlMode.Velocity, 4*rpm*2048); //TODO: Gear ratio multiplier MIGHT HAVE TO MULTIPLY BY 2048- will test later
371-
if ((gyro.getRoll() > idealAngle + 5) && !upperShooterLimit.get()) { // TODO: Adjust degrees of freedom +- 5
372-
hood.set(ControlMode.PercentOutput, 0.1);
373-
ready = false;
374-
}
375-
else if ((gyro.getRoll() < idealAngle-5) && !lowerShooterLimit.get()) {
376-
hood.set(ControlMode.PercentOutput, -0.1);
377-
ready = false;
378-
}
379-
else {
380-
ready = true;
381-
}
382-
if (ready) {
383-
rollers.set(ControlMode.PercentOutput, 1);
384-
}
385-
//adjust to fine tuning
386-
// double OneRPM = (60*idealVelocity)/(8*Math.PI); //RPM required if 100% of speed from flywheel is transferred to the ball
387-
// double actualRPM = 10*OneRPM; //RPM needed bearing 10% of velocity is transferred from flywheel to ball
388-
// double flywheelMotor = actualRPM/the maximum rpm of the motor; //The motor output in percentage of the flywheel motor
389-
}
375+
// private boolean ballSeen() {// TODO: Sriman/Ayush code this so we know if there's a ball
376+
// return true;
377+
// }
378+
379+
// public void fire() {
380+
// double transferPercent = 0.35;
381+
// double[][] limelightData = getLimelightData();
382+
// double idealAngle = limelightData[0][1];
383+
// double idealVelocity = limelightData[0][2]; // flywheel diameter in meters
384+
385+
// double rpm = getRPM(idealVelocity, transferPercent);
386+
// if (!intakeLimit.get()) {
387+
// wrist.set(ControlMode.PercentOutput, 0.75);
388+
// }
389+
// shooterFly.set(ControlMode.Velocity, 4*rpm*2048); //TODO: Gear ratio multiplier MIGHT HAVE TO MULTIPLY BY 2048- will test later
390+
// if ((gyro.getRoll() > idealAngle + 5) && !upperShooterLimit.get()) { // TODO: Adjust degrees of freedom +- 5
391+
// hood.set(ControlMode.PercentOutput, 0.1);
392+
// ready = false;
393+
// }
394+
// else if ((gyro.getRoll() < idealAngle-5) && !lowerShooterLimit.get()) {
395+
// hood.set(ControlMode.PercentOutput, -0.1);
396+
// ready = false;
397+
// }
398+
// else {
399+
// ready = true;
400+
// }
401+
// if (ready) {
402+
// rollers.set(ControlMode.PercentOutput, 1);
403+
// }
404+
// else {
405+
// climbRotation.set(ControlMode.PercentOutput, 0);
406+
// }
407+
// //adjust to fine tuning
408+
// // double OneRPM = (60*idealVelocity)/(8*Math.PI); //RPM required if 100% of speed from flywheel is transferred to the ball
409+
// // double actualRPM = 10*OneRPM; //RPM needed bearing 10% of velocity is transferred from flywheel to ball
410+
// // double flywheelMotor = actualRPM/the maximum rpm of the motor; //The motor output in percentage of the flywheel motor
411+
// }
390412

391413
//TODO: Determines the RPM to set flywheel based on what RPM corresponds to what ball velocity
392414
public double getRPM(double idealVelocity, double transferPercent) {
@@ -422,9 +444,9 @@ else if (controller.getYButton()) {
422444
//Wrist for intake, no encoder (for teleop)
423445
public void wrist() {
424446
if (controller.getBButton()) { //sets intake in position to feed ball into flywheel
425-
wrist.set(ControlMode.PercentOutput, 0.75);
447+
wrist.set(ControlMode.PercentOutput, 0.2);
426448
} else if (controller.getXButton()) { //sets intake in position to pick balls off field
427-
wrist.set(ControlMode.PercentOutput, -0.4);
449+
wrist.set(ControlMode.PercentOutput, -0.2);
428450
} else {
429451
wrist.set(ControlMode.PercentOutput, 0);
430452
}

0 commit comments

Comments
 (0)