Added claw zeroing code.
diff --git a/frc971/control_loops/claw/claw.gyp b/frc971/control_loops/claw/claw.gyp
index 5766d92..8bb94c0 100644
--- a/frc971/control_loops/claw/claw.gyp
+++ b/frc971/control_loops/claw/claw.gyp
@@ -51,6 +51,19 @@
],
},
{
+ 'target_name': 'claw_calibration',
+ 'type': 'executable',
+ 'sources': [
+ 'claw_calibration.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ 'claw_loop',
+ '<(AOS)/common/common.gyp:controls',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
+ ],
+ },
+ {
'target_name': 'claw',
'type': 'executable',
'sources': [
diff --git a/frc971/control_loops/claw/claw.q b/frc971/control_loops/claw/claw.q
index 16472b7..333f83c 100644
--- a/frc971/control_loops/claw/claw.q
+++ b/frc971/control_loops/claw/claw.q
@@ -45,7 +45,7 @@
double intake_voltage;
double top_claw_voltage;
double bottom_claw_voltage;
- double tusk_voltage;
+ double tusk_voltage;
};
queue Goal goal;
diff --git a/frc971/control_loops/claw/claw_calibration.cc b/frc971/control_loops/claw/claw_calibration.cc
new file mode 100644
index 0000000..0167c08
--- /dev/null
+++ b/frc971/control_loops/claw/claw_calibration.cc
@@ -0,0 +1,232 @@
+#include "frc971/control_loops/claw/claw.q.h"
+#include "frc971/control_loops/control_loops.q.h"
+
+#include "aos/linux_code/init.h"
+#include "frc971/constants.h"
+
+namespace frc971 {
+
+typedef constants::Values::Claws Claws;
+
+bool DoGetPositionOfEdge(
+ const double start_position,
+ const control_loops::HalfClawPosition &last_claw_position,
+ const control_loops::HalfClawPosition &claw_position,
+ const HallEffectStruct &last_hall_effect,
+ const HallEffectStruct &hall_effect,
+ Claws::AnglePair *limits) {
+
+ if (hall_effect.posedge_count != last_hall_effect.posedge_count) {
+ if (claw_position.posedge_value < last_claw_position.position) {
+ limits->upper_angle = claw_position.posedge_value - start_position;
+ } else {
+ limits->lower_decreasing_angle =
+ claw_position.posedge_value - start_position;
+ }
+ return true;
+ }
+ if (hall_effect.negedge_count != last_hall_effect.negedge_count) {
+ if (claw_position.negedge_value > last_claw_position.position) {
+ limits->upper_decreasing_angle =
+ claw_position.negedge_value - start_position;
+ } else {
+ limits->lower_angle = claw_position.negedge_value - start_position;
+ }
+ return true;
+ }
+
+ return false;
+}
+
+bool GetPositionOfEdge(
+ const double start_position,
+ const control_loops::HalfClawPosition &last_claw_position,
+ const control_loops::HalfClawPosition &claw_position, Claws::Claw *claw) {
+
+ if (DoGetPositionOfEdge(start_position, last_claw_position, claw_position,
+ last_claw_position.front, claw_position.front,
+ &claw->front)) {
+ return true;
+ }
+ if (DoGetPositionOfEdge(start_position, last_claw_position, claw_position,
+ last_claw_position.calibration,
+ claw_position.calibration, &claw->calibration)) {
+ return true;
+ }
+ if (DoGetPositionOfEdge(start_position, last_claw_position, claw_position,
+ last_claw_position.back, claw_position.back,
+ &claw->back)) {
+ return true;
+ }
+
+ double position = claw_position.position - start_position;
+
+ if (position > claw->upper_limit) {
+ claw->upper_hard_limit = claw->upper_limit = position;
+ return true;
+ }
+ if (position < claw->lower_limit) {
+ claw->lower_hard_limit = claw->lower_limit = position;
+ return true;
+ }
+ return false;
+}
+
+int Main() {
+ while (!control_loops::claw_queue_group.position.FetchNextBlocking());
+
+ const double top_start_position =
+ control_loops::claw_queue_group.position->top.position;
+ const double bottom_start_position =
+ control_loops::claw_queue_group.position->bottom.position;
+
+ Claws limits;
+
+ limits.claw_zeroing_off_speed = 0.5;
+ limits.claw_zeroing_speed = 0.1;
+ limits.claw_zeroing_separation = 0.1;
+
+ // claw separation that would be considered a collision
+ limits.claw_min_separation = 0.0;
+ limits.claw_max_separation = 0.0;
+
+ // We should never get closer/farther than these.
+ limits.soft_min_separation = 0.0;
+ limits.soft_max_separation = 0.0;
+
+ limits.upper_claw.lower_hard_limit = 0.0;
+ limits.upper_claw.upper_hard_limit = 0.0;
+ limits.upper_claw.lower_limit = 0.0;
+ limits.upper_claw.upper_limit = 0.0;
+ limits.upper_claw.front.lower_angle = 0.0;
+ limits.upper_claw.front.upper_angle = 0.0;
+ limits.upper_claw.front.lower_decreasing_angle = 0.0;
+ limits.upper_claw.front.upper_decreasing_angle = 0.0;
+ limits.upper_claw.calibration.lower_angle = 0.0;
+ limits.upper_claw.calibration.upper_angle = 0.0;
+ limits.upper_claw.calibration.lower_decreasing_angle = 0.0;
+ limits.upper_claw.calibration.upper_decreasing_angle = 0.0;
+ limits.upper_claw.back.lower_angle = 0.0;
+ limits.upper_claw.back.upper_angle = 0.0;
+ limits.upper_claw.back.lower_decreasing_angle = 0.0;
+ limits.upper_claw.back.upper_decreasing_angle = 0.0;
+
+ limits.lower_claw.lower_hard_limit = 0.0;
+ limits.lower_claw.upper_hard_limit = 0.0;
+ limits.lower_claw.lower_limit = 0.0;
+ limits.lower_claw.upper_limit = 0.0;
+ limits.lower_claw.front.lower_angle = 0.0;
+ limits.lower_claw.front.upper_angle = 0.0;
+ limits.lower_claw.front.lower_decreasing_angle = 0.0;
+ limits.lower_claw.front.upper_decreasing_angle = 0.0;
+ limits.lower_claw.calibration.lower_angle = 0.0;
+ limits.lower_claw.calibration.upper_angle = 0.0;
+ limits.lower_claw.calibration.lower_decreasing_angle = 0.0;
+ limits.lower_claw.calibration.upper_decreasing_angle = 0.0;
+ limits.lower_claw.back.lower_angle = 0.0;
+ limits.lower_claw.back.upper_angle = 0.0;
+ limits.lower_claw.back.lower_decreasing_angle = 0.0;
+ limits.lower_claw.back.upper_decreasing_angle = 0.0;
+
+ limits.claw_unimportant_epsilon = 0.01;
+ limits.start_fine_tune_pos = -0.2;
+ limits.max_zeroing_voltage = 4.0;
+
+ control_loops::ClawGroup::Position last_position =
+ *control_loops::claw_queue_group.position;
+
+ while (true) {
+ if (control_loops::claw_queue_group.position.FetchNextBlocking()) {
+ bool print = false;
+ if (GetPositionOfEdge(top_start_position, last_position.top,
+ control_loops::claw_queue_group.position->top,
+ &limits.upper_claw)) {
+ print = true;
+ LOG(DEBUG, "Got an edge on the upper claw\n");
+ }
+ if (GetPositionOfEdge(bottom_start_position, last_position.bottom,
+ control_loops::claw_queue_group.position->bottom,
+ &limits.lower_claw)) {
+ print = true;
+ LOG(DEBUG, "Got an edge on the lower claw\n");
+ }
+ const double top_position =
+ control_loops::claw_queue_group.position->top.position -
+ top_start_position;
+ const double bottom_position =
+ control_loops::claw_queue_group.position->bottom.position -
+ bottom_start_position;
+ const double separation = top_position - bottom_position;
+ if (separation > limits.claw_max_separation) {
+ limits.soft_max_separation = limits.claw_max_separation = separation;
+ print = true;
+ }
+ if (separation < limits.claw_min_separation) {
+ limits.soft_min_separation = limits.claw_min_separation = separation;
+ print = true;
+ }
+
+ if (print) {
+ printf("{%f,\n", limits.claw_zeroing_off_speed);
+ printf("%f,\n", limits.claw_zeroing_speed);
+ printf("%f,\n", limits.claw_zeroing_separation);
+ printf("%f,\n", limits.claw_min_separation);
+ printf("%f,\n", limits.claw_max_separation);
+ printf("%f,\n", limits.soft_min_separation);
+ printf("%f,\n", limits.soft_max_separation);
+ printf(
+ "{%f, %f, %f, %f, {%f, %f, %f, %f}, {%f, %f, %f, %f}, {%f, %f, %f, "
+ "%f}},\n",
+ limits.upper_claw.lower_hard_limit,
+ limits.upper_claw.upper_hard_limit, limits.upper_claw.lower_limit,
+ limits.upper_claw.upper_limit, limits.upper_claw.front.lower_angle,
+ limits.upper_claw.front.upper_angle,
+ limits.upper_claw.front.lower_decreasing_angle,
+ limits.upper_claw.front.upper_decreasing_angle,
+ limits.upper_claw.calibration.lower_angle,
+ limits.upper_claw.calibration.upper_angle,
+ limits.upper_claw.calibration.lower_decreasing_angle,
+ limits.upper_claw.calibration.upper_decreasing_angle,
+ limits.upper_claw.back.lower_angle,
+ limits.upper_claw.back.upper_angle,
+ limits.upper_claw.back.lower_decreasing_angle,
+ limits.upper_claw.back.upper_decreasing_angle);
+
+ printf(
+ "{%f, %f, %f, %f, {%f, %f, %f, %f}, {%f, %f, %f, %f}, {%f, %f, %f, "
+ "%f}},\n",
+ limits.lower_claw.lower_hard_limit,
+ limits.lower_claw.upper_hard_limit, limits.lower_claw.lower_limit,
+ limits.lower_claw.upper_limit, limits.lower_claw.front.lower_angle,
+ limits.lower_claw.front.upper_angle,
+ limits.lower_claw.front.lower_decreasing_angle,
+ limits.lower_claw.front.upper_decreasing_angle,
+ limits.lower_claw.calibration.lower_angle,
+ limits.lower_claw.calibration.upper_angle,
+ limits.lower_claw.calibration.lower_decreasing_angle,
+ limits.lower_claw.calibration.upper_decreasing_angle,
+ limits.lower_claw.back.lower_angle,
+ limits.lower_claw.back.upper_angle,
+ limits.lower_claw.back.lower_decreasing_angle,
+ limits.lower_claw.back.upper_decreasing_angle);
+ printf("%f, // claw_unimportant_epsilon\n",
+ limits.claw_unimportant_epsilon);
+ printf("%f, // start_fine_tune_pos\n", limits.start_fine_tune_pos);
+ printf("%f,\n", limits.max_zeroing_voltage);
+ printf("}\n");
+ }
+
+ last_position = *control_loops::claw_queue_group.position;
+ }
+ }
+ return 0;
+}
+
+} // namespace frc971
+
+int main() {
+ ::aos::Init();
+ int returnvalue = ::frc971::Main();
+ ::aos::Cleanup();
+ return returnvalue;
+}
diff --git a/frc971/prime/prime.gyp b/frc971/prime/prime.gyp
index 42a9894..2b5d600 100644
--- a/frc971/prime/prime.gyp
+++ b/frc971/prime/prime.gyp
@@ -8,6 +8,7 @@
'../control_loops/drivetrain/drivetrain.gyp:drivetrain',
'../control_loops/drivetrain/drivetrain.gyp:drivetrain_lib_test',
'../control_loops/claw/claw.gyp:claw',
+ '../control_loops/claw/claw.gyp:claw_calibration',
'../control_loops/claw/claw.gyp:claw_lib_test',
'../control_loops/shooter/shooter.gyp:shooter',
'../control_loops/shooter/shooter.gyp:shooter_lib_test',