Skip to content

Commit ce57dfc

Browse files
Spikes-2212Spikes-2212
authored
Merge pull request #177 from Spikes-2212-Programming-Guild/dev
Dev
2 parents de99f3d + f70e1ef commit ce57dfc

15 files changed

+886
-99
lines changed

‎SpikesLib2.json

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
{
22
"fileName": "SpikesLib2.json",
33
"name": "SpikesLib2",
4-
"version": "v3.0.0",
4+
"version": "v3.1.0",
55
"uuid" : "018a8f54-8662-11ec-a8a3-0242ac120002",
66
"jsonUrl": "https://raw.githubusercontent.com/Spikes-2212-Programming-Guild/SpikesLib2/master/SpikesLib2.json",
77
"mavenUrls": [
@@ -14,7 +14,7 @@
1414
{
1515
"groupId": "com.github.Spikes-2212-Programming-Guild",
1616
"artifactId": "SpikesLib2",
17-
"version": "v3.0.0"
17+
"version": "v3.1.0"
1818
}
1919
]
2020
}

‎src/main/java/com/spikes2212/command/drivetrains/commands/smartmotorcontrollerdrivetrain/MoveSmartMotorControllerTankDrivetrain.java

Lines changed: 135 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,135 @@
1+
package com.spikes2212.command.drivetrains.commands.smartmotorcontrollerdrivetrain;
2+
3+
import com.spikes2212.command.drivetrains.smartmotorcontrollerdrivetrain.SmartMotorControllerTankDrivetrain;
4+
import com.spikes2212.command.genericsubsystem.smartmotorcontrollersubsystem.SmartMotorControllerGenericSubsystem;
5+
import com.spikes2212.control.FeedForwardSettings;
6+
import com.spikes2212.control.PIDSettings;
7+
import com.spikes2212.util.UnifiedControlMode;
8+
import edu.wpi.first.wpilibj.Timer;
9+
import edu.wpi.first.wpilibj2.command.CommandBase;
10+
11+
import java.util.function.Supplier;
12+
13+
/**
14+
* A command that moves a {@link SmartMotorControllerTankDrivetrain} using its master motor controllers' control loops.
15+
*
16+
* @author Yoel Perman Brilliant
17+
* @see SmartMotorControllerTankDrivetrain
18+
*/
19+
public class MoveSmartMotorControllerTankDrivetrain extends CommandBase {
20+
21+
/**
22+
* The {@link SmartMotorControllerTankDrivetrain} this command will run on.
23+
*/
24+
protected final SmartMotorControllerTankDrivetrain drivetrain;
25+
26+
/**
27+
* The left side loop's PID constants.
28+
*/
29+
protected final PIDSettings leftPIDSettings;
30+
31+
/**
32+
* The right side loop's PID constants.
33+
*/
34+
protected final PIDSettings rightPIDSettings;
35+
36+
/**
37+
* The loop's feed forward gains.
38+
*/
39+
protected final FeedForwardSettings feedForwardSettings;
40+
41+
/**
42+
* The loop's control mode (e.g. voltage, velocity, position...).
43+
*/
44+
protected final UnifiedControlMode controlMode;
45+
46+
/**
47+
* The setpoint this command should bring the left side of the {@link SmartMotorControllerTankDrivetrain} to.
48+
*/
49+
protected final Supplier<Double> leftSetpoint;
50+
51+
/**
52+
* The setpoint this command should bring the right side of the {@link SmartMotorControllerTankDrivetrain} to.
53+
*/
54+
protected final Supplier<Double> rightSetpoint;
55+
56+
/**
57+
* The most recent timestamp on which the left side's loop has not reached its target setpoint.
58+
*/
59+
private double lastTimeLeftNotOnTarget;
60+
61+
/**
62+
* The most recent timestamp on which the right side's loop has not reached its target setpoint.
63+
*/
64+
private double lastTimeRightNotOnTarget;
65+
66+
/**
67+
* Constructs a new instance of {@link MoveSmartMotorControllerTankDrivetrain}.
68+
*
69+
* @param drivetrain the {@link SmartMotorControllerTankDrivetrain} this command will run on
70+
* @param leftPIDSettings the left side's loop's PID constants
71+
* @param rightPIDSettings the right side's loop's PID constants
72+
* @param feedForwardSettings the loops' feed forward gains
73+
* @param controlMode the loops' control mode (e.g. voltage, velocity, position...)
74+
* @param leftSetpoint the setpoint this command should bring the
75+
* {@link SmartMotorControllerTankDrivetrain}'s left side to
76+
* @param rightSetpoint the setpoint this command should bring the
77+
* {@link SmartMotorControllerTankDrivetrain}'s right side to
78+
*/
79+
public MoveSmartMotorControllerTankDrivetrain(SmartMotorControllerTankDrivetrain drivetrain,
80+
PIDSettings leftPIDSettings, PIDSettings rightPIDSettings,
81+
FeedForwardSettings feedForwardSettings,
82+
UnifiedControlMode controlMode, Supplier<Double> leftSetpoint,
83+
Supplier<Double> rightSetpoint) {
84+
addRequirements(drivetrain);
85+
this.drivetrain = drivetrain;
86+
this.controlMode = controlMode;
87+
this.leftPIDSettings = leftPIDSettings;
88+
this.rightPIDSettings = rightPIDSettings;
89+
this.feedForwardSettings = feedForwardSettings;
90+
this.leftSetpoint = leftSetpoint;
91+
this.rightSetpoint = rightSetpoint;
92+
this.lastTimeLeftNotOnTarget = 0;
93+
this.lastTimeRightNotOnTarget = 0;
94+
}
95+
96+
/**
97+
* Configures the drivetrain's control loops.
98+
*/
99+
@Override
100+
public void initialize() {
101+
drivetrain.configureLoop(leftPIDSettings, rightPIDSettings, feedForwardSettings);
102+
}
103+
104+
/**
105+
* Updates any control loops running on the drivetrain's motor controllers.
106+
*/
107+
@Override
108+
public void execute() {
109+
drivetrain.pidSet(controlMode, leftSetpoint.get(), rightSetpoint.get(), leftPIDSettings,
110+
rightPIDSettings, feedForwardSettings);
111+
}
112+
113+
@Override
114+
public void end(boolean interrupted) {
115+
drivetrain.finish();
116+
}
117+
118+
/**
119+
* Checks if the drivetrain has been at its target setpoint for longer than its allowed wait time.
120+
*
121+
* @return {@code true} if the command should finish running, {@code false} otherwise
122+
*/
123+
@Override
124+
public boolean isFinished() {
125+
double now = Timer.getFPGATimestamp();
126+
if (!drivetrain.leftOnTarget(controlMode, leftPIDSettings.getTolerance(), leftSetpoint.get())) {
127+
lastTimeLeftNotOnTarget = now;
128+
}
129+
if (!drivetrain.rightOnTarget(controlMode, rightPIDSettings.getTolerance(), rightSetpoint.get())) {
130+
lastTimeRightNotOnTarget = now;
131+
}
132+
return now - lastTimeLeftNotOnTarget >= leftPIDSettings.getWaitTime() &&
133+
now - lastTimeRightNotOnTarget >= rightPIDSettings.getWaitTime();
134+
}
135+
}

‎src/main/java/com/spikes2212/command/drivetrains/commands/smartmotorcontrollerdrivetrain/MoveSmartMotorControllerTankDrivetrainTrapezically.java

Lines changed: 60 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,60 @@
1+
package com.spikes2212.command.drivetrains.commands.smartmotorcontrollerdrivetrain;
2+
3+
import com.spikes2212.command.drivetrains.smartmotorcontrollerdrivetrain.SmartMotorControllerTankDrivetrain;
4+
import com.spikes2212.control.FeedForwardSettings;
5+
import com.spikes2212.control.PIDSettings;
6+
import com.spikes2212.control.TrapezoidProfileSettings;
7+
import com.spikes2212.util.UnifiedControlMode;
8+
9+
import java.util.function.Supplier;
10+
11+
/**
12+
* A command that moves a {@link SmartMotorControllerTankDrivetrain} by running a trapezoid profile control loop on its
13+
* master motor controllers.
14+
*
15+
* @author Yoel Perman Brilliant
16+
* @see SmartMotorControllerTankDrivetrain
17+
* @see MoveSmartMotorControllerTankDrivetrain
18+
*/
19+
public class MoveSmartMotorControllerTankDrivetrainTrapezically extends MoveSmartMotorControllerTankDrivetrain {
20+
21+
/**
22+
* The loops' trapezoid profile configurations.
23+
*/
24+
protected final TrapezoidProfileSettings trapezoidProfileSettings;
25+
26+
/**
27+
* Constructs a new instance of {@link MoveSmartMotorControllerTankDrivetrainTrapezically}.
28+
*
29+
* @param drivetrain the {@link SmartMotorControllerTankDrivetrain} this command will run on
30+
* @param leftPIDSettings the left side's loop's PID constants
31+
* @param rightPIDSettings the right side's loop's PID constants
32+
* @param feedForwardSettings the loop's feed forward gains
33+
* @param leftSetpoint the setpoint this command should bring the
34+
* {@link SmartMotorControllerTankDrivetrain}'s left side to
35+
* @param rightSetpoint the setpoint this command should bring the
36+
* {@link SmartMotorControllerTankDrivetrain}'s right side to
37+
* @param trapezoidProfileSettings the trapezoid profile settings
38+
*/
39+
public MoveSmartMotorControllerTankDrivetrainTrapezically(SmartMotorControllerTankDrivetrain drivetrain,
40+
PIDSettings leftPIDSettings, PIDSettings rightPIDSettings,
41+
FeedForwardSettings feedForwardSettings,
42+
Supplier<Double> leftSetpoint,
43+
Supplier<Double> rightSetpoint,
44+
TrapezoidProfileSettings trapezoidProfileSettings) {
45+
super(drivetrain, leftPIDSettings, rightPIDSettings, feedForwardSettings, UnifiedControlMode.TRAPEZOID_PROFILE,
46+
leftSetpoint, rightSetpoint);
47+
this.trapezoidProfileSettings = trapezoidProfileSettings;
48+
}
49+
50+
@Override
51+
public void initialize() {
52+
drivetrain.configureLoop(leftPIDSettings, rightPIDSettings, feedForwardSettings, trapezoidProfileSettings);
53+
}
54+
55+
@Override
56+
public void execute() {
57+
drivetrain.pidSet(controlMode, leftSetpoint.get(), rightSetpoint.get(), leftPIDSettings, rightPIDSettings,
58+
feedForwardSettings, trapezoidProfileSettings);
59+
}
60+
}

0 commit comments

Comments
 (0)