14
14
15
15
16
16
class MyRobot (wpilib .TimedRobot ):
17
-
18
17
MOTOR_PORT = 0
19
18
ENCODER_A_CHANNEL = 0
20
19
ENCODER_B_CHANNEL = 1
@@ -32,12 +31,18 @@ class MyRobot(wpilib.TimedRobot):
32
31
FLYWHEEL_GEARING = 1.0
33
32
34
33
# 1/2 MR²
35
- FLYWHEEL_MOMENT_OF_INERTIA = 0.5 * wpimath .units .lbsToKilograms (1.5 ) * math .pow (wpimath .units .inchesToMeters (4 ), 2 )
34
+ FLYWHEEL_MOMENT_OF_INERTIA = (
35
+ 0.5
36
+ * wpimath .units .lbsToKilograms (1.5 )
37
+ * math .pow (wpimath .units .inchesToMeters (4 ), 2 )
38
+ )
36
39
37
40
def robotInit (self ):
38
41
"""Robot initialization function"""
39
42
40
- self .feedforward = wpimath .controller .SimpleMotorFeedforwardMeters (self .FLYWHEEL_KS , self .FLYWHEEL_KV , self .FLYWHEEL_KA )
43
+ self .feedforward = wpimath .controller .SimpleMotorFeedforwardMeters (
44
+ self .FLYWHEEL_KS , self .FLYWHEEL_KV , self .FLYWHEEL_KA
45
+ )
41
46
42
47
# Joystick to control setpoint
43
48
self .joystick = wpilib .Joystick (0 )
@@ -48,7 +53,9 @@ def robotInit(self):
48
53
self .bangBangControler = wpimath .controller .BangBangController ()
49
54
50
55
# Simulation classes help us simulate our robot
51
- self .flywheelSim = wpilib .simulation .FlywheelSim (DCMotor .NEO (1 ), self .FLYWHEEL_GEARING , self .FLYWHEEL_MOMENT_OF_INERTIA );
56
+ self .flywheelSim = wpilib .simulation .FlywheelSim (
57
+ DCMotor .NEO (1 ), self .FLYWHEEL_GEARING , self .FLYWHEEL_MOMENT_OF_INERTIA
58
+ )
52
59
self .encoderSim = wpilib .simulation .EncoderSim (self .encoder )
53
60
54
61
# Add bang-bang controler to SmartDashboard and networktables.
@@ -59,24 +66,33 @@ def teleopPeriodic(self):
59
66
60
67
# Scale setpoint value between 0 and maxSetpointValue
61
68
setpoint = max (
62
- 0.0 ,
63
- self .joystick .getRawAxis (0 ) * wpimath .units .rotationsPerMinuteToRadiansPerSecond (self .MAX_SETPOINT_VALUE )
69
+ 0.0 ,
70
+ self .joystick .getRawAxis (0 )
71
+ * wpimath .units .rotationsPerMinuteToRadiansPerSecond (
72
+ self .MAX_SETPOINT_VALUE
73
+ ),
64
74
)
65
75
66
76
# Set setpoint and measurement of the bang-bang controller
67
- bangOutput = self .bangBangControler .calculate (self .encoder .getRate (), setpoint ) * 12.0
77
+ bangOutput = (
78
+ self .bangBangControler .calculate (self .encoder .getRate (), setpoint ) * 12.0
79
+ )
68
80
69
81
# Controls a motor with the output of the BangBang controller and a
70
82
# feedforward. The feedforward is reduced slightly to avoid overspeeding
71
83
# the shooter.
72
- self .flywheelMotor .setVoltage (bangOutput + 0.9 * self .feedforward .calculate (setpoint ))
84
+ self .flywheelMotor .setVoltage (
85
+ bangOutput + 0.9 * self .feedforward .calculate (setpoint )
86
+ )
73
87
74
88
def _simulationPeriodic (self ):
75
89
"""Update our simulation. This should be run every robot loop in simulation."""
76
90
77
91
# To update our simulation, we set motor voltage inputs, update the
78
92
# simulation, and write the simulated velocities to our simulated encoder
79
- self .flywheelSim .setInputVoltage (self .flywheelMotor .get () * wpilib .RobotController .getInputVoltage ())
93
+ self .flywheelSim .setInputVoltage (
94
+ self .flywheelMotor .get () * wpilib .RobotController .getInputVoltage ()
95
+ )
80
96
self .flywheelSim .update (0.02 )
81
97
self .encoderSim .setRate (self .flywheelSim .getAngularVelocity ())
82
98
0 commit comments