deleted all of the 2013 robot-specific code
diff --git a/frc971/input/JoystickReader.cc b/frc971/input/JoystickReader.cc
deleted file mode 100644
index 8f65cdd..0000000
--- a/frc971/input/JoystickReader.cc
+++ /dev/null
@@ -1,262 +0,0 @@
-#include <stdio.h>
-#include <string.h>
-#include <unistd.h>
-#include <math.h>
-
-#include "aos/linux_code/init.h"
-#include "aos/prime/input/joystick_input.h"
-#include "aos/common/logging/logging.h"
-
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/queues/GyroAngle.q.h"
-#include "frc971/queues/Piston.q.h"
-#include "frc971/control_loops/wrist/wrist_motor.q.h"
-#include "frc971/autonomous/auto.q.h"
-#include "frc971/control_loops/index/index_motor.q.h"
-#include "frc971/control_loops/shooter/shooter_motor.q.h"
-#include "frc971/control_loops/angle_adjust/angle_adjust_motor.q.h"
-#include "frc971/queues/CameraTarget.q.h"
-
-using ::frc971::control_loops::drivetrain;
-using ::frc971::control_loops::shifters;
-using ::frc971::sensors::gyro;
-using ::frc971::control_loops::wrist;
-using ::frc971::control_loops::index_loop;
-using ::frc971::control_loops::shooter;
-using ::frc971::control_loops::angle_adjust;
-using ::frc971::control_loops::hangers;
-using ::frc971::vision::target_angle;
-
-using ::aos::input::driver_station::ButtonLocation;
-using ::aos::input::driver_station::JoystickAxis;
-using ::aos::input::driver_station::ControlBit;
-
-namespace frc971 {
-namespace input {
-namespace joysticks {
-
-const ButtonLocation kDriveControlLoopEnable1(1, 7),
- kDriveControlLoopEnable2(1, 11);
-const JoystickAxis kSteeringWheel(1, 1), kDriveThrottle(2, 2);
-const ButtonLocation kShiftHigh(2, 1), kShiftLow(2, 3);
-const ButtonLocation kQuickTurn(1, 5);
-
-const ButtonLocation kLongShot(3, 5);
-const ButtonLocation kMediumShot(3, 3);
-const ButtonLocation kShortShot(3, 6);
-const ButtonLocation kPitShot1(2, 7), kPitShot2(2, 10);
-
-const ButtonLocation kWristDown(3, 8);
-
-const ButtonLocation kFire(3, 11);
-const ButtonLocation kIntake(3, 10);
-const ButtonLocation kForceFire(3, 12);
-const ButtonLocation kForceIndexUp(3, 9), kForceIndexDown(3, 7);
-const ButtonLocation kForceSpitOut(2, 11);
-
-const ButtonLocation kDeployHangers(3, 1);
-
-class Reader : public ::aos::input::JoystickInput {
- public:
- static const bool kWristAlwaysDown = false;
-
- Reader() {
- shifters.MakeWithBuilder().set(true).Send();
- }
-
- virtual void RunIteration(const ::aos::input::driver_station::Data &data) {
- static bool is_high_gear = false;
- static double angle_adjust_goal = 0.42;
-
- if (data.GetControlBit(ControlBit::kAutonomous)) {
- if (data.PosEdge(ControlBit::kEnabled)){
- LOG(INFO, "Starting auto mode\n");
- ::frc971::autonomous::autonomous.MakeWithBuilder()
- .run_auto(true).Send();
- } else if (data.NegEdge(ControlBit::kEnabled)) {
- LOG(INFO, "Stopping auto mode\n");
- ::frc971::autonomous::autonomous.MakeWithBuilder()
- .run_auto(false).Send();
- }
- } else { // teleop
- bool is_control_loop_driving = false;
- double left_goal = 0.0;
- 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)) {
- static double distance = 0.0;
- static double angle = 0.0;
- static double filtered_goal_distance = 0.0;
- if (data.PosEdge(kDriveControlLoopEnable1) ||
- data.PosEdge(kDriveControlLoopEnable2)) {
- if (drivetrain.position.FetchLatest() && gyro.FetchLatest()) {
- distance = (drivetrain.position->left_encoder +
- drivetrain.position->right_encoder) / 2.0
- - throttle * kThrottleGain / 2.0;
- angle = gyro->angle;
- filtered_goal_distance = distance;
- }
- }
- is_control_loop_driving = true;
-
- //const double gyro_angle = Gyro.View().angle;
- const double goal_theta = angle - wheel * 0.27;
- const double goal_distance = distance + throttle * kThrottleGain;
- const double robot_width = 22.0 / 100.0 * 2.54;
- const double kMaxVelocity = 0.6;
- if (goal_distance > kMaxVelocity * 0.02 + filtered_goal_distance) {
- filtered_goal_distance += kMaxVelocity * 0.02;
- } else if (goal_distance < -kMaxVelocity * 0.02 +
- filtered_goal_distance) {
- filtered_goal_distance -= kMaxVelocity * 0.02;
- } else {
- filtered_goal_distance = goal_distance;
- }
- left_goal = filtered_goal_distance - robot_width * goal_theta / 2.0;
- right_goal = filtered_goal_distance + robot_width * goal_theta / 2.0;
- is_high_gear = false;
-
- 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())) {
- LOG(WARNING, "sending stick values failed\n");
- }
-
- if (data.PosEdge(kShiftHigh)) {
- is_high_gear = false;
- }
- if (data.PosEdge(kShiftLow)) {
- is_high_gear = true;
- }
-
- // Whether we should change wrist positions to indicate that the hopper is
- // clear.
- bool hopper_clear = false;
-
- // Where the wrist should be to pick up a frisbee.
- // TODO(brians): Make these globally accessible and clean up auto.
- static const double kWristPickup = -0.580;
- static const double kWristNearGround = -0.4;
- // Where the wrist gets stored when up.
- // All the way up is 1.5.
- static const double kWristUp = 1.43;
- static const double kWristCleared = kWristUp - 0.2;
- static double wrist_down_position = kWristPickup;
- double wrist_up_position = kWristUp;
- double wrist_pickup_position = data.IsPressed(kIntake) ?
- kWristPickup : kWristNearGround;
- if (index_loop.status.FetchLatest() || index_loop.status.get()) {
- if (index_loop.status->hopper_disc_count >= 4) {
- wrist_down_position = kWristNearGround;
- } else {
- wrist_down_position = wrist_pickup_position;
- }
- hopper_clear = index_loop.status->hopper_clear;
- }
-
- ::aos::ScopedMessagePtr<control_loops::ShooterLoop::Goal> shooter_goal =
- shooter.goal.MakeMessage();
- shooter_goal->velocity = 0;
- if (data.IsPressed(kPitShot1) && data.IsPressed(kPitShot2)) {
- shooter_goal->velocity = 131;
- if (hopper_clear) wrist_up_position = kWristCleared;
- angle_adjust_goal = 0.70;
- } else if (data.IsPressed(kLongShot)) {
-#if 0
- target_angle.FetchLatest();
- if (target_angle.IsNewerThanMS(500)) {
- shooter_goal->velocity = target_angle->shooter_speed;
- angle_adjust_goal = target_angle->shooter_angle;
- // TODO(brians): do the math right here
- if (!hopper_clear) wrist_up_position = 0.70;
- } else {
- LOG(WARNING, "camera frame too old\n");
- // Pretend like no button is pressed.
- }
-#endif
- } else if (data.IsPressed(kMediumShot)) {
- // middle wheel on the back line (same as auto)
- shooter_goal->velocity = 395;
- if (!hopper_clear) wrist_up_position = 1.23 - 0.4;
- angle_adjust_goal = 0.520;
- } else if (data.IsPressed(kShortShot)) {
- shooter_goal->velocity = 375;
- if (hopper_clear) wrist_up_position = kWristCleared;
- angle_adjust_goal = 0.671;
- }
- angle_adjust.goal.MakeWithBuilder().goal(angle_adjust_goal).Send();
-
- wrist.goal.MakeWithBuilder()
- .goal(data.IsPressed(kWristDown) ?
- wrist_down_position :
- wrist_up_position)
- .Send();
-
- ::aos::ScopedMessagePtr<control_loops::IndexLoop::Goal> index_goal =
- index_loop.goal.MakeMessage();
- if (data.IsPressed(kFire)) {
- // FIRE
- index_goal->goal_state = 4;
- } else if (shooter_goal->velocity != 0) {
- // get ready to shoot
- index_goal->goal_state = 3;
- } else if (data.IsPressed(kIntake)) {
- // intake
- index_goal->goal_state = 2;
- } else {
- // get ready to intake
- index_goal->goal_state = 1;
- }
- index_goal->force_fire = data.IsPressed(kForceFire);
-
- const bool index_up = data.IsPressed(kForceIndexUp);
- const bool index_down = data.IsPressed(kForceIndexDown);
- const bool spit_out = data.IsPressed(kForceSpitOut);
- index_goal->override_index = index_up || index_down || spit_out;
- index_goal->override_transfer = spit_out;
- if (index_up && index_down) {
- index_goal->index_voltage = 0.0;
- } else if (index_up) {
- index_goal->index_voltage = 12.0;
- } else if (index_down) {
- index_goal->index_voltage = -12.0;
- }
- if (spit_out) {
- index_goal->index_voltage = -12.0;
- index_goal->transfer_voltage = -12.0;
- }
-
- index_goal.Send();
- shooter_goal.Send();
- }
-
- static int hanger_cycles = 0;
- if (data.IsPressed(kDeployHangers)) {
- ++hanger_cycles;
- angle_adjust_goal = 0.4;
- } else {
- hanger_cycles = 0;
- }
- hangers.MakeWithBuilder().set(hanger_cycles >= 10).Send();
- }
-};
-
-} // namespace joysticks
-} // namespace input
-} // namespace frc971
-
-int main() {
- ::aos::Init();
- ::frc971::input::joysticks::Reader reader;
- reader.Run();
- ::aos::Cleanup();
-}
diff --git a/frc971/input/input.gyp b/frc971/input/input.gyp
index 33bf3c4..0fef215 100644
--- a/frc971/input/input.gyp
+++ b/frc971/input/input.gyp
@@ -1,22 +1,17 @@
{
'targets': [
{
- 'target_name': 'JoystickReader',
+ 'target_name': 'joystick_reader',
'type': 'executable',
'sources': [
- 'JoystickReader.cc',
+ 'joystick_reader.cc',
],
'dependencies': [
'<(AOS)/prime/input/input.gyp:joystick_input',
'<(AOS)/linux_code/linux_code.gyp:init',
'<(AOS)/build/aos.gyp:logging',
- '<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
'<(DEPTH)/frc971/queues/queues.gyp:queues',
- '<(DEPTH)/frc971/control_loops/angle_adjust/angle_adjust.gyp:angle_adjust_loop',
- '<(DEPTH)/frc971/control_loops/wrist/wrist.gyp:wrist_loop',
- '<(DEPTH)/frc971/control_loops/index/index.gyp:index_loop',
- '<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
'<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
'<(DEPTH)/frc971/autonomous/autonomous.gyp:auto_queue',
],
@@ -30,10 +25,6 @@
'dependencies': [
'<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
'<(DEPTH)/frc971/queues/queues.gyp:queues',
- '<(DEPTH)/frc971/control_loops/angle_adjust/angle_adjust.gyp:angle_adjust_loop',
- '<(DEPTH)/frc971/control_loops/wrist/wrist.gyp:wrist_loop',
- '<(DEPTH)/frc971/control_loops/index/index.gyp:index_loop',
- '<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
'<(AOS)/linux_code/linux_code.gyp:init',
'<(AOS)/build/aos.gyp:logging',
'<(AOS)/common/util/util.gyp:wrapping_counter',
diff --git a/frc971/input/joystick_reader.cc b/frc971/input/joystick_reader.cc
new file mode 100644
index 0000000..4372098
--- /dev/null
+++ b/frc971/input/joystick_reader.cc
@@ -0,0 +1,124 @@
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+#include <math.h>
+
+#include "aos/linux_code/init.h"
+#include "aos/prime/input/joystick_input.h"
+#include "aos/common/logging/logging.h"
+
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/queues/gyro_angle.q.h"
+#include "frc971/queues/piston.q.h"
+#include "frc971/autonomous/auto.q.h"
+
+using ::frc971::control_loops::drivetrain;
+using ::frc971::control_loops::shifters;
+using ::frc971::sensors::gyro;
+
+using ::aos::input::driver_station::ButtonLocation;
+using ::aos::input::driver_station::JoystickAxis;
+using ::aos::input::driver_station::ControlBit;
+
+namespace frc971 {
+namespace input {
+namespace joysticks {
+
+const ButtonLocation kDriveControlLoopEnable1(1, 7),
+ kDriveControlLoopEnable2(1, 11);
+const JoystickAxis kSteeringWheel(1, 1), kDriveThrottle(2, 2);
+const ButtonLocation kShiftHigh(2, 1), kShiftLow(2, 3);
+const ButtonLocation kQuickTurn(1, 5);
+
+class Reader : public ::aos::input::JoystickInput {
+ public:
+ Reader() {
+ shifters.MakeWithBuilder().set(true).Send();
+ }
+
+ virtual void RunIteration(const ::aos::input::driver_station::Data &data) {
+ static bool is_high_gear = false;
+
+ if (data.GetControlBit(ControlBit::kAutonomous)) {
+ if (data.PosEdge(ControlBit::kEnabled)){
+ LOG(INFO, "Starting auto mode\n");
+ ::frc971::autonomous::autonomous.MakeWithBuilder()
+ .run_auto(true).Send();
+ } else if (data.NegEdge(ControlBit::kEnabled)) {
+ LOG(INFO, "Stopping auto mode\n");
+ ::frc971::autonomous::autonomous.MakeWithBuilder()
+ .run_auto(false).Send();
+ }
+ } else { // teleop
+ bool is_control_loop_driving = false;
+ double left_goal = 0.0;
+ 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)) {
+ static double distance = 0.0;
+ static double angle = 0.0;
+ static double filtered_goal_distance = 0.0;
+ if (data.PosEdge(kDriveControlLoopEnable1) ||
+ data.PosEdge(kDriveControlLoopEnable2)) {
+ if (drivetrain.position.FetchLatest() && gyro.FetchLatest()) {
+ distance = (drivetrain.position->left_encoder +
+ drivetrain.position->right_encoder) / 2.0
+ - throttle * kThrottleGain / 2.0;
+ angle = gyro->angle;
+ filtered_goal_distance = distance;
+ }
+ }
+ is_control_loop_driving = true;
+
+ //const double gyro_angle = Gyro.View().angle;
+ const double goal_theta = angle - wheel * 0.27;
+ const double goal_distance = distance + throttle * kThrottleGain;
+ const double robot_width = 22.0 / 100.0 * 2.54;
+ const double kMaxVelocity = 0.6;
+ if (goal_distance > kMaxVelocity * 0.02 + filtered_goal_distance) {
+ filtered_goal_distance += kMaxVelocity * 0.02;
+ } else if (goal_distance < -kMaxVelocity * 0.02 +
+ filtered_goal_distance) {
+ filtered_goal_distance -= kMaxVelocity * 0.02;
+ } else {
+ filtered_goal_distance = goal_distance;
+ }
+ left_goal = filtered_goal_distance - robot_width * goal_theta / 2.0;
+ right_goal = filtered_goal_distance + robot_width * goal_theta / 2.0;
+ is_high_gear = false;
+
+ 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())) {
+ LOG(WARNING, "sending stick values failed\n");
+ }
+
+ if (data.PosEdge(kShiftHigh)) {
+ is_high_gear = false;
+ }
+ if (data.PosEdge(kShiftLow)) {
+ is_high_gear = true;
+ }
+ }
+ }
+};
+
+} // namespace joysticks
+} // namespace input
+} // namespace frc971
+
+int main() {
+ ::aos::Init();
+ ::frc971::input::joysticks::Reader reader;
+ reader.Run();
+ ::aos::Cleanup();
+}
diff --git a/frc971/input/sensor_receiver.cc b/frc971/input/sensor_receiver.cc
index 2f5c450..eb2bb9a 100644
--- a/frc971/input/sensor_receiver.cc
+++ b/frc971/input/sensor_receiver.cc
@@ -8,11 +8,7 @@
#include "bbb/sensor_reader.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/control_loops/wrist/wrist_motor.q.h"
-#include "frc971/control_loops/angle_adjust/angle_adjust_motor.q.h"
-#include "frc971/control_loops/index/index_motor.q.h"
-#include "frc971/control_loops/shooter/shooter_motor.q.h"
-#include "frc971/queues/GyroAngle.q.h"
+#include "frc971/queues/gyro_angle.q.h"
#include "frc971/constants.h"
#ifndef M_PI
@@ -20,10 +16,6 @@
#endif
using ::frc971::control_loops::drivetrain;
-using ::frc971::control_loops::wrist;
-using ::frc971::control_loops::angle_adjust;
-using ::frc971::control_loops::shooter;
-using ::frc971::control_loops::index_loop;
using ::frc971::sensors::gyro;
using ::aos::util::WrappingCounter;
@@ -36,28 +28,6 @@
(3.5 /*wheel diameter*/ * 2.54 / 100.0 * M_PI);
}
-double wrist_translate(int32_t in) {
- return static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*quad*/) *
- (14.0 / 50.0 * 20.0 / 84.0) /*gears*/ * (2 * M_PI);
-}
-
-double angle_adjust_translate(int32_t in) {
- static const double kCableDiameter = 0.060;
- return -static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*quad*/) *
- ((0.75 + kCableDiameter) / (16.61125 + kCableDiameter)) /*pulleys*/ *
- (2 * M_PI);
-}
-
-double shooter_translate(int32_t in) {
- return static_cast<double>(in) / (32.0 /*cpr*/ * 4.0 /*quad*/) *
- (15.0 / 34.0) /*gears*/ * (2 * M_PI);
-}
-
-double index_translate(int32_t in) {
- return -static_cast<double>(in) / (128.0 /*cpr*/ * 4.0 /*quad*/) *
- (1.0) /*gears*/ * (2 * M_PI);
-}
-
// Translates values from the ADC into voltage.
double adc_translate(uint16_t in) {
static const double kVRefN = 0;
@@ -83,71 +53,6 @@
return (voltage - k.low) / (k.high - k.low);
}
-WrappingCounter top_rise_;
-WrappingCounter top_fall_;
-WrappingCounter bottom_rise_;
-WrappingCounter bottom_fall_delay_;
-WrappingCounter bottom_fall_;
-void ProcessData(const ::bbb::DataStruct *data, bool bad_gyro) {
- if (!bad_gyro) {
- gyro.MakeWithBuilder()
- .angle(gyro_translate(data->gyro_angle))
- .Send();
- }
-
- drivetrain.position.MakeWithBuilder()
- .right_encoder(drivetrain_translate(data->main.right_drive))
- .left_encoder(-drivetrain_translate(data->main.left_drive))
- .left_shifter_position(hall_translate(constants::GetValues().left_drive,
- data->main.left_drive_hall))
- .right_shifter_position(hall_translate(
- constants::GetValues().right_drive, data->main.right_drive_hall))
- .battery_voltage(battery_translate(data->main.battery_voltage))
- .Send();
-
- wrist.position.MakeWithBuilder()
- .pos(wrist_translate(data->main.wrist))
- .hall_effect(data->main.wrist_hall_effect)
- .calibration(wrist_translate(data->main.capture_wrist_rise))
- .Send();
-
- angle_adjust.position.MakeWithBuilder()
- .angle(angle_adjust_translate(data->main.shooter_angle))
- .bottom_hall_effect(data->main.angle_adjust_bottom_hall_effect)
- .middle_hall_effect(false)
- .bottom_calibration(angle_adjust_translate(
- data->main.capture_shooter_angle_rise))
- .middle_calibration(angle_adjust_translate(
- 0))
- .Send();
-
- shooter.position.MakeWithBuilder()
- .position(shooter_translate(data->main.shooter))
- .Send();
-
- index_loop.position.MakeWithBuilder()
- .index_position(index_translate(data->main.indexer))
- .top_disc_detect(data->main.top_disc)
- .top_disc_posedge_count(top_rise_.Update(data->main.top_rise_count))
- .top_disc_posedge_position(
- index_translate(data->main.capture_top_rise))
- .top_disc_negedge_count(top_fall_.Update(data->main.top_fall_count))
- .top_disc_negedge_position(
- index_translate(data->main.capture_top_fall))
- .bottom_disc_detect(data->main.bottom_disc)
- .bottom_disc_posedge_count(
- bottom_rise_.Update(data->main.bottom_rise_count))
- .bottom_disc_negedge_count(
- bottom_fall_.Update(data->main.bottom_fall_count))
- .bottom_disc_negedge_wait_position(index_translate(
- data->main.capture_bottom_fall_delay))
- .bottom_disc_negedge_wait_count(
- bottom_fall_delay_.Update(data->main.bottom_fall_delay_count))
- .loader_top(data->main.loader_top)
- .loader_bottom(data->main.loader_bottom)
- .Send();
-}
-
void PacketReceived(const ::bbb::DataStruct *data,
const ::aos::time::Time &cape_timestamp) {
LOG(DEBUG, "cape timestamp %010" PRId32 ".%09" PRId32 "s\n",
@@ -169,12 +74,22 @@
} else {
bad_gyro = false;
}
- static int i = 0;
- ++i;
- if (i == 5) {
- i = 0;
- ProcessData(data, bad_gyro);
+
+ if (!bad_gyro) {
+ gyro.MakeWithBuilder()
+ .angle(gyro_translate(data->gyro_angle))
+ .Send();
}
+
+ drivetrain.position.MakeWithBuilder()
+ .right_encoder(drivetrain_translate(data->main.right_drive))
+ .left_encoder(-drivetrain_translate(data->main.left_drive))
+ .left_shifter_position(hall_translate(constants::GetValues().left_drive,
+ data->main.left_drive_hall))
+ .right_shifter_position(hall_translate(
+ constants::GetValues().right_drive, data->main.right_drive_hall))
+ .battery_voltage(battery_translate(data->main.battery_voltage))
+ .Send();
}
} // namespace