-
-
Notifications
You must be signed in to change notification settings - Fork 7
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
* Restructure feedforward to MotorFeedforward, add arm and elevator ff * spotless apply
- Loading branch information
Showing
8 changed files
with
270 additions
and
225 deletions.
There are no files selected for viewing
46 changes: 46 additions & 0 deletions
46
src/com/stuypulse/stuylib/control/angle/feedforward/AngleArmFeedforward.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,46 @@ | ||
/* Copyright (c) 2023 StuyPulse Robotics. All rights reserved. */ | ||
/* This work is licensed under the terms of the MIT license */ | ||
/* found in the root directory of this project. */ | ||
|
||
package com.stuypulse.stuylib.control.angle.feedforward; | ||
|
||
import com.stuypulse.stuylib.control.angle.AngleController; | ||
import com.stuypulse.stuylib.math.Angle; | ||
|
||
/** | ||
* A feedforward term to account for gravity for motorized arms that can move continuously (if not | ||
* use `ArmFeedforward`) | ||
* | ||
* <p>The motor feedforward used in the context of an arm will not account for gravity that is | ||
* acting on the arm. | ||
* | ||
* <p>Can be paired with MotorFeedforward or other controllers with .add | ||
* | ||
* @author Myles Pasetsky (myles.pasetsky@gmail.com) | ||
*/ | ||
public class AngleArmFeedforward extends AngleController { | ||
|
||
/** voltage to hold arm horizontal */ | ||
private final Number kG; | ||
|
||
/** | ||
* Create arm feedforward | ||
* | ||
* @param kG term to hold arm vertical against gravity (volts) | ||
*/ | ||
public AngleArmFeedforward(Number kG) { | ||
this.kG = kG; | ||
} | ||
|
||
/** | ||
* Calculates voltage to hold arm at the setpoint angle | ||
* | ||
* @param setpoint setpoint | ||
* @param measurement measurement | ||
* @return kG * cos(setpoint) | ||
*/ | ||
@Override | ||
protected double calculate(Angle setpoint, Angle measurement) { | ||
return kG.doubleValue() * setpoint.cos(); | ||
} | ||
} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
61 changes: 61 additions & 0 deletions
61
src/com/stuypulse/stuylib/control/feedforward/ArmFeedforward.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,61 @@ | ||
/* Copyright (c) 2023 StuyPulse Robotics. All rights reserved. */ | ||
/* This work is licensed under the terms of the MIT license */ | ||
/* found in the root directory of this project. */ | ||
|
||
package com.stuypulse.stuylib.control.feedforward; | ||
|
||
import com.stuypulse.stuylib.control.Controller; | ||
import com.stuypulse.stuylib.streams.filters.IFilter; | ||
|
||
/** | ||
* A feedforward term to account for gravity for motorized arms. | ||
* | ||
* <p>The motor feedforward used in the context of an arm will not account for gravity that is | ||
* acting on the arm. | ||
* | ||
* <p>Can be paired with MotorFeedforward or other controllers with .add | ||
* | ||
* @author Myles Pasetsky (myles.pasetsky@gmail.com) | ||
*/ | ||
public class ArmFeedforward extends Controller { | ||
|
||
/** voltage to hold arm horizontal */ | ||
private final Number kG; | ||
|
||
/** | ||
* function to configure units of cosine (or even use sin if angles are measured differently) | ||
*/ | ||
private final IFilter cosine; | ||
|
||
/** | ||
* Create arm feedforward | ||
* | ||
* @param kG term to hold arm vertical against gravity (volts) | ||
*/ | ||
public ArmFeedforward(Number kG) { | ||
this(kG, x -> Math.cos(Math.toRadians(x))); | ||
} | ||
|
||
/** | ||
* Create arm feedforward | ||
* | ||
* @param kG term to hold arm vertical against gravity (volts) | ||
* @param cosine function to calculate cosine of setpoint | ||
*/ | ||
public ArmFeedforward(Number kG, IFilter cosine) { | ||
this.kG = kG; | ||
this.cosine = cosine; | ||
} | ||
|
||
/** | ||
* Calculates voltage to hold arm at the setpoint angle | ||
* | ||
* @param setpoint setpoint | ||
* @param measurement measurement | ||
* @return kG * cos(setpoint) | ||
*/ | ||
@Override | ||
protected double calculate(double setpoint, double measurement) { | ||
return kG.doubleValue() * cosine.get(setpoint); | ||
} | ||
} |
44 changes: 44 additions & 0 deletions
44
src/com/stuypulse/stuylib/control/feedforward/ElevatorFeedforward.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,44 @@ | ||
/* Copyright (c) 2023 StuyPulse Robotics. All rights reserved. */ | ||
/* This work is licensed under the terms of the MIT license */ | ||
/* found in the root directory of this project. */ | ||
|
||
package com.stuypulse.stuylib.control.feedforward; | ||
|
||
import com.stuypulse.stuylib.control.Controller; | ||
|
||
/** | ||
* A feedforward term to account for gravity for motorized lifts. | ||
* | ||
* <p>The motor feedforward used in the context of a lift will not account for gravity that is | ||
* acting on the lift. | ||
* | ||
* <p>Can be paired with MotorFeedforward or other controllers with .add | ||
* | ||
* @author Myles Pasetsky (myles.pasetsky@gmail.com) | ||
*/ | ||
public class ElevatorFeedforward extends Controller { | ||
|
||
/** voltage to hold arm in place against gravity */ | ||