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

Commit 8ee9a13

Browse files
nelly-yynelly-yy
committed
PID Simulator for lift, currently tuning constants
Couple of issues: Validity of constants that affect acceleration as well as PID constant tuning. On a separate note, all the commented lines with parameters on top of variables is for dynamic value testing ( part of junit ) .
1 parent 9f30235 commit 8ee9a13

File tree

1 file changed

+275
-0
lines changed

1 file changed

+275
-0
lines changed

‎Robot2018/test/org/usfirst/frc/team199/Robot2018/PIDConstSim.java‎

Lines changed: 275 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,275 @@
1+
package org.usfirst.frc.team199.Robot2018;
2+
3+
import org.junit.jupiter.api.*;
4+
import org.junit.jupiter.api.BeforeEach;
5+
import org.junit.jupiter.api.Test;
6+
import org.junit.runner.RunWith;
7+
import org.mockito.invocation.InvocationOnMock;
8+
import org.mockito.stubbing.Answer;
9+
10+
import static org.junit.Assert.assertTrue;
11+
import static org.mockito.Mockito.mock;
12+
import static org.mockito.Mockito.when;
13+
14+
import java.util.Arrays;
15+
import java.util.Collection;
16+
17+
import edu.wpi.first.wpilibj.HLUsageReporting;
18+
import edu.wpi.first.wpilibj.PIDController;
19+
import edu.wpi.first.wpilibj.PIDOutput;
20+
import edu.wpi.first.wpilibj.PIDSource;
21+
import edu.wpi.first.wpilibj.PIDSourceType;
22+
import edu.wpi.first.wpilibj.Timer;
23+
import edu.wpi.first.wpilibj.internal.HardwareTimer;
24+
import org.junit.runners.Parameterized;
25+
import org.junit.runners.Parameterized.Parameters;
26+
import org.junit.runners.Parameterized.Parameter;
27+
28+
//@RunWith(value = Parameterized.class)
29+
class PIDConstSim {
30+
static final double initkP = 10;
31+
static final double initkI = 6;
32+
static final double initkD = 6;
33+
34+
//@Parameter(value = )
35+
double liftkP = initkP;
36+
double liftkI = initkI;
37+
double liftkD = initkD;
38+
39+
private final double targetOne = 24;
40+
private final double targetTwo = 60;
41+
private final double targetThree = 45;
42+
// 1 in in meters
43+
private final double tolerance = 0.0254;
44+
45+
double position = 0;
46+
double velocity_ = 0;
47+
double voltage_ = 0;
48+
double offset_ = -0.1;
49+
50+
// Stall Torque in N m
51+
static final double kStallTorque = 2.41;
52+
// Stall Current in Amps
53+
static final double kStallCurrent = 131;
54+
// Free Speed in RPM
55+
static final double kFreeSpeed = 5330;
56+
// Free Current in Amps
57+
static final double kFreeCurrent = 2.7;
58+
// Mass of the Elevator
59+
// 51.3 lbs
60+
static final double kMass = 23.2693;
61+
// Number of motors
62+
static final double kNumMotors = 3.0;
63+
// Resistance of the motor
64+
static final double kResistance = 12.0 / kStallCurrent;
65+
// Motor velocity constant
66+
static final double Kv = ((kFreeSpeed / 60.0 * 2.0 * Math.PI) /
67+
(12.0 - kResistance * kFreeCurrent));
68+
// Torque constant
69+
static final double Kt = (kNumMotors * kStallTorque) / kStallCurrent;
70+
// Gear ratio
71+
// 11.754 lift gear ratio reduction?
72+
static final double kG = 11.754;
73+
// Radius of pulley
74+
static final double kr = 0.02273;
75+
76+
// Control loop time step
77+
static final double kDt = 0.01;
78+
79+
double current_time = 0;
80+
81+
// V = I * R + omega / Kv
82+
// torque = Kt * I
83+
84+
85+
// (name = "{index}: testAdd({0}+{1}) = {2}")
86+
/*@Parameters
87+
public static double[][] data() {
88+
// array is P I and D finalants in order
89+
//double [][][] PIDfinalTable = new double[5][5][5];
90+
double [][] PIDfinalTable = new double[3][5];
91+
double diffInter3 = initkD / 10;
92+
double diffInter2 = initkI / 10;
93+
double diffInter1 = initkP / 10;
94+
for(int i = 0; i < 3; i++) {
95+
for(int j = -2; j < 3; j++) {
96+
double res;
97+
if(i == 0)
98+
res = initkP + j * diffInter1;
99+
else if(i == 1)
100+
res = initkI + j * diffInter2;
101+
else
102+
res = initkD + j * diffInter3;
103+
PIDfinalTable[i][j] = res;
104+
}
105+
}
106+
//return (Iterable<Double[]>)Arrays.asList(PIDfinalTable);
107+
return PIDfinalTable;
108+
}
109+
*/
110+
111+
private double GetAcceleration(double voltage) {
112+
return -Kt * kG * kG / (Kv * kResistance * kr * kr * kMass) * velocity_ +
113+
kG * Kt / (kResistance * kr * kMass) * voltage;
114+
}
115+
116+
private void simulateTime(double time, double voltage) {
117+
final double starting_time = time;
118+
while (time > 0) {
119+
final double current_dt = Math.min(time, 0.001);
120+
//System.out.println("vel before: " + velocity_);
121+
position += current_dt * velocity_;
122+
double acc = GetAcceleration(voltage);
123+
//System.out.println("acc: " + acc);
124+
velocity_ += current_dt * acc;
125+
//System.out.println("vel after: " + velocity_);
126+
time -= 0.001;
127+
//EXPECT_LE(position, ElevatorLoop::kMaxHeight + 0.01);
128+
//EXPECT_GE(position, ElevatorLoop::kMinHeight - 0.01);
129+
}
130+
current_time += starting_time;
131+
}
132+
133+
@BeforeEach
134+
void setUp() {
135+
// Since VelocityPIDController extends PIDController and PIDController calls
136+
// static methods in wpilib that only work on robot,
137+
// we setup these mocks to allow the tests to run off robot.
138+
// can't start at 0 because it will crash timer
139+
current_time = kDt;
140+
HardwareTimer tim = mock(HardwareTimer.class);
141+
Timer.Interface timerInstance = mock(Timer.Interface.class);
142+
when(tim.newTimer()).thenReturn(timerInstance);
143+
when(timerInstance.get()).thenAnswer(
144+
new Answer() {
145+
public Object answer(InvocationOnMock invocation) {
146+
//return getTime();
147+
return current_time;
148+
}
149+
});
150+
151+
Timer.SetImplementation(tim);
152+
HLUsageReporting.Interface usageReporter = mock(HLUsageReporting.Interface.class);
153+
HLUsageReporting.SetImplementation(usageReporter);
154+
}
155+
156+
class PIDSim{
157+
public PIDSim() {
158+
//position = 0;
159+
}
160+
161+
public double pidGet() {
162+
return position;
163+
}
164+
165+
public void pidWrite(double output) {
166+
//System.out.println("voltage: " + output);
167+
simulateTime(kDt, output * 12);
168+
System.out.println("position: " + position);
169+
}
170+
}
171+
172+
class PIDSimController extends PIDController{
173+
174+
public PIDSimController(double p, double i, double d, PIDSource src, PIDOutput out) {
175+
super(p,i,d,src,out);
176+
}
177+
178+
public void calc() {
179+
this.calculate();
180+
}
181+
182+
}
183+
184+
class PIDSimSource implements PIDSource{
185+
private PIDSim sim;
186+
private PIDSourceType type;
187+
public PIDSimSource(PIDSim sim) {
188+
this.sim = sim;
189+
}
190+
191+
@Override
192+
public PIDSourceType getPIDSourceType() {
193+
return type;
194+
}
195+
196+
@Override
197+
public double pidGet() {
198+
return sim.pidGet();
199+
}
200+
201+
@Override
202+
public void setPIDSourceType(PIDSourceType pidSource) {
203+
// TODO Auto-generated method stub
204+
type = pidSource;
205+
}
206+
}
207+
208+
class PIDSimOutput implements PIDOutput{
209+
private PIDSim sim;
210+
public PIDSimOutput(PIDSim sim) {
211+
this.sim = sim;
212+
}
213+
@Override
214+
public void pidWrite(double output) {
215+
sim.pidWrite(output);
216+
}
217+
}
218+
219+
void test24() {
220+
PIDSim sim = new PIDSim();
221+
PIDSimSource src = new PIDSimSource(sim);
222+
src.setPIDSourceType(PIDSourceType.kDisplacement);
223+
PIDSimOutput out = new PIDSimOutput(sim);
224+
PIDSimController liftController = new PIDSimController(liftkP, liftkI, liftkD, src, out);
225+
liftController.setSetpoint(0.6);
226+
liftController.setOutputRange(-1.0, 1.0);
227+
liftController.enable();
228+
//1 second?
229+
for(int i = 0; i < 100; i++) {
230+
liftController.calc();
231+
}
232+
System.out.println("Test" + targetOne + ": " + sim.pidGet());
233+
System.out.println("curr_time_1: " + current_time);
234+
assertTrue(sim.pidGet() > 0);
235+
assertTrue(Math.abs(0.6 - sim.pidGet()) < tolerance);
236+
}
237+
238+
void test60() {
239+
PIDSim sim = new PIDSim();
240+
PIDSimSource src = new PIDSimSource(sim);
241+
src.setPIDSourceType(PIDSourceType.kDisplacement);
242+
PIDSimOutput out = new PIDSimOutput(sim);
243+
PIDSimController liftController = new PIDSimController(liftkP, liftkI, liftkD, src, out);
244+
liftController.setSetpoint(1.524);
245+
liftController.setOutputRange(-1.0, 1.0);
246+
liftController.enable();
247+
//1 second?
248+
for(int i = 0; i < 200; i++) {
249+
liftController.calc();
250+
}
251+
System.out.println("" + sim.pidGet());
252+
assertTrue(sim.pidGet() > 0);
253+
assertTrue(Math.abs(1.524 - sim.pidGet()) < tolerance);
254+
}
255+
256+
@Test
257+
void test45() {
258+
PIDSim sim = new PIDSim();
259+
PIDSimSource src = new PIDSimSource(sim);
260+
src.setPIDSourceType(PIDSourceType.kDisplacement);
261+
PIDSimOutput out = new PIDSimOutput(sim);
262+
PIDSimController liftController = new PIDSimController(liftkP, liftkI, liftkD, src, out);
263+
liftController.setSetpoint(1.143);
264+
liftController.setOutputRange(-1.0, 1.0);
265+
liftController.enable();
266+
//1 second?
267+
for(int i = 0; i < 100; i++) {
268+
liftController.calc();
269+
}
270+
System.out.println("" + sim.pidGet());
271+
assertTrue(sim.pidGet() > 0);
272+
assertTrue(Math.abs(1.143 - sim.pidGet()) < tolerance);
273+
}
274+
275+
}

0 commit comments

Comments
 (0)