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);
}
diff --git a/frc971/control_loops/claw/claw.q b/frc971/control_loops/claw/claw.q
index b9b6822..16472b7 100644
--- a/frc971/control_loops/claw/claw.q
+++ b/frc971/control_loops/claw/claw.q
@@ -45,6 +45,7 @@
double intake_voltage;
double top_claw_voltage;
double bottom_claw_voltage;
+ double tusk_voltage;
};
queue Goal goal;
diff --git a/frc971/input/input.gyp b/frc971/input/input.gyp
index 9ebfa59..195195e 100644
--- a/frc971/input/input.gyp
+++ b/frc971/input/input.gyp
@@ -14,6 +14,8 @@
'<(DEPTH)/frc971/queues/queues.gyp:queues',
'<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
'<(DEPTH)/frc971/autonomous/autonomous.gyp:auto_queue',
+ '<(DEPTH)/frc971/control_loops/claw/claw.gyp:claw_loop',
+ '<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
],
},
{
diff --git a/frc971/input/joystick_reader.cc b/frc971/input/joystick_reader.cc
index e078e53..1150304 100644
--- a/frc971/input/joystick_reader.cc
+++ b/frc971/input/joystick_reader.cc
@@ -10,6 +10,8 @@
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/queues/gyro_angle.q.h"
#include "frc971/autonomous/auto.q.h"
+#include "frc971/control_loops/claw/claw.q.h"
+#include "frc971/control_loops/shooter/shooter.q.h"
using ::frc971::control_loops::drivetrain;
using ::frc971::sensors::gyro;
@@ -51,10 +53,9 @@
double right_goal = 0.0;
const double wheel = -data.GetAxis(kSteeringWheel);
const double throttle = -data.GetAxis(kDriveThrottle);
- LOG(DEBUG, "wheel %f throttle %f\n", wheel, throttle);
const double kThrottleGain = 1.0 / 2.5;
- if (data.IsPressed(kDriveControlLoopEnable1) ||
- data.IsPressed(kDriveControlLoopEnable2)) {
+ if (false && (data.IsPressed(kDriveControlLoopEnable1) ||
+ data.IsPressed(kDriveControlLoopEnable2))) {
static double distance = 0.0;
static double angle = 0.0;
static double filtered_goal_distance = 0.0;
@@ -89,12 +90,12 @@
LOG(DEBUG, "Left goal %f Right goal %f\n", left_goal, right_goal);
}
- if (!(drivetrain.goal.MakeWithBuilder()
- .steering(wheel)
- .throttle(throttle)
- .highgear(is_high_gear).quickturn(data.IsPressed(kQuickTurn))
- .control_loop_driving(is_control_loop_driving)
- .left_goal(left_goal).right_goal(right_goal).Send())) {
+ if (!drivetrain.goal.MakeWithBuilder()
+ .steering(wheel)
+ .throttle(throttle)
+ .highgear(is_high_gear).quickturn(data.IsPressed(kQuickTurn))
+ .control_loop_driving(is_control_loop_driving)
+ .left_goal(left_goal).right_goal(right_goal).Send()) {
LOG(WARNING, "sending stick values failed\n");
}
@@ -104,6 +105,20 @@
if (data.PosEdge(kShiftLow)) {
is_high_gear = true;
}
+
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(0)
+ .separation_angle(0)
+ .intake(false).Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ }
+ if (!control_loops::shooter_queue_group.goal.MakeWithBuilder()
+ .shot_power(0)
+ .shot_requested(false)
+ .unload_requested(true)
+ .Send()) {
+ LOG(WARNING, "sending shooter goal failed\n");
+ }
}
}
};
diff --git a/frc971/input/sensor_receiver.cc b/frc971/input/sensor_receiver.cc
index a7005a4..ff4fdf6 100644
--- a/frc971/input/sensor_receiver.cc
+++ b/frc971/input/sensor_receiver.cc
@@ -80,7 +80,8 @@
return in;
}
-void CopyHallEffectEdges(HallEffectStruct *output,
+template<typename Structure>
+void CopyHallEffectEdges(Structure *output,
const ::bbb::HallEffectEdges &input,
State::HallEffectCounters *state) {
output->posedge_count = state->posedges.Update(input.posedges);
@@ -153,9 +154,7 @@
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;
+ shooter_position->plunger = 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;
@@ -163,15 +162,13 @@
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->latch = 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->pusher_distal.posedge_value =
+ shooter_translate(data->main.pusher_distal_posedge_position);
+ shooter_position->pusher_proximal.posedge_value =
+ shooter_translate(data->main.pusher_proximal_posedge_position);
shooter_position.Send();
}
diff --git a/frc971/input/usb.gyp b/frc971/input/usb.gyp
deleted file mode 100644
index 72e9b6b..0000000
--- a/frc971/input/usb.gyp
+++ /dev/null
@@ -1,31 +0,0 @@
-# This file is needed because gyp wants to drag in all of the targets in a file
-# which is a problem because the main and bot3 code contain some executables
-# with the same names.
-{
- 'targets': [
- {
- 'target_name': 'usb_receiver',
- 'type': 'static_library',
- 'sources': [
- 'usb_receiver.cc',
- ],
- 'dependencies': [
- '<(DEPTH)/gyro_board/src/libusb-driver/libusb-driver.gyp:libusb_wrap',
- '<(AOS)/build/aos.gyp:logging',
- '<(AOS)/common/common.gyp:time',
- '<(AOS)/common/common.gyp:controls',
- ],
- 'export_dependent_settings': [
- '<(DEPTH)/gyro_board/src/libusb-driver/libusb-driver.gyp:libusb_wrap',
- '<(AOS)/common/common.gyp:time',
- ],
- 'variables': {
- # TODO(brians): Add dependency on this file too (or something).
- 'checksum': '<!(<(DEPTH)/gyro_board/src/usb/data_struct_checksum.sh)',
- },
- 'defines': [
- 'GYRO_BOARD_DATA_CHECKSUM=<(checksum)',
- ],
- },
- ],
-}
diff --git a/frc971/output/motor_writer.cc b/frc971/output/motor_writer.cc
index 0af5e2b..44525cb 100644
--- a/frc971/output/motor_writer.cc
+++ b/frc971/output/motor_writer.cc
@@ -10,6 +10,8 @@
#include "aos/common/logging/queue_logging.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/claw/claw.q.h"
+#include "frc971/control_loops/shooter/shooter.q.h"
using ::aos::util::SimpleLogInterval;
@@ -32,28 +34,64 @@
if (true) {
drivetrain.output.FetchLatest();
- if (drivetrain.output.get()) {
- LOG_STRUCT(DEBUG, "will output", *drivetrain.output.get());
- }
if (drivetrain.output.IsNewerThanMS(kOutputMaxAgeMS)) {
- SetPWMOutput(2, drivetrain.output->right_voltage / 12.0, kTalonBounds);
+ LOG_STRUCT(DEBUG, "will output", *drivetrain.output.get());
SetPWMOutput(3, drivetrain.output->right_voltage / 12.0, kTalonBounds);
- SetPWMOutput(5, -drivetrain.output->left_voltage / 12.0, kTalonBounds);
- SetPWMOutput(6, -drivetrain.output->left_voltage / 12.0, kTalonBounds);
- SetSolenoid(1, drivetrain.output->left_high);
- SetSolenoid(2, drivetrain.output->right_high);
+ SetPWMOutput(8, -drivetrain.output->left_voltage / 12.0, kTalonBounds);
+ SetSolenoid(7, drivetrain.output->left_high);
+ SetSolenoid(8, drivetrain.output->right_high);
} else {
- DisablePWMOutput(2);
DisablePWMOutput(3);
- DisablePWMOutput(5);
- DisablePWMOutput(6);
+ DisablePWMOutput(8);
LOG_INTERVAL(drivetrain_old_);
}
}
+
+ {
+ static auto &shooter =
+ ::frc971::control_loops::shooter_queue_group.output;
+ shooter.FetchLatest();
+ if (shooter.IsNewerThanMS(kOutputMaxAgeMS)) {
+ LOG_STRUCT(DEBUG, "will output", *shooter.get());
+ SetPWMOutput(9, shooter->voltage / 12.0, kTalonBounds);
+ SetSolenoid(6, !shooter->latch_piston);
+ SetSolenoid(5, !shooter->brake_piston);
+ } else {
+ DisablePWMOutput(9);
+ SetSolenoid(5, false); // engage the brake
+ LOG_INTERVAL(shooter_old_);
+ }
+ }
+
+ {
+ static auto &claw = ::frc971::control_loops::claw_queue_group.output;
+ claw.FetchLatest();
+ if (claw.IsNewerThanMS(kOutputMaxAgeMS)) {
+ LOG_STRUCT(DEBUG, "will output", *claw.get());
+ SetPWMOutput(6, claw->intake_voltage / 12.0, kTalonBounds);
+ SetPWMOutput(7, claw->intake_voltage / 12.0, kTalonBounds);
+ SetPWMOutput(1, claw->bottom_claw_voltage / 12.0, kTalonBounds);
+ SetPWMOutput(2, claw->top_claw_voltage / 12.0, kTalonBounds);
+ SetPWMOutput(5, claw->tusk_voltage / 12.0, kTalonBounds); // left
+ SetPWMOutput(4, claw->tusk_voltage / 12.0, kTalonBounds); // right
+ } else {
+ DisablePWMOutput(6);
+ DisablePWMOutput(7);
+ DisablePWMOutput(1);
+ DisablePWMOutput(2);
+ DisablePWMOutput(4);
+ DisablePWMOutput(5);
+ LOG_INTERVAL(claw_old_);
+ }
+ }
}
SimpleLogInterval drivetrain_old_ =
SimpleLogInterval(kOldLogInterval, WARNING, "drivetrain too old");
+ SimpleLogInterval shooter_old_ =
+ SimpleLogInterval(kOldLogInterval, WARNING, "shooter too old");
+ SimpleLogInterval claw_old_ =
+ SimpleLogInterval(kOldLogInterval, WARNING, "claw too old");
};
constexpr ::aos::time::Time MotorWriter::kOldLogInterval;
diff --git a/frc971/output/output.gyp b/frc971/output/output.gyp
index 19fb6d1..6faca12 100644
--- a/frc971/output/output.gyp
+++ b/frc971/output/output.gyp
@@ -38,6 +38,8 @@
'<(AOS)/common/util/util.gyp:log_interval',
'<(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/prime/scripts/start_list.txt b/frc971/prime/scripts/start_list.txt
index d7d20f4..179c122 100644
--- a/frc971/prime/scripts/start_list.txt
+++ b/frc971/prime/scripts/start_list.txt
@@ -4,3 +4,5 @@
drivetrain
auto
sensor_receiver
+claw
+shooter
diff --git a/frc971/queues/photo_sensor.q b/frc971/queues/photo_sensor.q
deleted file mode 100644
index 383bc25..0000000
--- a/frc971/queues/photo_sensor.q
+++ /dev/null
@@ -1,10 +0,0 @@
-package frc971.sensors;
-
-message BlockedSensor {
- bool blocked;
-};
-
-queue BlockedSensor bottom_ball_sensor;
-queue BlockedSensor top_ball_sensor;
-queue BlockedSensor left_bump;
-queue BlockedSensor right_bump;
diff --git a/frc971/queues/queues.gyp b/frc971/queues/queues.gyp
index a67ffe2..ac53a70 100644
--- a/frc971/queues/queues.gyp
+++ b/frc971/queues/queues.gyp
@@ -2,7 +2,6 @@
'variables': {
'queue_files': [
'gyro_angle.q',
- 'photo_sensor.q',
'to_log.q',
]
},