From 8be6d3c9a31e91329b23f320f1695d75cce6f896 Mon Sep 17 00:00:00 2001 From: Suave101 <57422635+Suave101@users.noreply.github.com> Date: Fri, 5 Jan 2024 02:37:32 -0600 Subject: [PATCH 1/4] added StateSpaceFlywheel --- StateSpaceElevator/robot.py | 156 ++++++++++++++++++++++++++++++++++++ 1 file changed, 156 insertions(+) create mode 100644 StateSpaceElevator/robot.py diff --git a/StateSpaceElevator/robot.py b/StateSpaceElevator/robot.py new file mode 100644 index 0000000..2eb1652 --- /dev/null +++ b/StateSpaceElevator/robot.py @@ -0,0 +1,156 @@ +#!/usr/bin/env python3 +# +# Copyright (c) FIRST and other WPILib contributors. +# Open Source Software; you can modify and/or share it under the terms of +# the WPILib BSD license file in the root directory of this project. +# + + +import wpilib +import wpimath.estimator +import wpimath.system +import wpimath.trajectory +import wpimath.units +import wpimath.system.plant +import wpimath.controller +import math + +""" +This is a sample program to demonstrate how to use a state-space controller to control an +elevator. +""" + + +class MyRobot(wpilib.TimedRobot): + def robotInit(self): + self.kMotorPort = 0 + self.kEncoderAChannel = 0 + self.kEncoderBChannel = 1 + self.kJoystickPort = 0 + self.kHighGoalPosition = wpimath.units.feetToMeters(3) + self.kLowGoalPosition = wpimath.units.feetToMeters(0) + + self.kCarriageMass = 4.5 # kilograms + + # A 1.5in diameter drum has a radius of 0.75in, or 0.019in. + self.kDrumRadius = 1.5 / 2.0 * 25.4 / 1000.0 + + # Reduction between motors and encoder, as output over input. If the elevator spins slower than + # the motors, this number should be greater than one. + self.kElevatorGearing = 6.0 + + self.profile = wpimath.trajectory.TrapezoidProfile( + wpimath.trajectory.TrapezoidProfile.Constraints( + wpimath.units.feetToMeters(3.0), + wpimath.units.feetToMeters(6.0), # Max elevator speed and acceleration. + ) + ) + + self.lastProfiledReference = wpimath.trajectory.TrapezoidProfile.State() + + # The plant holds a state-space model of our elevator. This system has the following properties: + # + # States: [position, velocity], in meters and meters per second. + # Inputs (what we can "put in"): [voltage], in volts. + # Outputs (what we can measure): [position], in meters. + # + # This elevator is driven by two NEO motors. + self.elevatorPlant = wpimath.system.LinearSystemId.elevatorSystem( + wpimath.system.plant.DCMotor.NEO(2), + self.kCarriageMass, + self.kDrumRadius, + self.kElevatorGearing, + ) + + # The observer fuses our encoder data and voltage inputs to reject noise. + self.observer = wpimath.estimator.KalmanFilter_2_1_1( + self.elevatorPlant, + ( + wpimath.units.inchesToMeters(2), + wpimath.units.inchesToMeters(40), + ), # How accurate we think our + # model is, in meters and meters/second. + ( + 0.001, + ), # How accurate we think our encoder position data is. In this case we very + # highly trust our encoder position reading. + 0.020, + ) + + # A LQR uses feedback to create voltage commands. + self.controller = wpimath.controller.LinearQuadraticRegulator_2_1( + self.elevatorPlant, + ( + wpimath.units.inchesToMeters(1.0), + wpimath.units.inchesToMeters(10.0), + ), # qelms. Position and velocity + # error tolerances, in meters and meters per second. Decrease this to more heavily penalize state excursion, + # or make the controller behave more aggressively. In this example we weight position much more highly than + # velocity, but this can be tuned to balance the two. + ( + 12.0, + ), # relms. Control effort (voltage) tolerance. Decrease this to more heavily penalize control + # effort, or make the controller less aggressive. 12 is a good starting point because that is the + # (approximate) maximum voltage of a battery. + 0.020, + # Nominal time between loops. 0.020 for TimedRobot, but can be lower if using notifiers. + ) + + # The state-space loop combines a controller, observer, feedforward and plant for easy control. + self.loop = wpimath.system.LinearSystemLoop_2_1_1( + self.elevatorPlant, self.controller, self.observer, 12.0, 0.020 + ) + + # An encoder set up to measure elevator height in meters + self.encoder = wpilib.Encoder(self.kEncoderAChannel, self.kEncoderBChannel) + + self.motor = wpilib.PWMSparkMax(self.kMotorPort) + + # A joystick to read the trigger + self.joystick = wpilib.Joystick(self.kJoystickPort) + + # Circumference = pi * d, so distance per click = pi * d / counts + self.encoder.setDistancePerPulse(math.pi * 2 * self.kDrumRadius / 4096.0) + + def teleopInit(self): + + # Reset our loop to make sure it's in a known state. + self.loop.reset((self.encoder.getDistance(), self.encoder.getRate())) + + # Reset our last reference to the current state. + self.lastProfiledReference = wpimath.trajectory.TrapezoidProfile.State( + self.encoder.getDistance(), self.encoder.getRate() + ) + + def teleopPeriodic(self): + if self.joystick.getTrigger(): + + # the trigger is pressed, so we go to the high goal. + goal = wpimath.trajectory.TrapezoidProfile.State( + self.kHighGoalPosition, 0.0 + ) + else: + + # Otherwise, we go to the low goal + goal = wpimath.trajectory.TrapezoidProfile.State(self.kLowGoalPosition, 0.0) + + # Step our TrapezoidalProfile forward 20ms and set it as our next reference + self.lastProfiledReference = self.profile.calculate(0.020) + self.loop.setNextR(self.lastProfiledReference.position) + + # Correct our Kalman filter's state vector estimate with encoder data. + self.loop.correct((self.encoder.getDistance())) + + # Update our LQR to generate new voltage commands and use the voltages to predict the next state with out + # Kalman filter. + self.loop.predict(0.020) + + # Send the new calculated voltage to the motors. + # voltage = duty cycle * battery voltage, so + # duty cycle = voltage / battery voltage + nextVoltage = self.loop.U(0) + self.motor.setVoltage(nextVoltage) + + +if __name__ == "__main__": + wpilib.run(MyRobot) From 5454ae6299933e8ba49ead79d288849132fd5fd0 Mon Sep 17 00:00:00 2001 From: Suave101 <57422635+Suave101@users.noreply.github.com> Date: Fri, 5 Jan 2024 02:40:45 -0600 Subject: [PATCH 2/4] Update run_tests.sh --- run_tests.sh | 1 + 1 file changed, 1 insertion(+) diff --git a/run_tests.sh b/run_tests.sh index f6e46d4..f05e304 100755 --- a/run_tests.sh +++ b/run_tests.sh @@ -44,6 +44,7 @@ BASE_TESTS=" ShuffleBoard Solenoid StatefulAutonomous + StateSpaceElevator StateSpaceFlywheel StateSpaceFlywheelSysId SwerveBot From 26c1175c681b97afff88435056c2eaeb3e60f703 Mon Sep 17 00:00:00 2001 From: Suave101 <57422635+Suave101@users.noreply.github.com> Date: Fri, 5 Jan 2024 02:42:08 -0600 Subject: [PATCH 3/4] Format Black --- StateSpaceElevator/robot.py | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/StateSpaceElevator/robot.py b/StateSpaceElevator/robot.py index 2eb1652..9ff7e57 100644 --- a/StateSpaceElevator/robot.py +++ b/StateSpaceElevator/robot.py @@ -33,10 +33,12 @@ def robotInit(self): self.kCarriageMass = 4.5 # kilograms # A 1.5in diameter drum has a radius of 0.75in, or 0.019in. + self.kDrumRadius = 1.5 / 2.0 * 25.4 / 1000.0 # Reduction between motors and encoder, as output over input. If the elevator spins slower than # the motors, this number should be greater than one. + self.kElevatorGearing = 6.0 self.profile = wpimath.trajectory.TrapezoidProfile( @@ -55,6 +57,7 @@ def robotInit(self): # Outputs (what we can measure): [position], in meters. # # This elevator is driven by two NEO motors. + self.elevatorPlant = wpimath.system.LinearSystemId.elevatorSystem( wpimath.system.plant.DCMotor.NEO(2), self.kCarriageMass, @@ -63,6 +66,7 @@ def robotInit(self): ) # The observer fuses our encoder data and voltage inputs to reject noise. + self.observer = wpimath.estimator.KalmanFilter_2_1_1( self.elevatorPlant, ( @@ -78,6 +82,7 @@ def robotInit(self): ) # A LQR uses feedback to create voltage commands. + self.controller = wpimath.controller.LinearQuadraticRegulator_2_1( self.elevatorPlant, ( @@ -97,27 +102,33 @@ def robotInit(self): ) # The state-space loop combines a controller, observer, feedforward and plant for easy control. + self.loop = wpimath.system.LinearSystemLoop_2_1_1( self.elevatorPlant, self.controller, self.observer, 12.0, 0.020 ) # An encoder set up to measure elevator height in meters + self.encoder = wpilib.Encoder(self.kEncoderAChannel, self.kEncoderBChannel) self.motor = wpilib.PWMSparkMax(self.kMotorPort) # A joystick to read the trigger + self.joystick = wpilib.Joystick(self.kJoystickPort) # Circumference = pi * d, so distance per click = pi * d / counts + self.encoder.setDistancePerPulse(math.pi * 2 * self.kDrumRadius / 4096.0) def teleopInit(self): # Reset our loop to make sure it's in a known state. + self.loop.reset((self.encoder.getDistance(), self.encoder.getRate())) # Reset our last reference to the current state. + self.lastProfiledReference = wpimath.trajectory.TrapezoidProfile.State( self.encoder.getDistance(), self.encoder.getRate() ) @@ -126,28 +137,33 @@ def teleopPeriodic(self): if self.joystick.getTrigger(): # the trigger is pressed, so we go to the high goal. + goal = wpimath.trajectory.TrapezoidProfile.State( self.kHighGoalPosition, 0.0 ) else: # Otherwise, we go to the low goal - goal = wpimath.trajectory.TrapezoidProfile.State(self.kLowGoalPosition, 0.0) + goal = wpimath.trajectory.TrapezoidProfile.State(self.kLowGoalPosition, 0.0) # Step our TrapezoidalProfile forward 20ms and set it as our next reference + self.lastProfiledReference = self.profile.calculate(0.020) self.loop.setNextR(self.lastProfiledReference.position) # Correct our Kalman filter's state vector estimate with encoder data. + self.loop.correct((self.encoder.getDistance())) # Update our LQR to generate new voltage commands and use the voltages to predict the next state with out # Kalman filter. + self.loop.predict(0.020) # Send the new calculated voltage to the motors. # voltage = duty cycle * battery voltage, so # duty cycle = voltage / battery voltage + nextVoltage = self.loop.U(0) self.motor.setVoltage(nextVoltage) From b4609d9ec181ef79bf67fecdb7eadb71a4de2dd1 Mon Sep 17 00:00:00 2001 From: Suave101 <57422635+Suave101@users.noreply.github.com> Date: Fri, 5 Jan 2024 02:47:02 -0600 Subject: [PATCH 4/4] Format Black v23.12.1... --- StateSpaceElevator/robot.py | 3 --- 1 file changed, 3 deletions(-) diff --git a/StateSpaceElevator/robot.py b/StateSpaceElevator/robot.py index 9ff7e57..4a0444c 100644 --- a/StateSpaceElevator/robot.py +++ b/StateSpaceElevator/robot.py @@ -122,7 +122,6 @@ def robotInit(self): self.encoder.setDistancePerPulse(math.pi * 2 * self.kDrumRadius / 4096.0) def teleopInit(self): - # Reset our loop to make sure it's in a known state. self.loop.reset((self.encoder.getDistance(), self.encoder.getRate())) @@ -135,14 +134,12 @@ def teleopInit(self): def teleopPeriodic(self): if self.joystick.getTrigger(): - # the trigger is pressed, so we go to the high goal. goal = wpimath.trajectory.TrapezoidProfile.State( self.kHighGoalPosition, 0.0 ) else: - # Otherwise, we go to the low goal goal = wpimath.trajectory.TrapezoidProfile.State(self.kLowGoalPosition, 0.0)