Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Elevator #13

Merged
merged 38 commits into from
Jan 28, 2024
Merged
Changes from 1 commit
Commits
Show all changes
38 commits
Select commit Hold shift + click to select a range
92a0dfb
elevatorIO
nbhog Jan 12, 2024
e6b176c
started Elevator.kt file
nbhog Jan 14, 2024
1af374c
Reformatted Code
CodingMaster121 Jan 14, 2024
b161b28
Added Elevator.kt variables
CodingMaster121 Jan 16, 2024
6946de7
Added init for Elevator.kt
CodingMaster121 Jan 16, 2024
f5faca7
finished elevator.kt file
nbhog Jan 17, 2024
27a2573
started hardware code
nbhog Jan 18, 2024
51c66ed
started kraken hardware file
nbhog Jan 19, 2024
bd74ffe
finished hardware file
nbhog Jan 20, 2024
3a37208
toLog IO file
Shom770 Jan 9, 2024
d1711e7
Finished IntakeIO
Jan 10, 2024
983021a
Began intake file
fyrhanish Jan 11, 2024
4a0a407
Added state machine code
Jan 12, 2024
176d5a0
Removed arm from intake code
Jan 13, 2024
2bed455
intake code is finished
Shom770 Jan 13, 2024
d7203ca
remove encoder offset
Shom770 Jan 13, 2024
cc19b01
Removed zero state
Jan 14, 2024
1e29138
Initial commit for IntakeIOSim
NeonCoal Jan 14, 2024
8898d8e
update constants
NeonCoal Jan 16, 2024
dc978d6
stuff
NeonCoal Jan 16, 2024
9244539
fixing sim
NeonCoal Jan 16, 2024
e496d45
initial commit
Shom770 Jan 16, 2024
1b0397f
current changes so far
Shom770 Jan 16, 2024
95035f5
working dt:
Shom770 Jan 16, 2024
7164749
add wrapper class on velocity and position voltage requests
sswadkar Jan 20, 2024
6de3656
update falconutils to 1.1.28
sswadkar Jan 20, 2024
eef9240
rebase
MatthewChoulas Jan 20, 2024
8ed8301
rebase
MatthewChoulas Jan 20, 2024
8da8ad1
fix more rebase
MatthewChoulas Jan 20, 2024
81472cb
fix phoenix wrapper issues
MatthewChoulas Jan 20, 2024
c46c3bc
idk
MatthewChoulas Jan 20, 2024
1082757
fixed imports
nbhog Jan 21, 2024
4d50bec
fixed some programming issues
nbhog Jan 21, 2024
cf89177
fixed more programming issues
nbhog Jan 23, 2024
686b95b
fixed some programming issues
Aagames1 Jan 23, 2024
7c07104
fixed more problems
nbhog Jan 23, 2024
d075a80
tested simulation
nbhog Jan 24, 2024
308fc8d
bit more tuning
MatthewChoulas Jan 24, 2024
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
Finished IntakeIO
lakelandspark authored and MatthewChoulas committed Jan 20, 2024

Unverified

This commit is not signed, but one or more authors requires that any commit attributed to them is signed.
commit d1711e7510706c7b63546e00e308feb9c19212dc
Original file line number Diff line number Diff line change
@@ -0,0 +1,21 @@
package com.team4099.robot2023.config.constants

import org.team4099.lib.units.base.amps
import org.team4099.lib.units.derived.degrees
import org.team4099.lib.units.derived.volts

object IntakeConstants {
val VOLTAGE_COMPENSATION = 12.0.volts

// TODO: Add value for encoder offset
val ABSOLUTE_ENCODER_OFFSET = 0.0.degrees

// TODO: Change gear ratio according to robot
val ROLLER_CURRENT_LIMIT = 50.0.amps
val ARM_CURRENT_LIMIT = 50.0.amps
const val ROLLER_MOTOR_INVERTED = true
const val ARM_MOTOR_INVERTED = false
const val ROLLER_GEAR_RATIO = 36.0 / 18.0
const val ARM_GEAR_RATIO = ((60.0 / 12.0) * (80.0 / 18.0) * (32.0 / 16.0))
const val ARM_ENCODER_GEAR_RATIO = 36.0 / 18.0
}
Original file line number Diff line number Diff line change
@@ -1,4 +1,3 @@
package com.team4099.robot2023.subsystems.Shooter

class Shooter {
}
class Shooter
Original file line number Diff line number Diff line change
@@ -1,4 +1,3 @@
package com.team4099.robot2023.subsystems.TelescopingArm

class TelescopingArm {
}
class TelescopingArm
Original file line number Diff line number Diff line change
@@ -1,6 +1,5 @@
package com.team4099.robot2023.subsystems.feeder

import edu.wpi.first.wpilibj2.command.SubsystemBase
class Feeder(val io: FeederIO) : SubsystemBase() {

}
class Feeder(val io: FeederIO) : SubsystemBase()
Original file line number Diff line number Diff line change
@@ -1,4 +1,3 @@
package com.team4099.robot2023.subsystems.feeder

interface FeederIO {
}
interface FeederIO
Original file line number Diff line number Diff line change
@@ -1,4 +1,3 @@
package com.team4099.robot2023.subsystems.intake

class Intake {
}
class Intake
124 changes: 109 additions & 15 deletions src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt
Original file line number Diff line number Diff line change
@@ -6,33 +6,127 @@ import org.team4099.lib.units.base.amps
import org.team4099.lib.units.base.celsius
import org.team4099.lib.units.base.inAmperes
import org.team4099.lib.units.base.inCelsius
import org.team4099.lib.units.base.meters
import org.team4099.lib.units.derived.Angle
import org.team4099.lib.units.derived.DerivativeGain
import org.team4099.lib.units.derived.ElectricalPotential
import org.team4099.lib.units.derived.IntegralGain
import org.team4099.lib.units.derived.ProportionalGain
import org.team4099.lib.units.derived.Radian
import org.team4099.lib.units.derived.Volt
import org.team4099.lib.units.derived.degrees
import org.team4099.lib.units.derived.inDegrees
import org.team4099.lib.units.derived.inVolts
import org.team4099.lib.units.derived.rotations
import org.team4099.lib.units.derived.volts
import org.team4099.lib.units.inDegreesPerSecond
import org.team4099.lib.units.inRotationsPerMinute
import org.team4099.lib.units.perMinute
import org.team4099.lib.units.perSecond

interface IntakeIO {
class IntakeIOInputs : LoggableInputs {
var rollerVelocity = 0.0.rotations.perMinute
class IntakeIOInputs : LoggableInputs {
var rollerVelocity = 0.0.rotations.perMinute
var rollerAppliedVoltage = 0.0.volts

var rollerAppliedVoltage = 0.0.volts
var rollerSupplyCurrent = 0.0.amps
var rollerStatorCurrent = 0.0.amps
var rollerTemp = 0.0.celsius
var rollerSupplyCurrent = 0.0.amps
var rollerStatorCurrent = 0.0.amps
var rollerTemp = 0.0.celsius

override fun toLog(table: LogTable?) {
table?.put("rollerVelocityRPM", rollerVelocity.inRotationsPerMinute)
var armPosition = 0.0.degrees
var armVelocity = 0.0.degrees.perSecond
var armAbsoluteEncoderPosition = 0.0.degrees

table?.put("rollerAppliedVoltage", rollerAppliedVoltage.inVolts)
var armAppliedVoltage = 0.0.volts
var armSupplyCurrent = 0.0.amps
var armStatorCurrent = 0.0.amps
var armTemp = 0.0.celsius

table?.put("rollerSupplyCurrentAmps", rollerSupplyCurrent.inAmperes)
override fun toLog(table: LogTable?) {
table?.put("armPositionDegrees", armPosition.inDegrees)
table?.put("armAbsoluteEncoderPositionDegrees", armAbsoluteEncoderPosition.inDegrees)
table?.put("armVelocityDegreesPerSec", armVelocity.inDegreesPerSecond)
table?.put("armAppliedVoltage", armAppliedVoltage.inVolts)
table?.put("armSupplyCurrentAmps", armSupplyCurrent.inAmperes)
table?.put("armStatorCurrentAmps", armStatorCurrent.inAmperes)
table?.put("armTempCelsius", armTemp.inCelsius)

table?.put("rollerStatorCurrentAmps", rollerStatorCurrent.inAmperes)
table?.put("rollerVelocityRPM", rollerVelocity.inRotationsPerMinute)
table?.put("rollerAppliedVoltage", rollerAppliedVoltage.inVolts)
table?.put("rollerSupplyCurrentAmps", rollerSupplyCurrent.inAmperes)
table?.put("rollerStatorCurrentAmps", rollerStatorCurrent.inAmperes)
table?.put("rollerTempCelsius", rollerTemp.inCelsius)
}

override fun fromLog(table: LogTable?) {
table?.get("armPositionDegrees", armPosition.inDegrees)?.let { armPosition = it.degrees }

table?.get("armAbsoluteEncoderPositionDegrees", armAbsoluteEncoderPosition.inDegrees)?.let {
armAbsoluteEncoderPosition = it.degrees
}

table?.get("armVelocityDegreesPerSec", armVelocity.inDegreesPerSecond)?.let {
armVelocity = it.degrees.perSecond
}

table?.get("armAppliedVoltage", armAppliedVoltage.inVolts)?.let {
armAppliedVoltage = it.volts
}

table?.get("armSupplyCurrentAmps", armSupplyCurrent.inAmperes)?.let {
armSupplyCurrent = it.amps
}

table?.get("armStatorCurrentAmps", armStatorCurrent.inAmperes)?.let {
armStatorCurrent = it.amps
}

table?.get("armTempCelsius", armTemp.inCelsius)?.let { armTemp = it.celsius }

table?.get("rollerVelocityRPM", rollerVelocity.inRotationsPerMinute)?.let {
rollerVelocity = it.rotations.perSecond
}

table?.get("rollerAppliedVoltage", rollerAppliedVoltage.inVolts)?.let {
rollerAppliedVoltage = it.volts
}

table?.put("rollerTempCelsius", rollerTemp.inCelsius)
}
table?.get("rollerSupplyCurrentAmps", rollerSupplyCurrent.inAmperes)?.let {
rollerSupplyCurrent = it.amps
}

table?.get("rollerStatorCurrentAmps", rollerStatorCurrent.inAmperes)?.let {
rollerStatorCurrent = it.amps
}

table?.get("rollerTempCelsius", rollerTemp.inCelsius)?.let { rollerTemp = it.celsius }
}
}
}

fun updateInputs(io: IntakeIOInputs) {}

fun setRollerVoltage(voltage: ElectricalPotential) {}

fun setArmVoltage(voltage: ElectricalPotential) {}

fun setArmPosition(armPosition: Angle, feedforward: ElectricalPotential) {}

/**
* Updates the PID constants using the implementation controller
*
* @param kP accounts for linear error
* @param kI accounts for integral error
* @param kD accounts for derivative error
*/
fun configPID(
kP: ProportionalGain<Radian, Volt>,
kI: IntegralGain<Radian, Volt>,
kD: DerivativeGain<Radian, Volt>
) {}

/** Sets the current encoder position to be the zero value */
fun zeroEncoder() {}

fun setRollerBrakeMode(brake: Boolean) {}

fun setArmBrakeMode(brake: Boolean)
}
Loading