got all of the data from the uart to the queues
diff --git a/bbb_cape/src/cape/data_struct.h b/bbb_cape/src/cape/data_struct.h
index b381202..79b1f18 100644
--- a/bbb_cape/src/cape/data_struct.h
+++ b/bbb_cape/src/cape/data_struct.h
@@ -8,6 +8,21 @@
// In the prime code, bbb_cape/src/bbb/data_struct.h #includes this file.
#pragma pack(push, 1)
+typedef struct {
+ uint8_t posedges, negedges;
+} HallEffectEdges;
+typedef struct {
+ int32_t position, posedge_position, negedge_position;
+
+ HallEffectEdges front, calibration, back;
+
+ struct {
+ uint16_t front : 1;
+ uint16_t calibration : 1;
+ uint16_t back : 1;
+ } bools;
+} SingleClawPosition;
+
// Be careful with declaration order in here. ARM doesn't like unaligned
// accesses and this structure is packed, so messing the order up will cause the
// compiler to generate very inefficient code to access fields.
@@ -60,18 +75,30 @@
// This is for the comp and practice robots.
struct {
+ SingleClawPosition top_claw, bottom_claw;
+
int32_t left_drive;
int32_t right_drive;
+
+ // The length of the pulse from the ultrasonic sensor in 100kHZ ticks.
+ uint32_t ultrasonic_pulse_length;
+
+ int32_t shooter_position, shooter_posedge_position,
+ shooter_negedge_position;
+
uint16_t left_drive_hall;
uint16_t right_drive_hall;
uint16_t battery_voltage;
- // The length of the pulse from the ultrasonic sensor in 100kHZ ticks.
- uint32_t ultrasonic_pulse_length;
+ HallEffectEdges plunger, pusher_distal, pusher_proximal, latch;
struct {
- };
+ uint8_t plunger : 1;
+ uint8_t pusher_distal : 1;
+ uint8_t pusher_proximal : 1;
+ uint8_t latch : 1;
+ } bools;
} main;
};
} __attribute__((aligned(8)));
diff --git a/frc971/control_loops/claw/claw.gyp b/frc971/control_loops/claw/claw.gyp
index 52712d5..5766d92 100644
--- a/frc971/control_loops/claw/claw.gyp
+++ b/frc971/control_loops/claw/claw.gyp
@@ -1,7 +1,7 @@
{
'targets': [
{
- 'target_name': 'claw_loops',
+ 'target_name': 'claw_loop',
'type': 'static_library',
'sources': ['claw.q'],
'variables': {
@@ -25,13 +25,13 @@
'claw_motor_plant.cc',
],
'dependencies': [
- 'claw_loops',
+ 'claw_loop',
'<(AOS)/common/common.gyp:controls',
'<(DEPTH)/frc971/frc971.gyp:constants',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
],
'export_dependent_settings': [
- 'claw_loops',
+ 'claw_loop',
'<(AOS)/common/common.gyp:controls',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
],
@@ -44,7 +44,7 @@
],
'dependencies': [
'<(EXTERNALS):gtest',
- 'claw_loops',
+ 'claw_loop',
'claw_lib',
'<(AOS)/common/common.gyp:queue_testutils',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
diff --git a/frc971/control_loops/control_loops.q b/frc971/control_loops/control_loops.q
index ccc8388..631fd59 100644
--- a/frc971/control_loops/control_loops.q
+++ b/frc971/control_loops/control_loops.q
@@ -1,7 +1,15 @@
package frc971;
+// Records edges captured on a single hall effect sensor.
struct HallEffectStruct {
bool current;
int32_t posedge_count;
int32_t negedge_count;
};
+
+// Records the positions for a mechanism with edge-capturing sensors on it.
+struct HallEffectPositions {
+ double current;
+ double posedge;
+ double negedge;
+};
diff --git a/frc971/input/input.gyp b/frc971/input/input.gyp
index e87063b..9ebfa59 100644
--- a/frc971/input/input.gyp
+++ b/frc971/input/input.gyp
@@ -32,6 +32,8 @@
'<(DEPTH)/bbb_cape/src/bbb/bbb.gyp:sensor_reader',
'<(AOS)/common/common.gyp:time',
'<(AOS)/common/logging/logging.gyp:queue_logging',
+ '<(DEPTH)/frc971/control_loops/claw/claw.gyp:claw_loop',
+ '<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
],
},
],
diff --git a/frc971/input/sensor_receiver.cc b/frc971/input/sensor_receiver.cc
index 2d7af87..a7005a4 100644
--- a/frc971/input/sensor_receiver.cc
+++ b/frc971/input/sensor_receiver.cc
@@ -12,6 +12,8 @@
#include "frc971/queues/gyro_angle.q.h"
#include "frc971/constants.h"
#include "frc971/queues/to_log.q.h"
+#include "frc971/control_loops/shooter/shooter.q.h"
+#include "frc971/control_loops/claw/claw.q.h"
#ifndef M_PI
#define M_PI 3.14159265358979323846
@@ -24,6 +26,18 @@
namespace frc971 {
namespace {
+struct State {
+ struct HallEffectCounters {
+ WrappingCounter posedges, negedges;
+ };
+
+ HallEffectCounters plunger, pusher_distal, pusher_proximal, latch;
+
+ struct SingleClawState {
+ HallEffectCounters front, calibration, back;
+ } top_claw, bottom_claw;
+};
+
double drivetrain_translate(int32_t in) {
return static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*quad*/) *
constants::GetValues().drivetrain_encoder_ratio *
@@ -31,6 +45,7 @@
}
// Translates values from the ADC into voltage.
+// TODO(brian): Fix this.
double adc_translate(uint16_t in) {
static const double kVRefN = 0;
static const double kVRefP = 3.3;
@@ -55,8 +70,39 @@
return (voltage - k.low) / (k.high - k.low);
}
+double shooter_translate(int32_t in) {
+ // TODO(brians): Put real numbers in.
+ return in;
+}
+
+double claw_translate(int32_t in) {
+ // TODO(brians): Put real numbers in.
+ return in;
+}
+
+void CopyHallEffectEdges(HallEffectStruct *output,
+ const ::bbb::HallEffectEdges &input,
+ State::HallEffectCounters *state) {
+ output->posedge_count = state->posedges.Update(input.posedges);
+ output->negedge_count = state->negedges.Update(input.negedges);
+}
+
+void CopyClawPosition(control_loops::HalfClawPosition *output,
+ const ::bbb::SingleClawPosition &input,
+ State::SingleClawState *state) {
+ CopyHallEffectEdges(&output->front, input.front, &state->front);
+ CopyHallEffectEdges(&output->calibration, input.calibration,
+ &state->calibration);
+ CopyHallEffectEdges(&output->back, input.back, &state->back);
+
+ output->position = claw_translate(input.position);
+ output->posedge_value = claw_translate(input.posedge_position);
+ output->negedge_value = claw_translate(input.negedge_position);
+}
+
void PacketReceived(const ::bbb::DataStruct *data,
- const ::aos::time::Time &cape_timestamp) {
+ const ::aos::time::Time &cape_timestamp,
+ State *state) {
::frc971::logging_structs::CapeReading reading_to_log(cape_timestamp.sec(),
cape_timestamp.nsec());
LOG_STRUCT(DEBUG, "cape reading", reading_to_log);
@@ -93,6 +139,42 @@
constants::GetValues().right_drive, data->main.right_drive_hall))
.battery_voltage(battery_translate(data->main.battery_voltage))
.Send();
+
+ {
+ auto claw_position = control_loops::claw_queue_group.position.MakeMessage();
+ CopyClawPosition(&claw_position->top, data->main.top_claw,
+ &state->top_claw);
+ CopyClawPosition(&claw_position->bottom, data->main.bottom_claw,
+ &state->bottom_claw);
+ claw_position.Send();
+ }
+
+ {
+ auto shooter_position =
+ control_loops::shooter_queue_group.position.MakeMessage();
+
+ CopyHallEffectEdges(&shooter_position->plunger, data->main.plunger,
+ &state->plunger);
+ shooter_position->plunger.current = data->main.bools.plunger;
+ CopyHallEffectEdges(&shooter_position->pusher_distal,
+ data->main.pusher_distal, &state->pusher_distal);
+ shooter_position->pusher_distal.current = data->main.bools.pusher_distal;
+ CopyHallEffectEdges(&shooter_position->pusher_proximal,
+ data->main.pusher_proximal, &state->pusher_proximal);
+ shooter_position->pusher_proximal.current =
+ data->main.bools.pusher_proximal;
+ CopyHallEffectEdges(&shooter_position->latch, data->main.latch,
+ &state->latch);
+ shooter_position->latch.current = data->main.bools.latch;
+
+ shooter_position->position = shooter_translate(data->main.shooter_position);
+ shooter_position->pusher_posedge_value =
+ shooter_translate(data->main.shooter_posedge_position);
+ shooter_position->pusher_negedge_value =
+ shooter_translate(data->main.shooter_negedge_position);
+
+ shooter_position.Send();
+ }
}
} // namespace
@@ -101,8 +183,10 @@
int main() {
::aos::Init(::bbb::SensorReader::kRelativePriority);
::bbb::SensorReader reader("comp");
+ ::frc971::State state;
while (true) {
- ::frc971::PacketReceived(reader.ReadPacket(), reader.GetCapeTimestamp());
+ ::frc971::PacketReceived(reader.ReadPacket(), reader.GetCapeTimestamp(),
+ &state);
}
::aos::Cleanup();
}