Added plumbing for piston values through elevator/intake code.
Change-Id: I786fbc1411bc26063a7b03ea5428297317d21daa
diff --git a/bot3/control_loops/elevator/elevator.cc b/bot3/control_loops/elevator/elevator.cc
index 49433a7..a28dd7d 100644
--- a/bot3/control_loops/elevator/elevator.cc
+++ b/bot3/control_loops/elevator/elevator.cc
@@ -237,6 +237,10 @@
if (output) {
output->elevator = loop_->U(0, 0);
+ if(unsafe_goal) {
+ output->passive_support = unsafe_goal->passive_support;
+ output->can_support = unsafe_goal->can_support;
+ }
}
status->zeroed = state_ == RUNNING;
diff --git a/bot3/control_loops/elevator/elevator.q b/bot3/control_loops/elevator/elevator.q
index 35d56f9..6b2ba26 100644
--- a/bot3/control_loops/elevator/elevator.q
+++ b/bot3/control_loops/elevator/elevator.q
@@ -20,6 +20,13 @@
// Maximum elevator profile acceleration or 0 for the default.
float max_acceleration;
+
+ // Whether the passive elevator is supporting the stack/can; true means it
+ // is supporting; false means it is not.
+ bool passive_support;
+ // Whether the can support is restraining the can; true means it
+ // is supporting; false means it is not.
+ bool can_support;
};
message Position {
@@ -58,6 +65,9 @@
// True means support the stack, false means release the support from the
// stack.
bool passive_support;
+ // Toggle for the that supports the can in the robot.
+ // True means support the can, false means release the can.
+ bool can_support;
};
queue Goal goal;
diff --git a/bot3/control_loops/intake/intake.cc b/bot3/control_loops/intake/intake.cc
index 935730b..4788a7a 100644
--- a/bot3/control_loops/intake/intake.cc
+++ b/bot3/control_loops/intake/intake.cc
@@ -19,16 +19,18 @@
const int16_t intake_movement = goal->movement;
- if (intake_movement > 0) {
+ if (intake_movement > 0.0) {
// Suck.
output->intake = kIntakeVoltageFullPower;
- } else if (intake_movement < 0) {
+ } else if (intake_movement < 0.0) {
// Spit.
output->intake = -kIntakeVoltageFullPower;
} else {
// Stationary.
output->intake = 0.0;
}
+
+ output->claw_closed = goal->claw_closed;
}
}
diff --git a/bot3/control_loops/intake/intake.q b/bot3/control_loops/intake/intake.q
index 3047f6b..c09bf87 100644
--- a/bot3/control_loops/intake/intake.q
+++ b/bot3/control_loops/intake/intake.q
@@ -7,13 +7,18 @@
message Goal {
// Positive = suck, negative = spit, zero = stationary.
- int16_t movement;
+ double movement;
+
+ bool claw_closed;
};
message Position {};
message Output {
+ // Positive or negative, depending on whether we're sucking or spitting.
double intake;
+
+ bool claw_closed;
};
message Status {};