diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt index 4f1f2add..c5761a42 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt @@ -115,9 +115,8 @@ object Constants { const val PIGEON_2_ID = 1 } - object Manipulator { - const val INTAKE_MOTOR_ID = 51 - const val ARM_MOTOR_ID = 52 + object Shooter { + const val ROLLER_MOTOR_ID = 51 } object Alert { diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/ShooterConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/ShooterConstants.kt new file mode 100644 index 00000000..b9ff868c --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/ShooterConstants.kt @@ -0,0 +1,11 @@ +package com.team4099.robot2023.config.constants + +import org.team4099.lib.units.base.amps +import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.derived.gearRatio + +object ShooterConstants { + val ROLLER_GEAR_RATIO = 0.0.gearRatio + val ROLLER_VOLTAGE_COMPENSATION = 0.0.volts + val ROLLER_STATOR_CURRENT_LIMIT = 0.0.amps +} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt index 73c0b53d..99fecfb5 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt @@ -48,7 +48,9 @@ interface ShooterIO { } } - fun updateInputs(inputs: ShooterIOInputs) + fun updateInputs(inputs: ShooterIOInputs){ + + } fun setRollerPower (voltage: ElectricalPotential){ } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt index 6ee6352b..bd7db0bf 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt @@ -1,5 +1,43 @@ package com.team4099.robot2023.subsystems.Shooter -class ShooterIONeo { - val +import com.revrobotics.CANSparkMax +import com.revrobotics.CANSparkMaxLowLevel +import com.team4099.robot2023.config.constants.Constants +import com.team4099.robot2023.config.constants.ShooterConstants +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.derived.inVolts +import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.sparkMaxAngularMechanismSensor +import kotlin.math.absoluteValue + +object ShooterIONeo : ShooterIO{ + private val rollerSparkMax = CANSparkMax(Constants.Shooter.ROLLER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless) + private val rollerSensor = sparkMaxAngularMechanismSensor( rollerSparkMax, + ShooterConstants.ROLLER_GEAR_RATIO.asDrivenOverDriving, + ShooterConstants.ROLLER_VOLTAGE_COMPENSATION + ) + init{ + //reset the motors + rollerSparkMax.restoreFactoryDefaults() + rollerSparkMax.clearFaults() + + //voltage and current limits + rollerSparkMax.enableVoltageCompensation(ShooterConstants.ROLLER_VOLTAGE_COMPENSATION.inVolts) + rollerSparkMax.setSmartCurrentLimit(ShooterConstants.ROLLER_STATOR_CURRENT_LIMIT.inAmperes.toInt()) + } + + override fun updateInputs (inputs: ShooterIO.ShooterIOInputs){ + inputs.rollerVelocity = rollerSensor.velocity + inputs.rollerAppliedVoltage = rollerSparkMax.busVoltage.volts * rollerSparkMax.appliedOutput + inputs.rollerStatorCurrent = rollerSparkMax.outputCurrent.amps + // BatteryVoltage * SupplyCurrent = AppliedVoltage * StatorCurrent + // AppliedVoltage = percentOutput * BatteryVoltage + // SupplyCurrent = (percentOutput * BatteryVoltage / BatteryVoltage) * StatorCurrent = + // percentOutput * statorCurrent + inputs.rollerSupplyCurrent = inputs.rollerStatorCurrent * rollerSparkMax.appliedOutput.absoluteValue + inputs.rollerTempreature = rollerSparkMax.motorTemperature.celsius + } + } \ No newline at end of file