finished a lot of typing for the shooter. nothing works, but we need to sync up. working on shooter state machine.
diff --git a/frc971/control_loops/shooter/shooter.cc b/frc971/control_loops/shooter/shooter.cc
old mode 100644
new mode 100755
index 56fd74b..6977e7d
--- a/frc971/control_loops/shooter/shooter.cc
+++ b/frc971/control_loops/shooter/shooter.cc
@@ -35,34 +35,94 @@
}
}
-// Positive is up, and positive power is up.
+enum {
+ STATE_INITIALIZE,
+ STATE_REQUEST_LOAD,
+ STATE_LOAD_BACKTRACK,
+ STATE_LOAD,
+ STATE_LOADING_PROBLEM,
+ STATE_PREPARE_SHOT,
+ STATE_BRAKE_SET,
+ STATE_READY,
+ STATE_REQUEST_FIRE,
+ STATE_PREPARE_FIRE,
+ STATE_FIRE,
+ STATE_UNLOAD,
+ STATE_UNLOAD_MOVE,
+ STATE_READY_UNLOAD
+} State;
+
+// Positive is out, and positive power is out.
void ShooterMotor::RunIteration(
const ::aos::control_loops::Goal *goal,
const control_loops::ShooterLoop::Position *position,
::aos::control_loops::Output *output,
::aos::control_loops::Status * status) {
+ constexpr double dt = 0.01;
+
+ if (goal == NULL || position == NULL ||
+ output == NULL || status == NULL) {
+ transform-position_ptr = NULL;
+ if (output) output->voltage = 0;
+ LOG(ERROR, "Thought I would just check for null and die.\n");
+ }
// Disable the motors now so that all early returns will return with the
// motors disabled.
- if (output) {
- output->voltage = 0;
- }
+ output->voltage = 0;
ZeroedJoint<1>::PositionData transformed_position;
ZeroedJoint<1>::PositionData *transformed_position_ptr =
&transformed_position;
- if (!position) {
- transformed_position_ptr = NULL;
- } else {
- transformed_position.position = position->pos;
- transformed_position.hall_effects[0] = position->hall_effect;
- transformed_position.hall_effect_positions[0] = position->calibration;
- }
+ transformed_position.position = position->pos;
+ transformed_position.hall_effects[0] = position->hall_effect;
+ transformed_position.hall_effect_positions[0] = position->calibration;
const double voltage = zeroed_joint_.Update(transformed_position_ptr,
output != NULL,
goal->goal, 0.0);
+ const frc971::constants::Values &values = constants::GetValues();
+
+ switch (state_) {
+ case STATE_INITIALIZE:
+ shooter_.zeroing_state() = ZeroedStateFeedbackLoop::UNKNOWN_POSITION;
+ if (position->pusher_distal_hall_effect){
+ } else if (position->pusher_proximal_hall_effect) {
+ calibration_position_ = position->position -
+ } else {
+ }
+
+
+ break;
+ case STATE_REQUEST_LOAD:
+ break;
+ case STATE_LOAD_BACKTRACK:
+ break;
+ case STATE_LOAD:
+ break;
+ case STATE_LOADING_PROBLEM:
+ break;
+ case STATE_PREPARE_SHOT:
+ break;
+ case STATE_BRAKE_SET:
+ break;
+ case STATE_READY:
+ break;
+ case STATE_REQUEST_FIRE:
+ break;
+ case STATE_PREPARE_FIRE:
+ break;
+ case STATE_FIRE:
+ break;
+ case STATE_UNLOAD:
+ break;
+ case STATE_UNLOAD_MOVE:
+ break;
+ case STATE_READY_UNLOAD:
+ break;
+ }
+
if (position) {
LOG(DEBUG, "pos: %f hall: %s absolute: %f\n",
position->pos,
@@ -70,9 +130,7 @@
zeroed_joint_.absolute_position());
}
- if (output) {
- output->voltage = voltage;
- }
+ output->voltage = voltage;
status->done = ::std::abs(zeroed_joint_.absolute_position() - goal->goal) < 0.004;
}
diff --git a/frc971/control_loops/shooter/shooter.gyp b/frc971/control_loops/shooter/shooter.gyp
old mode 100644
new mode 100755
diff --git a/frc971/control_loops/shooter/shooter.q b/frc971/control_loops/shooter/shooter.q
old mode 100644
new mode 100755
index 860162e..ea53b2c
--- a/frc971/control_loops/shooter/shooter.q
+++ b/frc971/control_loops/shooter/shooter.q
@@ -1,35 +1,76 @@
package frc971.control_loops;
-import "aos/common/control_loop/control_looops.q";
+import "aos/common/control_loop/control_loops.q";
queue_group ShooterLoop {
implements aos.control_loops.ControlLoop;
message Goal {
- // The energy to load to in joules.
- double energy;
+ // encoder ticks of shot energy.
+ double shot_power;
// Shoots as soon as this is true.
- bool shoot;
+ bool shot_requested;
+ bool unload_requested;
};
message Position {
- bool back_hall_effect;
+ // back on the plunger
+ bool plunger_back_hall_effect;
+ // truely back on the pusher
+ bool pusher_distal_hall_effect;
+ // warning that we are back on the pusher
+ bool pusher_proximal_hall_effect;
+ // the latch is closed
+ bool latch_hall_effect;
+
+ // count of positive edges
+ int64_t plunger_back_hall_effect_posedge_count;
+ // count of negative edges
+ int64_t plunger_back_hall_effect_negedge_count;
+ // count of positive edges
+ int64_t pusher_distal_hall_effect_posedge_count;
+ // count of negative edges
+ int64_t pusher_distal_hall_effect_negedge_count;
+ // count of positive edges
+ int64_t pusher_proximal_hall_effect_posedge_count;
+ // count of negative edges
+ int64_t pusher_proximal_hall_effect_negedge_count;
+ // count of positive edges
+ int64_t latch_hall_effect_posedge_count;
+ // count of negative edges
+ int64_t latch_hall_effect_negedge_count;
+
// In meters, out is positive.
double position;
double back_calibration;
+
+ // last positive edge
+ double posedge_value;
+ // last negative edge
+ double negedge_value;
};
+ // I don't think this is needed, but it is here
+ // so I won't delete it yet.
message Status {
// Whether it's ready to shoot right now.
bool ready;
// Whether the plunger is in and out of the way of grabbing a ball.
bool cocked;
// How many times we've shot.
- int shots;
+ int32_t shots;
};
+ message Output {
+ // desired motor voltage
+ double voltage;
+ // true: close latch, false: open latch
+ double latch_piston;
+ // true: brake engaded, false: brake release
+ double brake_piston;
+ };
queue Goal goal;
queue Position position;
queue aos.control_loops.Output output;
queue Status status;
};
-queue_group ShooterLoop shooter;
+queue_group ShooterGroup shooter_queue_group;
diff --git a/frc971/control_loops/shooter/shooter_lib_test.cc b/frc971/control_loops/shooter/shooter_lib_test.cc
new file mode 100755
index 0000000..9960222
--- /dev/null
+++ b/frc971/control_loops/shooter/shooter_lib_test.cc
@@ -0,0 +1,305 @@
+#include <unistd.h>
+
+#include <memory>
+
+#include "gtest/gtest.h"
+#include "aos/common/queue.h"
+#include "aos/common/queue_testutils.h"
+#include "frc971/control_loops/shootershooter.q.h"
+#include "frc971/control_loops/shooter/shooter.h"
+#include "frc971/constants.h"
+#include "frc971/control_loops/shooter/shooter_motor_plant.h"
+
+
+using ::aos::time::Time;
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+
+// Class which simulates the shooter and sends out queue messages containing the
+// position.
+class ShooterSimulation {
+ public:
+ // Constructs a motor simulation.
+ ShooterSimulation(double initial_position)
+ : shooter_plant_(new StateFeedbackPlant<2, 1, 1>(MakeRawTopClawPlant()))
+ shooter_queue_group(".frc971.control_loops.claw_queue_group", 0x9f1a99dd,
+ ".frc971.control_loops.claw_queue_group.goal",
+ ".frc971.control_loops.claw_queue_group.position",
+ ".frc971.control_loops.claw_queue_group.output",
+ ".frc971.control_loops.claw_queue_group.status") {
+ Reinitialize(initial_position);
+ }
+
+
+ void Reinitialize(double initial_position) {
+ LOG(INFO, "Reinitializing to {top: %f}\n", initial_position);
+ StateFeedbackPlant<2, 1, 1>* plant = shooter_plant_.get();
+ initial_position_ = initial_position;
+ plant->X(0, 0) = initial_position_;
+ plant->X(1, 0) = 0.0;
+ plant->Y = plant->C() * plant->X;
+ last_voltage_ = 0.0;
+ last_position_.Zero();
+ SetPhysicalSensors(&last_position_);
+ }
+
+
+ // Returns the absolute angle of the wrist.
+ double GetAbsolutePosition() const {
+ return shooter_plant_->Y(0, 0);
+ }
+
+
+ // Returns the adjusted angle of the wrist.
+ double GetPosition() const {
+ return GetAbsolutePosition() - initial_position_;
+ }
+
+
+ // Makes sure pos is inside range (inclusive)
+ bool CheckRange(double pos, struct constants::Values::AnglePair pair) {
+ return (pos >= pair.lower_angle && pos <= pair.upper_angle);
+ }
+
+
+ // Sets the values of the physical sensors that can be directly observed
+ // (encoder, hall effect).
+ void SetPhysicalSensors(control_loops::ShooterGroup::Position *position) {
+ const frc971::constants::Values& values = constants::GetValues();
+ position->position = GetPosition();
+
+ LOG(DEBUG, "Physical shooter at {%f}\n", position->position);
+
+ // Signal that the hall effect sensor has been triggered if it is within
+ // the correct range.
+ position->plunger_back_hall_effect =
+ CheckRange(position->position, values.plunger_heffect);
+ position->pusher_distal_hall_effect =
+ CheckRange(position->position, values.pusher_distal_heffect);
+ position->pusher_proximal_hall_effect =
+ CheckRange(position->position, values.pusher_proximal_heffect);
+ position->latch_hall_effect =
+ CheckRange(position->position, values.latch_heffect);
+ }
+
+
+ void UpdateEffectEdge(bool effect, bool last_effect,
+ double upper_angle, double lower_angle, double position,
+ double &posedge_value, double &negedge_value,
+ int &posedge_count, int &negedge_count) {
+ if (effect && !last_effect) {
+ ++posedge_count;
+ if (last_position_.position < lower_angle) {
+ posedge_value = lower_angle - initial_position_;
+ } else {
+ posedge_value = upper_angle - initial_position_;
+ }
+ }
+
+ if (!effect && last_effect) {
+ ++negedge_count;
+ if (position < lower_angle) {
+ negedge_value = lower_angle - initial_position_;
+ } else {
+ negedge_value - upper_angle - initial_position_;
+ }
+ }
+ }
+
+
+ // Sends out the position queue messages.
+ void SendPositionMessage() {
+ const frc971::constants::Values& values = constants::GetValues();
+ ::aos::ScopedMessagePtr<control_loops::ShooterGroup::Position> position =
+ _queue_group.position.MakeMessage();
+
+ // Initialize all the counters to their previous values.
+ *position = last_position_;
+
+ SetPhysicalSensors(position.get());
+
+ // Handle the front hall effect.
+ if (position->plunger_back_hall_effect &&
+ !last_position_.plunger_back_hall_effect) {
+ ++position->plunger_back_hall_effect_posedge_count;
+
+ if (last_position_.position < values.upper_claw.front.lower_angle) {
+ position->top.posedge_value =
+ values.upper_claw.front.lower_angle - initial_position_;
+ } else {
+ position->top.posedge_value =
+ values.upper_claw.front.upper_angle - initial_position_;
+ }
+ }
+ if (!position->plunger_back_hall_effect &&
+ last_position_.plunger_back_hall_effect) {
+ ++position->plunger_back_hall_effect_negedge_count;
+
+ if (position->top.position < values.upper_claw.front.lower_angle) {
+ position->top.negedge_value =
+ values.upper_claw.front.lower_angle - initial_position_;
+ } else {
+ position->top.negedge_value =
+ values.upper_claw.front.upper_angle - initial_position_;
+ }
+ }
+
+ // Handle plunger hall effect
+ UpdateEffectEdge(
+ position->plunger_back_hall_effect,
+ last_position_.plunger_back_hall_effect,
+ values.plunger_back.upper_angle,
+ values.plunger_back.lower_angle,
+ position->position,
+ position->posedge_value,
+ position->negedge_value,
+ position->plunger_back_hall_effect_posedge_count,
+ position->plunger_back_hall_effect_negedge_count);
+
+ // Handle pusher distal hall effect
+ UpdateEffectEdge(
+ position->pusher_distal_hall_effect,
+ last_position_.pusher_distal_hall_effect,
+ values.pusher_distal.upper_angle,
+ values.pusher_distal.lower_angle,
+ position->position,
+ position->posedge_value,
+ position->negedge_value,
+ position->pusher_distal_hall_effect_posedge_count,
+ position->pusher_distal_hall_effect_negedge_count);
+
+ // Handle pusher proximal hall effect
+ UpdateEffectEdge(
+ position->pusher_proximal_hall_effect,
+ last_position_.pusher_proximal_hall_effect,
+ values.pusher_proximal.upper_angle,
+ values.pusher_proximal.lower_angle,
+ position->position,
+ position->posedge_value,
+ position->negedge_value,
+ position->pusher_proximal_hall_effect_posedge_count,
+ position->pusher_proximal_hall_effect_negedge_count);
+
+ // Handle latch hall effect
+ UpdateEffectEdge(
+ position->latch_hall_effect,
+ last_position_.latch_hall_effect,
+ values.latch.upper_angle,
+ values.latch.lower_angle,
+ position->position,
+ position->posedge_value,
+ position->negedge_value,
+ position->latch_hall_effect_posedge_count,
+ position->latch_hall_effect_negedge_count);
+
+ // Only set calibration if it changed last cycle. Calibration starts out
+ // with a value of 0.
+ last_position_ = *position;
+ position.Send();
+ }
+
+
+ // Simulates the claw moving for one timestep.
+ void Simulate() {
+ const frc971::constants::Values& v = constants::GetValues();
+ StateFeedbackPlant<2, 1, 1>* plant = shooter_plant_.get();
+ EXPECT_TRUE(shooter_queue_group.output.FetchLatest());
+ plant->U << last_voltage_;
+ plant->Update();
+
+ // check top claw inside limits
+ EXPECT_GE(v.upper_limit, plant->Y(0, 0));
+ EXPECT_LE(v.lower_limit, plant->Y(0, 0));
+
+ // TODO(austin): Check that the claws aren't too close to eachother.
+ last_voltage_ = shooter_queue_group.output->voltage;
+ }
+
+
+ // Top of the claw, the one with rollers
+ ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> shooter_plant_;
+
+
+ private:
+
+ ShooterGroup shooter_queue_group;
+ double initial_position_;
+ double last_voltage_;
+
+ control_loops::ShooterGroup::Position last_position_;
+};
+
+
+class ShooterTest : public ::testing::Test {
+ protected:
+ ::aos::common::testing::GlobalCoreInstance my_core;
+
+ // Create a new instance of the test queue so that it invalidates the queue
+ // that it points to. Otherwise, we will have a pointer to shared memory that
+ // is no longer valid.
+ ShooterGroup shooter_queue_group;
+
+ // Create a loop and simulation plant.
+ ShooterMotor shooter_motor_;
+ ShooterMotorSimulation shooter_motor_plant_;
+
+ // Minimum amount of acceptable seperation between the top and bottom of the
+ // claw.
+ double min_seperation_;
+
+
+ ShooterTest()
+ : shooter_queue_group(".frc971.control_loops.claw_queue_group", 0x9f1a99dd,
+ ".frc971.control_loops.claw_queue_group.goal",
+ ".frc971.control_loops.claw_queue_group.position",
+ ".frc971.control_loops.claw_queue_group.output",
+ ".frc971.control_loops.claw_queue_group.status"),
+ shooter_motor_(&claw_queue_group),
+ shooter_motor_plant_(0.4, 0.2) {
+ // Flush the robot state queue so we can use clean shared memory for this
+ // test.
+ ::aos::robot_state.Clear();
+ SendDSPacket(true);
+ }
+
+
+ void SendDSPacket(bool enabled) {
+ ::aos::robot_state.MakeWithBuilder().enabled(enabled)
+ .autonomous(false)
+ .team_id(971).Send();
+ ::aos::robot_state.FetchLatest();
+ }
+
+
+ void VerifyNearGoal() {
+ shooter_queue_group.goal.FetchLatest();
+ shooter_queue_group.position.FetchLatest();
+ double pos = shooter_motor_plant_.GetAbsolutePosition();
+ EXPECT_NEAR(shooter_queue_group.goal->shot_power, pos, 1e-4);
+ }
+
+ virtual ~ShooterTest() {
+ ::aos::robot_state.Clear();
+ }
+};
+
+
+// Tests that the wrist zeros correctly and goes to a position.
+TEST_F(ShooterTest, ZerosCorrectly) {
+ shooter_queue_group.goal.MakeWithBuilder()
+ .shot_power(5050.1)
+ .Send();
+ for (int i = 0; i < 400; ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+}
+
+} // namespace testing
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/shooter/shooter_main.cc b/frc971/control_loops/shooter/shooter_main.cc
old mode 100644
new mode 100755
diff --git a/frc971/control_loops/shooter/shooter_motor_plant.cc b/frc971/control_loops/shooter/shooter_motor_plant.cc
old mode 100644
new mode 100755
diff --git a/frc971/control_loops/shooter/shooter_motor_plant.h b/frc971/control_loops/shooter/shooter_motor_plant.h
old mode 100644
new mode 100755