Added seperate on and off constants for the wrist and angle adjust.
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;
}
}