got all of the I/O hooked up (I think)
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',
]
},