Renamed everything to claw.
- Renamed all the wrists calls to claw.
- Added a top and bottom wrist controller.
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index bcd62f9..7e6159f 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -13,8 +13,8 @@
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/control_loops/drivetrain/polydrivetrain_cim_plant.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/queues/GyroAngle.q.h"
-#include "frc971/queues/Piston.q.h"
+#include "frc971/queues/gyro_angle.q.h"
+#include "frc971/queues/piston.q.h"
#include "frc971/constants.h"
using frc971::sensors::gyro;
@@ -569,155 +569,6 @@
constexpr double PolyDrivetrain::Kt;
-
-class DrivetrainMotorsOL {
- public:
- DrivetrainMotorsOL() {
- _old_wheel = 0.0;
- wheel_ = 0.0;
- throttle_ = 0.0;
- quickturn_ = false;
- highgear_ = true;
- _neg_inertia_accumulator = 0.0;
- _left_pwm = 0.0;
- _right_pwm = 0.0;
- }
- void SetGoal(double wheel, double throttle, bool quickturn, bool highgear) {
- wheel_ = wheel;
- throttle_ = throttle;
- quickturn_ = quickturn;
- highgear_ = highgear;
- _left_pwm = 0.0;
- _right_pwm = 0.0;
- }
- void Update() {
- double overPower;
- float sensitivity = 1.7;
- float angular_power;
- float linear_power;
- double wheel;
-
- double neg_inertia = wheel_ - _old_wheel;
- _old_wheel = wheel_;
-
- double wheelNonLinearity;
- if (highgear_) {
- wheelNonLinearity = 0.1; // used to be csvReader->TURN_NONLIN_HIGH
- // Apply a sin function that's scaled to make it feel better.
- const double angular_range = M_PI / 2.0 * wheelNonLinearity;
- wheel = sin(angular_range * wheel_) / sin(angular_range);
- wheel = sin(angular_range * wheel) / sin(angular_range);
- } else {
- wheelNonLinearity = 0.2; // used to be csvReader->TURN_NONLIN_LOW
- // Apply a sin function that's scaled to make it feel better.
- const double angular_range = M_PI / 2.0 * wheelNonLinearity;
- wheel = sin(angular_range * wheel_) / sin(angular_range);
- wheel = sin(angular_range * wheel) / sin(angular_range);
- wheel = sin(angular_range * wheel) / sin(angular_range);
- }
-
- static const double kThrottleDeadband = 0.05;
- if (::std::abs(throttle_) < kThrottleDeadband) {
- throttle_ = 0;
- } else {
- throttle_ = copysign((::std::abs(throttle_) - kThrottleDeadband) /
- (1.0 - kThrottleDeadband), throttle_);
- }
-
- double neg_inertia_scalar;
- if (highgear_) {
- neg_inertia_scalar = 8.0; // used to be csvReader->NEG_INTERTIA_HIGH
- sensitivity = 1.22; // used to be csvReader->SENSE_HIGH
- } else {
- if (wheel * neg_inertia > 0) {
- neg_inertia_scalar = 5; // used to be csvReader->NEG_INERTIA_LOW_MORE
- } else {
- if (::std::abs(wheel) > 0.65) {
- neg_inertia_scalar = 5; // used to be csvReader->NEG_INTERTIA_LOW_LESS_EXT
- } else {
- neg_inertia_scalar = 5; // used to be csvReader->NEG_INTERTIA_LOW_LESS
- }
- }
- sensitivity = 1.24; // used to be csvReader->SENSE_LOW
- }
- double neg_inertia_power = neg_inertia * neg_inertia_scalar;
- _neg_inertia_accumulator += neg_inertia_power;
-
- wheel = wheel + _neg_inertia_accumulator;
- if (_neg_inertia_accumulator > 1) {
- _neg_inertia_accumulator -= 1;
- } else if (_neg_inertia_accumulator < -1) {
- _neg_inertia_accumulator += 1;
- } else {
- _neg_inertia_accumulator = 0;
- }
-
- linear_power = throttle_;
-
- if (quickturn_) {
- double qt_angular_power = wheel;
- if (::std::abs(linear_power) < 0.2) {
- if (qt_angular_power > 1) qt_angular_power = 1.0;
- if (qt_angular_power < -1) qt_angular_power = -1.0;
- } else {
- qt_angular_power = 0.0;
- }
- overPower = 1.0;
- if (highgear_) {
- sensitivity = 1.0;
- } else {
- sensitivity = 1.0;
- }
- angular_power = wheel;
- } else {
- overPower = 0.0;
- angular_power = ::std::abs(throttle_) * wheel * sensitivity;
- }
-
- _right_pwm = _left_pwm = linear_power;
- _left_pwm += angular_power;
- _right_pwm -= angular_power;
-
- if (_left_pwm > 1.0) {
- _right_pwm -= overPower*(_left_pwm - 1.0);
- _left_pwm = 1.0;
- } else if (_right_pwm > 1.0) {
- _left_pwm -= overPower*(_right_pwm - 1.0);
- _right_pwm = 1.0;
- } else if (_left_pwm < -1.0) {
- _right_pwm += overPower*(-1.0 - _left_pwm);
- _left_pwm = -1.0;
- } else if (_right_pwm < -1.0) {
- _left_pwm += overPower*(-1.0 - _right_pwm);
- _right_pwm = -1.0;
- }
- }
-
- void SendMotors(Drivetrain::Output *output) {
- LOG(DEBUG, "left pwm: %f right pwm: %f wheel: %f throttle: %f\n",
- _left_pwm, _right_pwm, wheel_, throttle_);
- if (output) {
- output->left_voltage = _left_pwm * 12.0;
- output->right_voltage = _right_pwm * 12.0;
- }
- if (highgear_) {
- shifters.MakeWithBuilder().set(false).Send();
- } else {
- shifters.MakeWithBuilder().set(true).Send();
- }
- }
-
- private:
- double _old_wheel;
- double wheel_;
- double throttle_;
- bool quickturn_;
- bool highgear_;
- double _neg_inertia_accumulator;
- double _left_pwm;
- double _right_pwm;
-};
-
void DrivetrainLoop::RunIteration(const Drivetrain::Goal *goal,
const Drivetrain::Position *position,
Drivetrain::Output *output,