Added constants from pyhton to c++. kSpringConstant and kMaxExtension are in python
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;
}