Added seperate on and off constants for the wrist and angle adjust.
diff --git a/frc971/constants.cpp b/frc971/constants.cpp
index 57e53fa..36d8b31 100644
--- a/frc971/constants.cpp
+++ b/frc971/constants.cpp
@@ -37,7 +37,8 @@
const double kPracticeWristLowerLimit = -36 * M_PI / 180.0;
const double kCompWristLowerLimit = -36 * M_PI / 180.0;
-const double kWristZeroingSpeed = 1.0;
+const double kWristZeroingSpeed = 0.25;
+const double kWristZeroingOffSpeed = 1.0;
const int kAngleAdjustHallEffect = 2;
@@ -60,6 +61,7 @@
const double kCompAngleAdjustLowerLimit = 0.32;
const double kAngleAdjustZeroingSpeed = -0.2;
+const double kAngleAdjustZeroingOffSpeed = -0.5;
const int kCompCameraCenter = -2;
const int kPracticeCameraCenter = -5;
@@ -79,6 +81,8 @@
// Zeroing speed.
double wrist_zeroing_speed;
+ // Zeroing off speed.
+ double wrist_zeroing_off_speed;
// AngleAdjust hall effect positive and negative edges.
const double *angle_adjust_hall_effect_start_angle;
@@ -93,6 +97,8 @@
// Zeroing speed.
double angle_adjust_zeroing_speed;
+ // Zeroing off speed.
+ double angle_adjust_zeroing_off_speed;
// what camera_center returns
int camera_center;
@@ -116,6 +122,7 @@
kCompWristUpperPhysicalLimit,
kCompWristLowerPhysicalLimit,
kWristZeroingSpeed,
+ kWristZeroingOffSpeed,
kCompAngleAdjustHallEffectStartAngle,
kCompAngleAdjustHallEffectStopAngle,
kCompAngleAdjustUpperLimit,
@@ -123,6 +130,7 @@
kCompAngleAdjustUpperPhysicalLimit,
kCompAngleAdjustLowerPhysicalLimit,
kAngleAdjustZeroingSpeed,
+ kAngleAdjustZeroingOffSpeed,
kCompCameraCenter};
break;
case kPracticeTeamNumber:
@@ -133,6 +141,7 @@
kPracticeWristUpperPhysicalLimit,
kPracticeWristLowerPhysicalLimit,
kWristZeroingSpeed,
+ kWristZeroingOffSpeed,
kPracticeAngleAdjustHallEffectStartAngle,
kPracticeAngleAdjustHallEffectStopAngle,
kPracticeAngleAdjustUpperLimit,
@@ -140,6 +149,7 @@
kPracticeAngleAdjustUpperPhysicalLimit,
kPracticeAngleAdjustLowerPhysicalLimit,
kAngleAdjustZeroingSpeed,
+ kAngleAdjustZeroingOffSpeed,
kPracticeCameraCenter};
break;
default:
@@ -200,6 +210,13 @@
return true;
}
+bool wrist_zeroing_off_speed(double *speed) {
+ const Values *const values = GetValues();
+ if (values == NULL) return false;
+ *speed = values->wrist_zeroing_off_speed;
+ return true;
+}
+
bool angle_adjust_hall_effect_start_angle(double *angle) {
const Values *const values = GetValues();
if (values == NULL) return false;
@@ -249,6 +266,13 @@
return true;
}
+bool angle_adjust_zeroing_off_speed(double *speed) {
+ const Values *const values = GetValues();
+ if (values == NULL) return false;
+ *speed = values->angle_adjust_zeroing_off_speed;
+ return true;
+}
+
bool camera_center(int *center) {
const Values *const values = GetValues();
if (values == NULL) return false;
diff --git a/frc971/constants.h b/frc971/constants.h
index 98baca4..296a4f4 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -25,6 +25,8 @@
// Returns the speed to move the wrist at when zeroing in rad/sec
bool wrist_zeroing_speed(double *speed);
+bool wrist_zeroing_off_speed(double *speed);
+
bool angle_adjust_hall_effect_start_angle(double *angle);
bool angle_adjust_hall_effect_stop_angle(double *angle);
// These are the soft stops for up and down.
@@ -36,6 +38,7 @@
// Returns speed to move the angle adjust when zeroing, in rad/sec
bool angle_adjust_zeroing_speed(double *speed);
+bool angle_adjust_zeroing_off_speed(double *speed);
// Sets *center to how many pixels off center the vertical line
// on the camera view is.
diff --git a/frc971/control_loops/angle_adjust/angle_adjust.cc b/frc971/control_loops/angle_adjust/angle_adjust.cc
index 8482a4b..ec06f7a 100644
--- a/frc971/control_loops/angle_adjust/angle_adjust.cc
+++ b/frc971/control_loops/angle_adjust/angle_adjust.cc
@@ -38,6 +38,12 @@
LOG(ERROR, "Failed to fetch the hall effect start angle constants.\n");
return false;
}
+ if (!constants::angle_adjust_zeroing_off_speed(
+ &config_data->zeroing_off_speed)) {
+ LOG(ERROR,
+ "Failed to fetch the angle adjust zeroing off speed constant.\n");
+ return false;
+ }
if (!constants::angle_adjust_zeroing_speed(
&config_data->zeroing_speed)) {
LOG(ERROR, "Failed to fetch the angle adjust zeroing speed constant.\n");
diff --git a/frc971/control_loops/wrist/wrist.cc b/frc971/control_loops/wrist/wrist.cc
index 13471fa..e364cd6 100644
--- a/frc971/control_loops/wrist/wrist.cc
+++ b/frc971/control_loops/wrist/wrist.cc
@@ -36,6 +36,11 @@
LOG(ERROR, "Failed to fetch the wrist start angle constant.\n");
return false;
}
+ if (!constants::wrist_zeroing_off_speed(&config_data->zeroing_off_speed)) {
+ LOG(ERROR, "Failed to fetch the wrist zeroing off speed constant.\n");
+ return false;
+ }
+
if (!constants::wrist_zeroing_speed(&config_data->zeroing_speed)) {
LOG(ERROR, "Failed to fetch the wrist zeroing speed constant.\n");
return false;
diff --git a/frc971/control_loops/zeroed_joint.h b/frc971/control_loops/zeroed_joint.h
index 4c54f27..3230622 100644
--- a/frc971/control_loops/zeroed_joint.h
+++ b/frc971/control_loops/zeroed_joint.h
@@ -67,6 +67,8 @@
double upper_limit;
// Speed (and direction) to move while zeroing.
double zeroing_speed;
+ // Speed (and direction) to move while moving off the sensor.
+ double zeroing_off_speed;
// Maximum voltage to apply when zeroing.
double max_zeroing_voltage;
// Angles where we see a positive edge from the hall effect sensors.
@@ -273,8 +275,8 @@
state_ = ZEROING;
} else {
// Slowly creep off the sensor.
- zeroing_position_ -= config_data_.zeroing_speed * dt;
- loop_->R << zeroing_position_, -config_data_.zeroing_speed;
+ zeroing_position_ -= config_data_.zeroing_off_speed * dt;
+ loop_->R << zeroing_position_, -config_data_.zeroing_off_speed;
break;
}
}