got the sensors working on the real robot
diff --git a/frc971/control_loops/shooter/shooter.cc b/frc971/control_loops/shooter/shooter.cc
index 08138ff..b898c58 100755
--- a/frc971/control_loops/shooter/shooter.cc
+++ b/frc971/control_loops/shooter/shooter.cc
@@ -73,12 +73,14 @@
return;
}
- if (initial_loop_) {
- // TODO(austin): If 'reset()', we are lost, start over.
- initial_loop_ = false;
- shooter_.SetPositionDirectly(position->position);
- } else {
- shooter_.SetPositionValues(position->position);
+ if (position) {
+ if (initial_loop_) {
+ // TODO(austin): If 'reset()', we are lost, start over.
+ initial_loop_ = false;
+ shooter_.SetPositionDirectly(position->position);
+ } else {
+ shooter_.SetPositionValues(position->position);
+ }
}
// Disable the motors now so that all early returns will return with the
diff --git a/frc971/control_loops/shooter/shooter.h b/frc971/control_loops/shooter/shooter.h
index 89109dd..8c8ab81 100755
--- a/frc971/control_loops/shooter/shooter.h
+++ b/frc971/control_loops/shooter/shooter.h
@@ -95,12 +95,12 @@
Correct(Y);
}
- void SetGoalPosition(double desired_position, double desired_velocity) {
+ void SetGoalPosition(double, double) {
// austin said something about which matrix to set, but I didn't under
// very much of it
//some_matrix = {desired_position, desired_velocity};
- LOG(DEBUG, "ZSFL> dp: %.2f dz: %.2f\n", desired_position, desired_velocity);
- R << desired_position, desired_velocity, 0;
+ //LOG(DEBUG, "ZSFL> dp: %.2f dz: %.2f\n", desired_position, desired_velocity);
+ //R << desired_position, desired_velocity, 0;
}
double position() const { return X_hat(0, 0); }
diff --git a/frc971/input/sensor_receiver.cc b/frc971/input/sensor_receiver.cc
index 369dab5..84ccc70 100644
--- a/frc971/input/sensor_receiver.cc
+++ b/frc971/input/sensor_receiver.cc
@@ -72,15 +72,15 @@
return (voltage - k.low) / (k.high - k.low);
}
-double shooter_translate(int32_t in) {
+double claw_translate(int32_t in) {
return static_cast<double>(in)
/ (256.0 /*cpr*/ * 4.0 /*quad*/)
- * (18.0 / 48.0 /*encoder gears*/)
- * (12.0 / 60.0 /*chain reduction*/)
+ / (18.0 / 48.0 /*encoder gears*/)
+ / (12.0 / 60.0 /*chain reduction*/)
* (M_PI / 180.0);
}
-double claw_translate(int32_t in) {
+double shooter_translate(int32_t in) {
return static_cast<double>(in)
/ (256.0 /*cpr*/ * 4.0 /*quad*/)
* 16 /*sprocket teeth*/ * 0.375 /*chain pitch*/
@@ -97,22 +97,29 @@
void CopyClawPosition(control_loops::HalfClawPosition *output,
const ::bbb::SingleClawPosition &input,
- State::SingleClawState *state) {
+ State::SingleClawState *state,
+ bool reversed) {
CopyHallEffectEdges(&output->front, input.front, &state->front);
+ output->front.current = input.bools.front;
CopyHallEffectEdges(&output->calibration, input.calibration,
&state->calibration);
+ output->calibration.current = input.bools.calibration;
CopyHallEffectEdges(&output->back, input.back, &state->back);
+ output->back.current = input.bools.back;
- output->position = claw_translate(input.position);
- output->posedge_value = claw_translate(input.posedge_position);
- output->negedge_value = claw_translate(input.negedge_position);
+ const double multiplier = reversed ? -1.0 : 1.0;
+
+ output->position = multiplier * claw_translate(input.position);
+ output->posedge_value = multiplier * claw_translate(input.posedge_position);
+ output->negedge_value = multiplier * claw_translate(input.negedge_position);
}
void PacketReceived(const ::bbb::DataStruct *data,
const ::aos::time::Time &cape_timestamp,
State *state) {
- ::frc971::logging_structs::CapeReading reading_to_log(cape_timestamp.sec(),
- cape_timestamp.nsec());
+ ::frc971::logging_structs::CapeReading reading_to_log(
+ cape_timestamp.sec(), cape_timestamp.nsec(),
+ data->main.ultrasonic_pulse_length);
LOG_STRUCT(DEBUG, "cape reading", reading_to_log);
bool bad_gyro;
if (data->uninitialized_gyro) {
@@ -151,9 +158,9 @@
{
auto claw_position = control_loops::claw_queue_group.position.MakeMessage();
CopyClawPosition(&claw_position->top, data->main.top_claw,
- &state->top_claw);
+ &state->top_claw, false);
CopyClawPosition(&claw_position->bottom, data->main.bottom_claw,
- &state->bottom_claw);
+ &state->bottom_claw, true);
claw_position.Send();
}
diff --git a/frc971/queues/to_log.q b/frc971/queues/to_log.q
index c3cc076..24e2402 100644
--- a/frc971/queues/to_log.q
+++ b/frc971/queues/to_log.q
@@ -3,4 +3,5 @@
struct CapeReading {
uint32_t sec;
uint32_t nsec;
+ uint32_t sonar;
};