Port y2014 bot3 to new control system.
Change-Id: I894277089335e36ea95b52e033056b1a8fb4ca30
diff --git a/y2014_bot3/control_loops/python/drivetrain.py b/y2014_bot3/control_loops/python/drivetrain.py
index bc0df04..c0c4595 100755
--- a/y2014_bot3/control_loops/python/drivetrain.py
+++ b/y2014_bot3/control_loops/python/drivetrain.py
@@ -56,30 +56,33 @@
# Stall Torque in N m
self.stall_torque = 2.42
# Stall Current in Amps
- self.stall_current = 133
+ self.stall_current = 133.0
# Free Speed in RPM. Used number from last year.
self.free_speed = 4650.0
# Free Current in Amps
self.free_current = 2.7
# Moment of inertia of the drivetrain in kg m^2
# Just borrowed from last year.
- self.J = 4.5
+ self.J = 10
# Mass of the robot, in kg.
- self.m = 60
+ self.m = 68
# Radius of the robot, in meters (from last year).
- self.rb = 0.7112 / 2.0
+ self.rb = 0.9603 / 2.0
# Radius of the wheels, in meters.
- self.r = .04445
+ self.r = 0.0508
# Resistance of the motor, divided by the number of motors.
- self.R = 12.0 / self.stall_current / 4
+ self.R = 12.0 / self.stall_current / 2
# Motor velocity constant
self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
(12.0 - self.R * self.free_current))
# Torque constant
self.Kt = self.stall_torque / self.stall_current
# Gear ratios
- self.G_low = 14.0 / 60.0 * 17.0 / 50.0
- self.G_high = 30.0 / 44.0 * 17.0 / 50.0
+ self.G_const = 18.0 / 44.0 * 18.0 / 60.0
+
+ self.G_low = self.G_const
+ self.G_high = self.G_const
+
if left_low:
self.Gl = self.G_low
else:
@@ -88,8 +91,9 @@
self.Gr = self.G_low
else:
self.Gr = self.G_high
+
# Control loop time step
- self.dt = 0.01
+ self.dt = 0.005
# These describe the way that a given side of a robot will be influenced
# by the other side. Units of 1 / kg.
@@ -223,13 +227,13 @@
drivetrain_high_low = Drivetrain(name="DrivetrainHighLow", left_low=False, right_low=True)
drivetrain_high_high = Drivetrain(name="DrivetrainHighHigh", left_low=False, right_low=False)
- if len(argv) != 3:
+ if len(argv) != 5:
print "Expected .h file name and .cc file name"
else:
dog_loop_writer = control_loop.ControlLoopWriter(
"Drivetrain", [drivetrain_low_low, drivetrain_low_high,
drivetrain_high_low, drivetrain_high_high],
- namespaces = ["bot3", "control_loops"])
+ namespaces=['y2014_bot3', 'control_loops'])
if argv[1][-3:] == '.cc':
dog_loop_writer.Write(argv[2], argv[1])
else:
diff --git a/y2014_bot3/control_loops/python/polydrivetrain.py b/y2014_bot3/control_loops/python/polydrivetrain.py
index 49d390b..2ffbed6 100755
--- a/y2014_bot3/control_loops/python/polydrivetrain.py
+++ b/y2014_bot3/control_loops/python/polydrivetrain.py
@@ -102,7 +102,7 @@
super(VelocityDrivetrainModel, self).__init__(name)
self._drivetrain = drivetrain.Drivetrain(left_low=left_low,
right_low=right_low)
- self.dt = 0.01
+ self.dt = 0.005
self.A_continuous = numpy.matrix(
[[self._drivetrain.A_continuous[1, 1], self._drivetrain.A_continuous[1, 3]],
[self._drivetrain.A_continuous[3, 1], self._drivetrain.A_continuous[3, 3]]])
@@ -168,7 +168,7 @@
[[-12.0000000000],
[-12.0000000000]])
- self.dt = 0.01
+ self.dt = 0.005
self.R = numpy.matrix(
[[0.0],
@@ -177,7 +177,7 @@
# ttrust is the comprimise between having full throttle negative inertia,
# and having no throttle negative inertia. A value of 0 is full throttle
# inertia. A value of 1 is no throttle negative inertia.
- self.ttrust = 1.0
+ self.ttrust = 1.1
self.left_gear = VelocityDrivetrain.LOW
self.right_gear = VelocityDrivetrain.LOW
@@ -302,26 +302,30 @@
# This is the same as sending the most torque down to the floor at the
# wheel.
- self.left_gear = self.ComputeGear(self.X[0, 0], should_print=True,
- current_gear=self.left_gear,
- gear_name="left")
- self.right_gear = self.ComputeGear(self.X[1, 0], should_print=True,
- current_gear=self.right_gear,
- gear_name="right")
- if self.IsInGear(self.left_gear):
- self.left_cim.X[0, 0] = self.MotorRPM(self.left_shifter_position, self.X[0, 0])
+ self.left_gear = self.right_gear = True
+ if False:
+ self.left_gear = self.ComputeGear(self.X[0, 0], should_print=True,
+ current_gear=self.left_gear,
+ gear_name="left")
+ self.right_gear = self.ComputeGear(self.X[1, 0], should_print=True,
+ current_gear=self.right_gear,
+ gear_name="right")
+ if self.IsInGear(self.left_gear):
+ self.left_cim.X[0, 0] = self.MotorRPM(self.left_shifter_position, self.X[0, 0])
- if self.IsInGear(self.right_gear):
- self.right_cim.X[0, 0] = self.MotorRPM(self.right_shifter_position, self.X[0, 0])
+ if self.IsInGear(self.right_gear):
+ self.right_cim.X[0, 0] = self.MotorRPM(self.right_shifter_position, self.X[0, 0])
- if self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
+ steering *= 2.3
+ if True or self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
# Filter the throttle to provide a nicer response.
fvel = self.FilterVelocity(throttle)
# Constant radius means that angualar_velocity / linear_velocity = constant.
# Compute the left and right velocities.
- left_velocity = fvel - steering * numpy.abs(fvel)
- right_velocity = fvel + steering * numpy.abs(fvel)
+ steering_velocity = numpy.abs(fvel) * steering
+ left_velocity = fvel - steering_velocity
+ right_velocity = fvel + steering_velocity
# Write this constraint in the form of K * R = w
# angular velocity / linear velocity = constant
@@ -379,7 +383,7 @@
# TODO(austin): Model the robot as not accelerating when you shift...
# This hack only works when you shift at the same time.
- if self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
+ if True or self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
self.X = self.CurrentDrivetrain().A * self.X + self.CurrentDrivetrain().B * self.U
self.left_gear, self.left_shifter_position = self.SimShifter(
@@ -394,7 +398,7 @@
def main(argv):
vdrivetrain = VelocityDrivetrain()
- if len(argv) != 5:
+ if len(argv) != 7:
print "Expected .h file name and .cc file name"
else:
dog_loop_writer = control_loop.ControlLoopWriter(
@@ -402,7 +406,7 @@
vdrivetrain.drivetrain_low_high,
vdrivetrain.drivetrain_high_low,
vdrivetrain.drivetrain_high_high],
- namespaces = ["bot3", "control_loops"])
+ namespaces=['y2014_bot3', 'control_loops'])
if argv[1][-3:] == '.cc':
dog_loop_writer.Write(argv[2], argv[1])
@@ -410,12 +414,13 @@
dog_loop_writer.Write(argv[1], argv[2])
cim_writer = control_loop.ControlLoopWriter(
- "CIM", [drivetrain.CIM()])
+ "CIM", [drivetrain.CIM()],
+ namespaces=['y2014_bot3', 'control_loops'])
- if argv[3][-3:] == '.cc':
- cim_writer.Write(argv[4], argv[3])
+ if argv[5][-3:] == '.cc':
+ cim_writer.Write(argv[6], argv[5])
else:
- cim_writer.Write(argv[3], argv[4])
+ cim_writer.Write(argv[5], argv[6])
return
vl_plot = []
@@ -442,13 +447,13 @@
else:
print "Right is low"
- for t in numpy.arange(0, 4.0, vdrivetrain.dt):
- if t < 1.0:
- vdrivetrain.Update(throttle=1.00, steering=0.0)
+ for t in numpy.arange(0, 1.7, vdrivetrain.dt):
+ if t < 0.5:
+ vdrivetrain.Update(throttle=0.00, steering=1.0)
elif t < 1.2:
- vdrivetrain.Update(throttle=1.00, steering=0.0)
+ vdrivetrain.Update(throttle=0.5, steering=1.0)
else:
- vdrivetrain.Update(throttle=1.00, steering=0.0)
+ vdrivetrain.Update(throttle=0.00, steering=1.0)
t_plot.append(t)
vl_plot.append(vdrivetrain.X[0, 0])
vr_plot.append(vdrivetrain.X[1, 0])
@@ -475,10 +480,10 @@
cim_velocity_plot.append(cim.X[0, 0])
cim_voltage_plot.append(U[0, 0] * 10)
cim_time.append(t)
- #pylab.plot(cim_time, cim_velocity_plot, label='cim spinup')
- #pylab.plot(cim_time, cim_voltage_plot, label='cim voltage')
- #pylab.legend()
- #pylab.show()
+ pylab.plot(cim_time, cim_velocity_plot, label='cim spinup')
+ pylab.plot(cim_time, cim_voltage_plot, label='cim voltage')
+ pylab.legend()
+ pylab.show()
# TODO(austin):
# Shifting compensation.