| Philipp Schrader | 9ffe298 | 2016-12-07 20:51:08 -0800 | [diff] [blame] | 1 | #!/usr/bin/python |
| 2 | |
| 3 | from frc971.control_loops.python import control_loop |
| 4 | import numpy |
| 5 | |
| 6 | class CIM(control_loop.ControlLoop): |
| 7 | def __init__(self): |
| 8 | super(CIM, self).__init__("CIM") |
| 9 | # Stall Torque in N m |
| 10 | self.stall_torque = 2.42 |
| 11 | # Stall Current in Amps |
| 12 | self.stall_current = 133 |
| 13 | # Free Speed in RPM |
| 14 | self.free_speed = 4650.0 |
| 15 | # Free Current in Amps |
| 16 | self.free_current = 2.7 |
| 17 | # Moment of inertia of the CIM in kg m^2 |
| 18 | self.J = 0.0001 |
| 19 | # Resistance of the motor, divided by 2 to account for the 2 motors |
| 20 | self.resistance = 12.0 / self.stall_current |
| 21 | # Motor velocity constant |
| 22 | self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) / |
| 23 | (12.0 - self.resistance * self.free_current)) |
| 24 | # Torque constant |
| 25 | self.Kt = self.stall_torque / self.stall_current |
| 26 | # Control loop time step |
| 27 | self.dt = 0.005 |
| 28 | |
| 29 | # State feedback matrices |
| 30 | self.A_continuous = numpy.matrix( |
| 31 | [[-self.Kt / self.Kv / (self.J * self.resistance)]]) |
| 32 | self.B_continuous = numpy.matrix( |
| 33 | [[self.Kt / (self.J * self.resistance)]]) |
| 34 | self.C = numpy.matrix([[1]]) |
| 35 | self.D = numpy.matrix([[0]]) |
| 36 | |
| 37 | self.A, self.B = self.ContinuousToDiscrete(self.A_continuous, |
| 38 | self.B_continuous, self.dt) |
| 39 | |
| 40 | self.PlaceControllerPoles([0.01]) |
| 41 | self.PlaceObserverPoles([0.01]) |
| 42 | |
| 43 | self.U_max = numpy.matrix([[12.0]]) |
| 44 | self.U_min = numpy.matrix([[-12.0]]) |
| 45 | |
| 46 | self.InitializeState() |