got all of the I/O hooked up (I think)
diff --git a/bbb_cape/src/bbb/cape_manager.cc b/bbb_cape/src/bbb/cape_manager.cc
index b5f2851..cf32971 100644
--- a/bbb_cape/src/bbb/cape_manager.cc
+++ b/bbb_cape/src/bbb/cape_manager.cc
@@ -11,7 +11,10 @@
namespace bbb {
CapeManager::CapeManager()
- : uart_(750000), reset_(2, 5, true), custom_bootloader_(2, 3, true) {}
+ : uart_(750000),
+ reset_(2, 5, true),
+ custom_bootloader_(2, 3, true),
+ bootloader_(2, 2, false) {}
void CapeManager::DownloadHex(const ::std::string &filename) {
HexByteReader file(filename);
diff --git a/bbb_cape/src/bbb/cape_manager.h b/bbb_cape/src/bbb/cape_manager.h
index 1cc1a57..e71a59f 100644
--- a/bbb_cape/src/bbb/cape_manager.h
+++ b/bbb_cape/src/bbb/cape_manager.h
@@ -29,7 +29,7 @@
UartReader uart_;
- Gpo reset_, custom_bootloader_;
+ Gpo reset_, custom_bootloader_, bootloader_;
DISALLOW_COPY_AND_ASSIGN(CapeManager);
};
diff --git a/bbb_cape/src/bbb/uart_reader.cc b/bbb_cape/src/bbb/uart_reader.cc
index c45c8ea..9379252 100644
--- a/bbb_cape/src/bbb/uart_reader.cc
+++ b/bbb_cape/src/bbb/uart_reader.cc
@@ -32,7 +32,7 @@
LOG(INFO, "unexporting BB-UART1\n");
if (system("bash -c 'echo -$(cat /sys/devices/bone_capemgr.*/slots"
" | fgrep BB-UART1"
- " | cut -c 2) > /sys/devices/bone_capemgr.*/slots'") ==
+ " | cut -d : -f 1) > /sys/devices/bone_capemgr.*/slots'") ==
-1) {
LOG(FATAL, "system([disable OMAP UART]) failed with %d: %s\n", errno,
strerror(errno));
diff --git a/bbb_cape/src/cape/data_struct.h b/bbb_cape/src/cape/data_struct.h
index 79b1f18..debef38 100644
--- a/bbb_cape/src/cape/data_struct.h
+++ b/bbb_cape/src/cape/data_struct.h
@@ -83,15 +83,15 @@
// 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;
+ int32_t shooter_position, pusher_distal_posedge_position,
+ pusher_proximal_posedge_position;
uint16_t left_drive_hall;
uint16_t right_drive_hall;
uint16_t battery_voltage;
- HallEffectEdges plunger, pusher_distal, pusher_proximal, latch;
+ HallEffectEdges pusher_distal, pusher_proximal;
struct {
uint8_t plunger : 1;
diff --git a/bbb_cape/src/cape/digital.h b/bbb_cape/src/cape/digital.h
index 9743c3f..698aeed 100644
--- a/bbb_cape/src/cape/digital.h
+++ b/bbb_cape/src/cape/digital.h
@@ -71,10 +71,12 @@
}
}
+// May disable other capture inputs too.
static inline void digital_capture_disable(int num) {
NVIC_DisableIRQ(digital_capture_getirqn(num));
}
+// May enable other capture inputs too.
static inline void digital_capture_enable(int num) {
NVIC_EnableIRQ(digital_capture_getirqn(num));
}
diff --git a/bbb_cape/src/cape/robot_comp.c b/bbb_cape/src/cape/robot_comp.c
index 455a1c0..5c51fe8 100644
--- a/bbb_cape/src/cape/robot_comp.c
+++ b/bbb_cape/src/cape/robot_comp.c
@@ -7,17 +7,97 @@
#include "cape/digital.h"
#include "cape/util.h"
-// TIM11.1 on PB9
-
+// TIM11.1 on PB9, aka digital input 6.
static volatile uint32_t ultrasonic_pulse_length = 0;
+typedef struct {
+ uint32_t posedges, negedges;
+} EdgeCounts;
+
+#define COPY_EDGE_COUNTS(edge_counts, hall_effect_edges) \
+ hall_effect_edges.posedges = edge_counts.posedges; \
+ hall_effect_edges.negedges = edge_counts.negedges;
+
+#define HALL_CAPTURE(num, edges, encoder, capture) \
+ void digital_capture_##num##P(void) { \
+ ++edges.posedges; \
+ capture.posedge = encoder_read(encoder); \
+ } \
+ void digital_capture_##num##N(void) { \
+ ++edges.negedges; \
+ capture.negedge = encoder_read(encoder); \
+ }
+#define HALL_CAPTURE_DECL(num, edges, encoder, capture) \
+ static volatile EdgeCounts edges = {0, 0}; \
+ HALL_CAPTURE(num, edges, encoder, capture)
+
+static volatile struct {
+ int32_t posedge, negedge;
+} pusher_distal_captures, pusher_proximal_captures;
+
+#define SHOOTER(plunger_num, pusher_distal_num, pusher_proximal_num, \
+ latch_num, encoder) \
+ HALL_CAPTURE_DECL(pusher_distal_num, pusher_distal, encoder, \
+ pusher_distal_captures); \
+ HALL_CAPTURE_DECL(pusher_proximal_num, pusher_proximal, encoder, \
+ pusher_proximal_captures); \
+ static inline void fill_shooter_values(struct DataStruct *packet) { \
+ digital_capture_disable(pusher_distal_num); \
+ digital_capture_disable(pusher_proximal_num); \
+ packet->main.shooter_position = encoder_read(encoder); \
+ packet->main.pusher_distal_posedge_position = \
+ pusher_distal_captures.posedge; \
+ packet->main.pusher_proximal_posedge_position = \
+ pusher_proximal_captures.negedge; \
+ packet->main.bools.pusher_distal = digital_read(pusher_distal_num); \
+ packet->main.bools.pusher_proximal = digital_read(pusher_proximal_num); \
+ COPY_EDGE_COUNTS(pusher_distal, packet->main.pusher_distal); \
+ COPY_EDGE_COUNTS(pusher_proximal, packet->main.pusher_proximal); \
+ digital_capture_enable(pusher_distal_num); \
+ digital_capture_enable(pusher_proximal_num); \
+ packet->main.bools.plunger = digital_read(plunger_num); \
+ packet->main.bools.latch = digital_read(latch_num); \
+ }
+
+SHOOTER(0, 1, 2, 3, 0)
+
+typedef struct {
+ int32_t posedge, negedge;
+ EdgeCounts front, calibration, back;
+} SingleClawCaptures;
+
+#define CLAW(front_num, calibration_num, back_num, name, encoder) \
+ static volatile SingleClawCaptures name = {0, 0, {0, 0}, {0, 0}, {0, 0}}; \
+ HALL_CAPTURE(front_num, name.front, encoder, name); \
+ HALL_CAPTURE(calibration_num, name.calibration, encoder, name); \
+ HALL_CAPTURE(back_num, name.back, encoder, name); \
+ static inline void fill_##name##_values(struct DataStruct *packet) { \
+ digital_capture_disable(front_num); \
+ digital_capture_disable(calibration_num); \
+ digital_capture_disable(back_num); \
+ packet->main.name.position = encoder_read(encoder); \
+ packet->main.name.posedge_position = name.posedge; \
+ packet->main.name.negedge_position = name.negedge; \
+ packet->main.name.bools.front = digital_read(front_num); \
+ packet->main.name.bools.calibration = digital_read(calibration_num); \
+ packet->main.name.bools.back = digital_read(back_num); \
+ COPY_EDGE_COUNTS(name.front, packet->main.name.front); \
+ COPY_EDGE_COUNTS(name.calibration, packet->main.name.calibration); \
+ COPY_EDGE_COUNTS(name.back, packet->main.name.back); \
+ digital_capture_enable(front_num); \
+ digital_capture_enable(calibration_num); \
+ digital_capture_enable(back_num); \
+ }
+
+CLAW(4, 5, 7, top_claw, 2);
+CLAW(8, 9, 10, bottom_claw, 7);
+
void TIM1_TRG_COM_TIM11_IRQHandler(void) {
TIM11->SR = ~TIM_SR_CC1IF;
ultrasonic_pulse_length = TIM11->CCR1;
}
void robot_init(void) {
- // Digital input 6.
gpio_setup_alt(GPIOB, 9, 3);
RCC->APB2ENR |= RCC_APB2ENR_TIM11EN;
TIM11->CR1 = TIM_CR1_URS;
@@ -34,4 +114,13 @@
}
void robot_fill_packet(struct DataStruct *packet) {
+ packet->main.left_drive = encoder_read(6);
+ packet->main.right_drive = encoder_read(5);
+ packet->main.left_drive_hall = analog_get(0);
+ packet->main.right_drive_hall = analog_get(1);
+
+ packet->main.ultrasonic_pulse_length = ultrasonic_pulse_length;
+
+ fill_top_claw_values(packet);
+ fill_bottom_claw_values(packet);
}