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
|
package org.usfirst.frc.team4272.robot2016;
import org.usfirst.frc.team4272.robotlib.PIDOutputSplitter;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.PIDOutput;
import edu.wpi.first.wpilibj.PIDSourceType;
import edu.wpi.first.wpilibj.Talon;
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)}
//public final Encoder lDriveE = new Encoder(/*DIO*/2,/*DIO*/3);
//public final Encoder rDriveE = new Encoder(/*DIO*/0,/*DIO*/1, true);
private final PIDOutput lDriveM1 = new Talon(/*PWM*/1), lDriveM2 = new Talon(/*PWM*/0);
private final PIDOutput rDriveM1 = new Talon(/*PWM*/2), rDriveM2 = new Talon(/*PWM*/4);
private final PIDOutput armM = new Talon(/*PWM*/8);
public final Encoder armE = new Encoder(/*DIO*/0, /*DIO*/1, /*DIO*/2);
private final PIDOutput fBallM = new PIDOutputSplitter(new Talon(/*PWM*/9), new Talon(/*PWM*/3));
private final PIDOutput bBallM = new PIDOutputSplitter(new Talon(/*PWM*/6), new Talon(/*PWM*/7));
public final DigitalInput armL = new DigitalInput(/*DIO*/3);
public final DigitalInput ballL = new DigitalInput(/*DIO*/4);
public HwRobot() {
//lDriveE.setPIDSourceType(PIDSourceType.kRate);
//rDriveE.setPIDSourceType(PIDSourceType.kRate);
armE.setPIDSourceType(PIDSourceType.kDisplacement);
}
public void run(Control c) {
if (armE.pidGet() < -295)
c.arm = 0;
lDriveM1.pidWrite( c.lDrive1);
lDriveM2.pidWrite( c.lDrive2);
rDriveM1.pidWrite(-c.rDrive1);
rDriveM2.pidWrite(-c.rDrive2);
armM.pidWrite(c.arm);
fBallM.pidWrite(-c.fBall);
bBallM.pidWrite(-c.bBall);
SmartDashboard.putNumber("fBall", c.fBall);
SmartDashboard.putNumber("bBall", c.bBall);
SmartDashboard.putNumber("armM", c.arm);
SmartDashboard.putNumber("armE", armE.pidGet());
}
}
|
