Added constants from pyhton to c++. kSpringConstant and kMaxExtension are in python
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 4b63aec..51ce0ff 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 const %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,11 @@
self._namespace_end = '\n'.join(
['} // namespace %s' % name for name in reversed(self._namespaces)])
+
+ self._constant_list = []
+ if (write_constants):
+ self._constant_list.append(Constant("kMaxExtension", "%f", 0.32385));
+ self._constant_list.append(Constant("kSpringConstant", "%f", 0.28));
def _TopDirectory(self):
return self._namespaces[0]
@@ -72,6 +90,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/shooter.py b/frc971/control_loops/python/shooter.py
index 89f682a..6b8c224 100755
--- a/frc971/control_loops/python/shooter.py
+++ b/frc971/control_loops/python/shooter.py
@@ -235,7 +235,9 @@
sprung_shooter = SprungShooterDeltaU()
shooter = ShooterDeltaU()
- loop_writer = control_loop.ControlLoopWriter("Shooter", [sprung_shooter, shooter])
+ loop_writer = control_loop.ControlLoopWriter("Shooter", [sprung_shooter,
+ shooter],
+ write_constants=True)
if argv[1][-3:] == '.cc':
loop_writer.Write(argv[2], argv[1])
else:
diff --git a/frc971/control_loops/shooter/shooter.cc b/frc971/control_loops/shooter/shooter.cc
index 41d6448..9cf563e 100755
--- a/frc971/control_loops/shooter/shooter.cc
+++ b/frc971/control_loops/shooter/shooter.cc
@@ -118,7 +118,8 @@
double ShooterMotor::PowerToPosition(double power) {
// LOG(WARNING, "power to position not correctly implemented\n");
const frc971::constants::Values &values = constants::GetValues();
- double new_pos = ::std::min(::std::max(power, values.shooter.lower_limit),
+ double new_pos = kMaxExtension - sqrt((power + power) / kSpringConstant);
+ new_pos = ::std::min(::std::max(power, values.shooter.lower_limit),
values.shooter.upper_limit);
return new_pos;
}
diff --git a/frc971/control_loops/shooter/shooter_motor_plant.h b/frc971/control_loops/shooter/shooter_motor_plant.h
old mode 100755
new mode 100644
index 968fd04..40a2f25
--- a/frc971/control_loops/shooter/shooter_motor_plant.h
+++ b/frc971/control_loops/shooter/shooter_motor_plant.h
@@ -5,6 +5,10 @@
namespace frc971 {
namespace control_loops {
+static const double kMaxExtension = 0.323850;
+
+static const double kSpringConstant = 0.280000;
+
StateFeedbackPlantCoefficients<3, 1, 1> MakeSprungShooterPlantCoefficients();
diff --git a/frc971/control_loops/shooter/unaugmented_shooter_motor_plant.h b/frc971/control_loops/shooter/unaugmented_shooter_motor_plant.h
old mode 100755
new mode 100644