-
Notifications
You must be signed in to change notification settings - Fork 169
/
Robot.java
160 lines (138 loc) · 5.68 KB
/
Robot.java
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
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
/**
* Phoenix Software License Agreement
*
* Copyright (C) Cross The Road Electronics. All rights
* reserved.
*
* Cross The Road Electronics (CTRE) licenses to you the right to
* use, publish, and distribute copies of CRF (Cross The Road) firmware files (*.crf) and
* Phoenix Software API Libraries ONLY when in use with CTR Electronics hardware products
* as well as the FRC roboRIO when in use in FRC Competition.
*
* THE SOFTWARE AND DOCUMENTATION ARE PROVIDED "AS IS" WITHOUT
* WARRANTY OF ANY KIND, EITHER EXPRESS OR IMPLIED, INCLUDING WITHOUT
* LIMITATION, ANY WARRANTY OF MERCHANTABILITY, FITNESS FOR A
* PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT SHALL
* CROSS THE ROAD ELECTRONICS BE LIABLE FOR ANY INCIDENTAL, SPECIAL,
* INDIRECT OR CONSEQUENTIAL DAMAGES, LOST PROFITS OR LOST DATA, COST OF
* PROCUREMENT OF SUBSTITUTE GOODS, TECHNOLOGY OR SERVICES, ANY CLAIMS
* BY THIRD PARTIES (INCLUDING BUT NOT LIMITED TO ANY DEFENSE
* THEREOF), ANY CLAIMS FOR INDEMNITY OR CONTRIBUTION, OR OTHER
* SIMILAR COSTS, WHETHER ASSERTED ON THE BASIS OF CONTRACT, TORT
* (INCLUDING NEGLIGENCE), BREACH OF WARRANTY, OR OTHERWISE
*/
/**
* Description:
* The VelocityClosedLoop example demonstrates the velocity closed-loop servo.
* Tested with Logitech F350 USB Gamepad inserted into Driver Station.
*
* Be sure to select the correct feedback sensor using configSelectedFeedbackSensor() below.
* Use Percent Output Mode (Holding A and using Left Joystick) to confirm talon is driving
* forward (Green LED on Talon/Victor) when the position sensor is moving in the positive
* direction. If this is not the case, flip the boolean input in setSensorPhase().
*
* Controls:
* Button 1: When held, start and run Velocity Closed Loop on Talon/Victor
* Left Joystick Y-Axis:
* + Percent Output: Throttle Talon forward and reverse, use to confirm hardware setup
* + Velocity Closed Loop: Servo Talon forward and reverse [-500, 500] RPM
*
* Gains for Velocity Closed Loop may need to be adjusted in Constants.java
*
* Supported Version:
* - Talon SRX: 4.00
* - Victor SPX: 4.00
* - Pigeon IMU: 4.00
* - CANifier: 4.00
*/
package frc.robot;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.Joystick;
import com.ctre.phoenix.motorcontrol.can.*;
import com.ctre.phoenix.motorcontrol.*;
import frc.robot.sim.PhysicsSim;
public class Robot extends TimedRobot {
/* Hardware */
WPI_TalonSRX _talon = new WPI_TalonSRX(1);
Joystick _joy = new Joystick(0);
/* String for output */
StringBuilder _sb = new StringBuilder();
/* Loop tracker for prints */
int _loops = 0;
public void simulationInit() {
PhysicsSim.getInstance().addTalonSRX(_talon, 1.5, 7200, true);
}
public void simulationPeriodic() {
PhysicsSim.getInstance().run();
}
public void robotInit() {
/* Factory Default all hardware to prevent unexpected behaviour */
_talon.configFactoryDefault();
/* Config sensor used for Primary PID [Velocity] */
_talon.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative,
Constants.kPIDLoopIdx,
Constants.kTimeoutMs);
/**
* Phase sensor accordingly.
* Positive Sensor Reading should match Green (blinking) Leds on Talon
*/
_talon.setSensorPhase(true);
/* Config the peak and nominal outputs */
_talon.configNominalOutputForward(0, Constants.kTimeoutMs);
_talon.configNominalOutputReverse(0, Constants.kTimeoutMs);
_talon.configPeakOutputForward(1, Constants.kTimeoutMs);
_talon.configPeakOutputReverse(-1, Constants.kTimeoutMs);
/* Config the Velocity closed loop gains in slot0 */
_talon.config_kF(Constants.kPIDLoopIdx, Constants.kGains_Velocit.kF, Constants.kTimeoutMs);
_talon.config_kP(Constants.kPIDLoopIdx, Constants.kGains_Velocit.kP, Constants.kTimeoutMs);
_talon.config_kI(Constants.kPIDLoopIdx, Constants.kGains_Velocit.kI, Constants.kTimeoutMs);
_talon.config_kD(Constants.kPIDLoopIdx, Constants.kGains_Velocit.kD, Constants.kTimeoutMs);
}
/**
* This function is called periodically during operator control
*/
public void teleopPeriodic() {
/* Get gamepad axis */
double leftYstick = -1 * _joy.getY();
/* Get Talon/Victor's current output percentage */
double motorOutput = _talon.getMotorOutputPercent();
/* Prepare line to print */
_sb.append("\tout:");
/* Cast to int to remove decimal places */
_sb.append((int) (motorOutput * 100));
_sb.append("%"); // Percent
_sb.append("\tspd:");
_sb.append(_talon.getSelectedSensorVelocity(Constants.kPIDLoopIdx));
_sb.append("u"); // Native units
/**
* When button 1 is held, start and run Velocity Closed loop.
* Velocity Closed Loop is controlled by joystick position x500 RPM, [-500, 500] RPM
*/
if (_joy.getRawButton(1)) {
/* Velocity Closed Loop */
/**
* Convert 500 RPM to units / 100ms.
* 4096 Units/Rev * 500 RPM / 600 100ms/min in either direction:
* velocity setpoint is in units/100ms
*/
double targetVelocity_UnitsPer100ms = leftYstick * 500.0 * 4096 / 600;
/* 500 RPM in either direction */
_talon.set(ControlMode.Velocity, targetVelocity_UnitsPer100ms);
/* Append more signals to print when in speed mode. */
_sb.append("\terr:");
_sb.append(_talon.getClosedLoopError(Constants.kPIDLoopIdx));
_sb.append("\ttrg:");
_sb.append(targetVelocity_UnitsPer100ms);
} else {
/* Percent Output */
_talon.set(ControlMode.PercentOutput, leftYstick);
}
/* Print built string every 10 loops */
if (++_loops >= 10) {
_loops = 0;
System.out.println(_sb.toString());
}
/* Reset built string */
_sb.setLength(0);
}
}