Merged in the stuff most recently on the robot.
diff --git a/frc971/control_loops/python/claw.py b/frc971/control_loops/python/claw.py
index aacf31e..ca69a2b 100755
--- a/frc971/control_loops/python/claw.py
+++ b/frc971/control_loops/python/claw.py
@@ -23,6 +23,7 @@
# measured from CAD
self.J_top = 0.3
self.J_bottom = 0.9
+
# Resistance of the motor
self.R = 12.0 / self.stall_current
# Motor velocity constant
@@ -144,8 +145,8 @@
print "eigenvalues"
print numpy.linalg.eig(F)[0]
- self.rpl = .05
- self.ipl = 0.008
+ self.rpl = .02
+ self.ipl = 0.004
self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
self.rpl + 1j * self.ipl,
self.rpl - 1j * self.ipl,
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 90faf9f..a103c79 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -1,8 +1,21 @@
import controls
import numpy
+class Constant(object):
+ def __init__ (self, name, formatt, value):
+ self.name = name
+ self.formatt = formatt
+ self.value = value
+ self.formatToType = {}
+ self.formatToType['%f'] = "double";
+ self.formatToType['%d'] = "int";
+ def __str__ (self):
+ return str("\nstatic constexpr %s %s = "+ self.formatt +";\n") % \
+ (self.formatToType[self.formatt], self.name, self.value)
+
+
class ControlLoopWriter(object):
- def __init__(self, gain_schedule_name, loops, namespaces=None):
+ def __init__(self, gain_schedule_name, loops, namespaces=None, write_constants=False):
"""Constructs a control loop writer.
Args:
@@ -24,6 +37,16 @@
self._namespace_end = '\n'.join(
['} // namespace %s' % name for name in reversed(self._namespaces)])
+
+ self._constant_list = []
+
+ def AddConstant(self, constant):
+ """Adds a constant to write.
+
+ Args:
+ constant: Constant, the constant to add to the header.
+ """
+ self._constant_list.append(constant)
def _TopDirectory(self):
return self._namespaces[0]
@@ -74,6 +97,10 @@
fd.write('\n')
fd.write(self._namespace_start)
+
+ for const in self._constant_list:
+ fd.write(str(const))
+
fd.write('\n\n')
for loop in self._loops:
fd.write(loop.DumpPlantHeader())
diff --git a/frc971/control_loops/python/drivetrain.py b/frc971/control_loops/python/drivetrain.py
index 001fd1e..3d6e441 100755
--- a/frc971/control_loops/python/drivetrain.py
+++ b/frc971/control_loops/python/drivetrain.py
@@ -1,6 +1,7 @@
#!/usr/bin/python
import control_loop
+import controls
import numpy
import sys
from matplotlib import pylab
@@ -89,7 +90,7 @@
self.Gr = self.G_high
# Control loop time step
self.dt = 0.01
-
+
# These describe the way that a given side of a robot will be influenced
# by the other side. Units of 1 / kg.
self.msp = 1.0 / self.m + self.rb * self.rb / self.J
@@ -118,13 +119,29 @@
self.D = numpy.matrix([[0, 0],
[0, 0]])
+ #print "THE NUMBER I WANT" + str(numpy.linalg.inv(self.A_continuous) * -self.B_continuous * numpy.matrix([[12.0], [12.0]]))
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
# Poles from last year.
self.hp = 0.65
self.lp = 0.83
- self.PlaceControllerPoles([self.hp, self.hp, self.lp, self.lp])
+ self.PlaceControllerPoles([self.hp, self.lp, self.hp, self.lp])
+ print self.K
+ q_pos = 0.07
+ q_vel = 1.0
+ self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0, 0.0, 0.0],
+ [0.0, (1.0 / (q_vel ** 2.0)), 0.0, 0.0],
+ [0.0, 0.0, (1.0 / (q_pos ** 2.0)), 0.0],
+ [0.0, 0.0, 0.0, (1.0 / (q_vel ** 2.0))]])
+
+ self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0)), 0.0],
+ [0.0, (1.0 / (12.0 ** 2.0))]])
+ self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
+ print self.A
+ print self.B
+ print self.K
+ print numpy.linalg.eig(self.A - self.B * self.K)[0]
self.hlp = 0.07
self.llp = 0.09
@@ -200,6 +217,7 @@
#pylab.show()
# Write the generated constants out to a file.
+ print "Output one"
drivetrain = Drivetrain()
if len(argv) != 5:
diff --git a/frc971/control_loops/python/polydrivetrain.py b/frc971/control_loops/python/polydrivetrain.py
index 280db16..f2dfdbe 100755
--- a/frc971/control_loops/python/polydrivetrain.py
+++ b/frc971/control_loops/python/polydrivetrain.py
@@ -396,10 +396,10 @@
print "Expected .h file name and .cc file name"
else:
dog_loop_writer = control_loop.ControlLoopWriter(
- "VDogDrivetrain", [vdrivetrain.drivetrain_low_low,
- vdrivetrain.drivetrain_low_high,
- vdrivetrain.drivetrain_high_low,
- vdrivetrain.drivetrain_high_high])
+ "VelocityDrivetrain", [vdrivetrain.drivetrain_low_low,
+ vdrivetrain.drivetrain_low_high,
+ vdrivetrain.drivetrain_high_low,
+ vdrivetrain.drivetrain_high_high])
if argv[1][-3:] == '.cc':
dog_loop_writer.Write(argv[2], argv[1])
diff --git a/frc971/control_loops/python/shooter.py b/frc971/control_loops/python/shooter.py
index 89f682a..69f2599 100755
--- a/frc971/control_loops/python/shooter.py
+++ b/frc971/control_loops/python/shooter.py
@@ -30,6 +30,9 @@
self.Kt = self.stall_torque / self.stall_current
# Spring constant for the springs, N/m
self.Ks = 2800.0
+ # Maximum extension distance (Distance from the 0 force point on the
+ # spring to the latch position.)
+ self.max_extension = 0.32385
# Gear ratio multiplied by radius of final sprocket.
self.G = 10.0 / 40.0 * 20.0 / 54.0 * 24.0 / 54.0 * 20.0 / 84.0 * 16.0 * (3.0 / 8.0) / (2.0 * numpy.pi) * 0.0254
@@ -235,7 +238,13 @@
sprung_shooter = SprungShooterDeltaU()
shooter = ShooterDeltaU()
- loop_writer = control_loop.ControlLoopWriter("Shooter", [sprung_shooter, shooter])
+ loop_writer = control_loop.ControlLoopWriter("Shooter", [sprung_shooter,
+ shooter])
+
+ loop_writer.AddConstant(control_loop.Constant("kMaxExtension", "%f",
+ sprung_shooter.max_extension))
+ loop_writer.AddConstant(control_loop.Constant("kSpringConstant", "%f",
+ sprung_shooter.Ks))
if argv[1][-3:] == '.cc':
loop_writer.Write(argv[2], argv[1])
else: