1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
|
/**
* Copyright (c) 2015-2017 Luke Shumaker.
* Copyright (c) 2017 Cameron Richards.
* Copyright (c) 2017 Noah Gaeta.
* Copyright (c) 2017 Thomas Griffith.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the FIRST nor the
* names of its contributors may be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY FIRST AND CONTRIBUTORS``AS IS'' AND ANY
* EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
* WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR
* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR
* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
package org.usfirst.frc.team4272.robot2017;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.PIDOutput;
import edu.wpi.first.wpilibj.PIDSource;
import edu.wpi.first.wpilibj.PIDSourceType;
import edu.wpi.first.wpilibj.PowerDistributionPanel;
import edu.wpi.first.wpilibj.Talon;
import edu.wpi.first.wpilibj.networktables.NetworkTable;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import org.usfirst.frc.team4272.robotlib.PIDOutputSplitter;
import org.usfirst.frc.team4272.robotlib.PIDServo;
import org.usfirst.frc.team4272.robotlib.RollingAvg;
public class HwRobot {
/* Relay == a Spike */
/* PCM = Pneumatics Control Module */
/* All of the numbered inputs are in the classes:
* - DIO: 0-9
* - Relay: 0-3
* - Analog In: 0-3
* - PWM: 0-9
* - PCM: 0-7 (the PCM is connected via CAN).
* - CAN
*
* For completeness, the roboRIO also has: i2c, RS-232, SPI,
* RSL, 2xUSB-A, an accelerometer, and an expansion port.
*
* And, for communication: USB-B and Ethernet.
*/
// naming convention: {side_letter}{Thing}{E|M (encoder/motor)}
// Actuators should be private /////////////////////////////////////////
private final PIDOutput
rDriveM = new Talon(/*PWM*/0), /* PDP 2, 3 */
lDriveM = new Talon(/*PWM*/1), /* PDP 12, 13 */
climber = new Talon(/*PWM*/3); /* PDP 4 */
private final DoubleSolenoid
shifter = new DoubleSolenoid(/*PCM*/4, /*PCM*/5),
ged = new DoubleSolenoid(/*PCM*/6, /*PCM*/7);
// Sensors/info should be public ///////////////////////////////////////
public final Encoder lDriveE = new Encoder(/*DIO*/0,/*DIO*/1, /*reverse*/false);
public final Encoder rDriveE = new Encoder(/*DIO*/2,/*DIO*/3, /*reverse*/true);
public final PIDSource lRate, rRate;
public final DriverStation ds = DriverStation.getInstance();
public final double axleWidth = 2.0; // in feet
// Use the constructor to initialize anything you can't
// initialize above.
public HwRobot() {
lDriveE.setDistancePerPulse(10.0/*feet*/ / 1865.75/*pulses*/);
rDriveE.setDistancePerPulse(10.0/*feet*/ / 1865.75/*pulses*/);
lDriveE.setPIDSourceType(PIDSourceType.kRate);
rDriveE.setPIDSourceType(PIDSourceType.kRate);
lRate = new RollingAvg(5, lDriveE);
rRate = new RollingAvg(5, rDriveE);
}
private double maxRate = 0;
private double minBatt = 20;
public void run(Control c) {
lDriveM.pidWrite(-c.lDrive);
rDriveM.pidWrite(c.rDrive);
climber.pidWrite(c.climber);
shifter.set(c.highGear ? DoubleSolenoid.Value.kForward : DoubleSolenoid.Value.kReverse);
ged.set(c.gedOut ? DoubleSolenoid.Value.kReverse : DoubleSolenoid.Value.kForward);
double rate = Math.abs((lDriveE.getRate()+rDriveE.getRate())/2);
SmartDashboard.putNumber("lDriveE distance", lDriveE.getDistance());
SmartDashboard.putNumber("lDriveE rate", Math.abs(lDriveE.getRate()));
SmartDashboard.putNumber("rDriveE distance", rDriveE.getDistance());
SmartDashboard.putNumber("rDriveE rate", Math.abs(rDriveE.getRate()));
SmartDashboard.putNumber("rate", rate);
if (rate > maxRate)
maxRate = rate;
SmartDashboard.putNumber("maxRate", maxRate);
double batt = ds.getBatteryVoltage();
SmartDashboard.putNumber("batt", batt);
if (batt < minBatt)
minBatt = batt;
SmartDashboard.putNumber("minBatt", minBatt);
SmartDashboard.putBoolean("highGear", c.highGear);
SmartDashboard.putBoolean("gedOut", c.gedOut);
}
}
|
