Added friction compensation to the angle adjust.
diff --git a/frc971/control_loops/angle_adjust/angle_adjust.cc b/frc971/control_loops/angle_adjust/angle_adjust.cc
index 06e3719..7670e08 100644
--- a/frc971/control_loops/angle_adjust/angle_adjust.cc
+++ b/frc971/control_loops/angle_adjust/angle_adjust.cc
@@ -51,6 +51,7 @@
}
config_data->max_zeroing_voltage = 4.0;
+ config_data->deadband_voltage = 0.6;
return true;
}
diff --git a/frc971/control_loops/angle_adjust/unaugmented_angle_adjust_motor_plant.cc b/frc971/control_loops/angle_adjust/unaugmented_angle_adjust_motor_plant.cc
index ea84dce..9efffc3 100644
--- a/frc971/control_loops/angle_adjust/unaugmented_angle_adjust_motor_plant.cc
+++ b/frc971/control_loops/angle_adjust/unaugmented_angle_adjust_motor_plant.cc
@@ -27,7 +27,7 @@
Eigen::Matrix<double, 2, 1> L;
L << 1.60656297069, 51.0341417582;
Eigen::Matrix<double, 1, 2> K;
- K << 311.565731672, 11.2839301509;
+ K << 264.830871921, 10.681380124;
return StateFeedbackController<2, 1, 1>(L, K, MakeAngleAdjustRawPlantCoefficients());
}
diff --git a/frc971/control_loops/python/angle_adjust.py b/frc971/control_loops/python/angle_adjust.py
index 8ccf30f..6d84c0c 100755
--- a/frc971/control_loops/python/angle_adjust.py
+++ b/frc971/control_loops/python/angle_adjust.py
@@ -44,7 +44,7 @@
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
- self.PlaceControllerPoles([.45, .8])
+ self.PlaceControllerPoles([.45, .83])
print "Unaugmented controller poles at"
print self.K
diff --git a/frc971/control_loops/wrist/wrist.cc b/frc971/control_loops/wrist/wrist.cc
index 97a0cef..3c76ff8 100644
--- a/frc971/control_loops/wrist/wrist.cc
+++ b/frc971/control_loops/wrist/wrist.cc
@@ -48,6 +48,7 @@
}
config_data->max_zeroing_voltage = 5.0;
+ config_data->deadband_voltage = 0.0;
return true;
}
diff --git a/frc971/control_loops/zeroed_joint.h b/frc971/control_loops/zeroed_joint.h
index 468dc13..0fbe33e 100644
--- a/frc971/control_loops/zeroed_joint.h
+++ b/frc971/control_loops/zeroed_joint.h
@@ -117,6 +117,8 @@
double zeroing_off_speed;
// Maximum voltage to apply when zeroing.
double max_zeroing_voltage;
+ // Deadband voltage.
+ double deadband_voltage;
// Angles where we see a positive edge from the hall effect sensors.
double hall_effect_start_angle[kNumZeroSensors];
};
@@ -432,7 +434,18 @@
break;
}
if (output_enabled) {
- return loop_->voltage();
+ double voltage = loop_->voltage();
+ if (voltage > 0) {
+ voltage += config_data_.deadband_voltage;
+ } else if (voltage < 0) {
+ voltage -= config_data_.deadband_voltage;
+ }
+ if (voltage > 12.0) {
+ voltage = 12.0;
+ } else if (voltage < -12.0) {
+ voltage = -12.0;
+ }
+ return voltage;
} else {
return 0.0;
}