Skip to content
This repository was archived by the owner on Sep 14, 2019. It is now read-only.

Commit 2c2738c

Browse files
latest changes so code can run on robot
includes shuffleboard json file intake srx commented out so doesn't look for motor that's not there some changes to calcDefkD method
1 parent f89470d commit 2c2738c

3 files changed

Lines changed: 95 additions & 14 deletions

File tree

Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java

Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -63,7 +63,7 @@ public class RobotMap {
6363
public static AHRS fancyGyro;
6464
public static DoubleSolenoid dtGear;
6565

66-
private final double DIST_PER_PULSE_RATIO = (5.0 * Math.PI) * (17 / 25) / (3 * 256);
66+
private final double DIST_PER_PULSE_RATIO = (5.0 * Math.PI) * (17.0 / 25) / (3.0 * 256);
6767

6868
/**
6969
* This function takes in a TalonSRX motorController and sets nominal and peak
@@ -123,8 +123,8 @@ public RobotMap() {
123123
leftEncDist.setPIDSourceType(PIDSourceType.kDisplacement);
124124
leftEncRate = new Encoder(leftEncPort1, leftEncPort2);
125125
leftEncRate.setPIDSourceType(PIDSourceType.kRate);
126-
leftEncDist.setDistancePerPulse(Robot.getConst("DPP", 0.013908));
127-
leftEncRate.setDistancePerPulse(Robot.getConst("DPP", 0.013908));
126+
leftEncDist.setDistancePerPulse(Robot.getConst("DPP", DIST_PER_PULSE_RATIO));
127+
leftEncRate.setDistancePerPulse(Robot.getConst("DPP", DIST_PER_PULSE_RATIO));
128128

129129
dtLeftMaster = new WPI_TalonSRX(getPort("LeftTalonSRXMaster", 1));
130130
configSRX(dtLeftMaster);
@@ -150,8 +150,8 @@ public RobotMap() {
150150
rightEncDist.setPIDSourceType(PIDSourceType.kDisplacement);
151151
rightEncRate = new Encoder(rightEncPort1, rightEncPort2);
152152
rightEncRate.setPIDSourceType(PIDSourceType.kRate);
153-
rightEncDist.setDistancePerPulse(Robot.getConst("DPP", 0.013908));
154-
rightEncRate.setDistancePerPulse(Robot.getConst("DPP", 0.013908));
153+
rightEncDist.setDistancePerPulse(Robot.getConst("DPP", DIST_PER_PULSE_RATIO));
154+
rightEncRate.setDistancePerPulse(Robot.getConst("DPP", DIST_PER_PULSE_RATIO));
155155

156156
dtRightMaster = new WPI_TalonSRX(getPort("RightTalonSRXMaster", 4));
157157
configSRX(dtRightMaster);
@@ -215,15 +215,15 @@ public double calcDefkD() {
215215
double gearReduction = Robot.getBool("High Gear", false) ? Robot.getConst("High Gear Gear Reduction", 5.392)
216216
: Robot.getConst("Low Gear Gear Reduction", 12.255);
217217
double radius = Robot.getConst("Radius of Drivetrain Wheel", 0.0635);
218-
double timeConstant = Robot.getConst("Omega Max", 5330) / gearReduction
218+
double timeConstant = Robot.getConst("Omega Max", 5330) / gearReduction / 60 * 2 * Math.PI
219219
* convertNtokG(Robot.getConst("Weight of Robot", 342)) / 2 * radius * radius
220220
/ (Robot.getConst("Stall Torque", 2.41) * gearReduction * 2);
221221
double cycleTime = Robot.getConst("Code cycle time", 0.05);
222222
/*
223223
* The denominator of kD is 1-(e ^ -cycleTime / timeConstant). The numerator is
224224
* one.
225225
*/
226-
double denominator = 1 - Math.pow(Math.E, -1 * cycleTime / timeConstant);
226+
double denominator = Math.pow(Math.E, 1 * cycleTime / timeConstant) - 1;
227227
return 1 / denominator;
228228
}
229229

Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEject.java

Lines changed: 4 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -1,17 +1,13 @@
11
package org.usfirst.frc.team199.Robot2018.subsystems;
22

3-
import org.usfirst.frc.team199.Robot2018.RobotMap;
4-
5-
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
6-
73
import edu.wpi.first.wpilibj.command.Subsystem;
84

95
/**
106
*
117
*/
128
public class IntakeEject extends Subsystem implements IntakeEjectInterface {
139

14-
private final WPI_TalonSRX intakeMotor = RobotMap.intakeMotor;
10+
// private final WPI_TalonSRX intakeMotor = RobotMap.intakeMotor;
1511

1612
/**
1713
* Set the default command for a subsystem here.
@@ -25,7 +21,8 @@ public void initDefaultCommand() {
2521
* returns current motor value
2622
*/
2723
public double getIntakeSpeed() {
28-
return intakeMotor.get();
24+
// return intakeMotor.get();
25+
return 0;
2926
}
3027

3128
/**
@@ -48,7 +45,7 @@ public boolean hasCube() {
4845
*
4946
*/
5047
public void stopIntake() {
51-
intakeMotor.stopMotor();
48+
// intakeMotor.stopMotor();
5249
}
5350

5451
/**

shuffleboard.json

Lines changed: 84 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,84 @@
1+
{
2+
"dividerPosition": 0.29673590504451036,
3+
"tabPane": [
4+
{
5+
"title": "PID Testing",
6+
"autoPopulate": false,
7+
"autoPopulatePrefix": "",
8+
"widgetPane": {
9+
"gridSize": 128.0,
10+
"showGrid": true,
11+
"hgap": 16.0,
12+
"vgap": 16.0,
13+
"tiles": {
14+
"0,0": {
15+
"size": [
16+
2,
17+
2
18+
],
19+
"content": {
20+
"_type": "PID Controller",
21+
"_source0": "network_table:///SmartDashboard/Left PID Controller",
22+
"_title": "SmartDashboard/Left PID Controller"
23+
}
24+
},
25+
"2,0": {
26+
"size": [
27+
2,
28+
2
29+
],
30+
"content": {
31+
"_type": "PID Controller",
32+
"_source0": "network_table:///SmartDashboard/Right PID Controller",
33+
"_title": "SmartDashboard/Right PID Controller"
34+
}
35+
},
36+
"0,2": {
37+
"size": [
38+
1,
39+
1
40+
],
41+
"content": {
42+
"_type": "Text View",
43+
"_source0": "network_table:///SmartDashboard/Left Enc Rate",
44+
"_title": "SmartDashboard/Left Enc Rate"
45+
}
46+
},
47+
"0,3": {
48+
"size": [
49+
1,
50+
1
51+
],
52+
"content": {
53+
"_type": "Text View",
54+
"_source0": "network_table:///SmartDashboard/Left VPID Error",
55+
"_title": "SmartDashboard/Left VPID Error"
56+
}
57+
},
58+
"2,2": {
59+
"size": [
60+
1,
61+
1
62+
],
63+
"content": {
64+
"_type": "Text View",
65+
"_source0": "network_table:///SmartDashboard/Right Enc Rate",
66+
"_title": "SmartDashboard/Right Enc Rate"
67+
}
68+
},
69+
"2,3": {
70+
"size": [
71+
1,
72+
1
73+
],
74+
"content": {
75+
"_type": "Text View",
76+
"_source0": "network_table:///SmartDashboard/Right VPID Error",
77+
"_title": "SmartDashboard/Right VPID Error"
78+
}
79+
}
80+
}
81+
}
82+
}
83+
]
84+
}

0 commit comments

Comments
 (0)