Finally bring in completely revamped gyro code.
The gyro code should now be in its final state for the third robot.
Conflicts:
bot3/input/gyro_reader.cc
frc971/atom_code/atom_code.gyp
frc971/atom_code/build.sh
frc971/crio/build.sh
frc971/input/input.gyp
diff --git a/.gitignore b/.gitignore
index 0879261..4b185e4 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,4 +1,5 @@
/out_atom/
+/out_bot3_atom
/out_crio/
/output/
*.pyc
diff --git a/aos/atom_code/output/motor_output.cc b/aos/atom_code/output/motor_output.cc
index 48acc38..e16bbb5 100644
--- a/aos/atom_code/output/motor_output.cc
+++ b/aos/atom_code/output/motor_output.cc
@@ -14,6 +14,8 @@
// going 2 to each side to make sure we get the full range
const MotorOutput::MotorControllerBounds MotorOutput::kTalonBounds
{213, 135, 132, 129, 50};
+const MotorOutput::MotorControllerBounds MotorOutput::kVictorBounds
+ {210, 138, 132, 126, 56};
uint8_t MotorOutput::MotorControllerBounds::Map(double value) const {
if (value == 0.0) return kCenter;
diff --git a/aos/atom_code/output/motor_output.h b/aos/atom_code/output/motor_output.h
index 8828f14..116e791 100644
--- a/aos/atom_code/output/motor_output.h
+++ b/aos/atom_code/output/motor_output.h
@@ -43,6 +43,8 @@
protected:
// Brian got the values here by trying values with hardware on 11/23/12.
static const MotorControllerBounds kTalonBounds;
+ // Taken from WPILib.
+ static const MotorControllerBounds kVictorBounds;
// Helper methods for filling out values_.
// All channels are the 1-indexed numbers that usually go into WPILib.
diff --git a/aos/build/build.sh b/aos/build/build.sh
index 9ec745a..8fef47d 100755
--- a/aos/build/build.sh
+++ b/aos/build/build.sh
@@ -9,7 +9,8 @@
PLATFORM=$1
GYP_MAIN=$2
DEBUG=$3
-ACTION=$4
+OUT_NAME=$4
+ACTION=$5
shift 3
shift || true # We might not have a 4th argument if ACTION is empty.
@@ -28,7 +29,7 @@
GYP_DIR=${AOS}/externals/gyp-${GYP_REVISION}
GYP=${GYP_DIR}/gyp
-OUTDIR=${AOS}/../out_${PLATFORM}
+OUTDIR=${AOS}/../out_${OUT_NAME}
BUILD_NINJA=${OUTDIR}/Default/build.ninja
[ -d ${NINJA_DIR} ] || git clone --branch ${NINJA_RELEASE} https://github.com/martine/ninja.git ${NINJA_DIR}
@@ -48,7 +49,7 @@
"`find ${AOS}/.. -newer ${BUILD_NINJA} \( -name '*.gyp' -or -name '*.gypi' \)`" ) ]]; then
${GYP} \
--check --depth=${AOS}/.. --no-circular-check -f ninja \
- -I${AOS}/build/aos.gypi -Goutput_dir=out_${PLATFORM} \
+ -I${AOS}/build/aos.gypi -Goutput_dir=out_${OUT_NAME} \
-DOS=${PLATFORM} -DWIND_BASE=${WIND_BASE} -DDEBUG=${DEBUG} \
${GYP_MAIN}
# Have to substitute "command = $compiler" so that it doesn't try to
diff --git a/bot3/atom_code/atom_code.gyp b/bot3/atom_code/atom_code.gyp
new file mode 100644
index 0000000..337e298
--- /dev/null
+++ b/bot3/atom_code/atom_code.gyp
@@ -0,0 +1,33 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'All',
+ 'type': 'none',
+ 'dependencies': [
+ '<(AOS)/build/aos_all.gyp:Atom',
+ '<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain',
+ '<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_lib_test',
+ #'<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_lib_test',
+ #'<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter',
+ '<(DEPTH)/bot3/autonomous/autonomous.gyp:auto',
+ '<(DEPTH)/bot3/input/input.gyp:joystick_reader',
+ '<(DEPTH)/bot3/input/input.gyp:gyro_reader',
+ #'../input/input.gyp:AutoMode',
+ '<(DEPTH)/bot3/output/output.gyp:MotorWriter',
+ '<(DEPTH)/bot3/output/output.gyp:CameraServer',
+ #'camera/camera.gyp:frc971',
+ '<(DEPTH)/gyro_board/src/libusb-driver/libusb-driver.gyp:get',
+ '<(DEPTH)/bot3/input/input.gyp:gyro_board_reader',
+ '<(DEPTH)/bot3/input/input.gyp:gyro_sensor_receiver',
+ ],
+ 'copies': [
+ {
+ 'destination': '<(rsync_dir)',
+ 'files': [
+ 'scripts/start_list.txt',
+ ],
+ },
+ ],
+ },
+ ],
+}
diff --git a/bot3/atom_code/build.sh b/bot3/atom_code/build.sh
new file mode 100755
index 0000000..a108cdf
--- /dev/null
+++ b/bot3/atom_code/build.sh
@@ -0,0 +1,3 @@
+#!/bin/bash
+
+../../aos/build/build.sh atom atom_code.gyp no bot3_atom $1
diff --git a/bot3/atom_code/scripts/start_list.txt b/bot3/atom_code/scripts/start_list.txt
new file mode 100644
index 0000000..83f541f
--- /dev/null
+++ b/bot3/atom_code/scripts/start_list.txt
@@ -0,0 +1,8 @@
+BinaryLogReader
+MotorWriter
+joystick_reader
+drivetrain
+CRIOLogReader
+shooter
+auto
+gyro_board_reader
diff --git a/bot3/autonomous/auto.cc b/bot3/autonomous/auto.cc
new file mode 100644
index 0000000..3167770
--- /dev/null
+++ b/bot3/autonomous/auto.cc
@@ -0,0 +1,26 @@
+#include "stdio.h"
+
+#include "aos/common/control_loop/Timing.h"
+#include "aos/common/time.h"
+#include "aos/common/util/trapezoid_profile.h"
+#include "aos/common/messages/RobotState.q.h"
+#include "aos/common/logging/logging.h"
+
+#include "bot3/autonomous/auto.q.h"
+#include "bot3/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/constants.h"
+
+using ::aos::time::Time;
+
+namespace bot3 {
+namespace autonomous {
+
+// start with N discs in the indexer
+void HandleAuto() {
+ // TODO (danielp): Do something in auto.
+ // (That's why I left all the includes.)
+ LOG(INFO, "Auto mode is not currently implemented on this robot.\n");
+}
+
+} // namespace autonomous
+} // namespace bot3
diff --git a/bot3/autonomous/auto.h b/bot3/autonomous/auto.h
new file mode 100644
index 0000000..d8a85a6
--- /dev/null
+++ b/bot3/autonomous/auto.h
@@ -0,0 +1,11 @@
+#ifndef BOT3_AUTONOMOUS_AUTO_H_
+#define BOT3_AUTONOMOUS_AUTO_H_
+
+namespace bot3 {
+namespace autonomous {
+
+void HandleAuto();
+
+} // namespace autonomous
+} // namespace bot3
+#endif // BOT3_AUTONOMOUS_AUTO_H_
diff --git a/bot3/autonomous/auto.q b/bot3/autonomous/auto.q
new file mode 100644
index 0000000..3636d31
--- /dev/null
+++ b/bot3/autonomous/auto.q
@@ -0,0 +1,8 @@
+package bot3.autonomous;
+
+message AutoControl {
+ // True if auto mode should be running, false otherwise.
+ bool run_auto;
+};
+
+queue AutoControl autonomous;
diff --git a/bot3/autonomous/auto_main.cc b/bot3/autonomous/auto_main.cc
new file mode 100644
index 0000000..333a67a
--- /dev/null
+++ b/bot3/autonomous/auto_main.cc
@@ -0,0 +1,39 @@
+#include "stdio.h"
+
+#include "aos/common/control_loop/Timing.h"
+#include "aos/common/time.h"
+#include "aos/atom_code/init.h"
+#include "aos/common/logging/logging.h"
+#include "bot3/autonomous/auto.q.h"
+#include "bot3/autonomous/auto.h"
+
+using ::aos::time::Time;
+
+int main(int /*argc*/, char * /*argv*/[]) {
+ ::aos::Init();
+
+ ::bot3::autonomous::autonomous.FetchLatest();
+ while (!::bot3::autonomous::autonomous.get()) {
+ ::bot3::autonomous::autonomous.FetchNextBlocking();
+ LOG(INFO, "Got another auto packet\n");
+ }
+
+ while (true) {
+ while (!::bot3::autonomous::autonomous->run_auto) {
+ ::bot3::autonomous::autonomous.FetchNextBlocking();
+ LOG(INFO, "Got another auto packet\n");
+ }
+ LOG(INFO, "Starting auto mode\n");
+ ::bot3::autonomous::HandleAuto();
+
+ LOG(INFO, "Auto mode exited, waiting for it to finish.\n");
+ while (::bot3::autonomous::autonomous->run_auto) {
+ ::bot3::autonomous::autonomous.FetchNextBlocking();
+ LOG(INFO, "Got another auto packet\n");
+ }
+ LOG(INFO, "Waiting for auto to start back up.\n");
+ }
+ ::aos::Cleanup();
+ return 0;
+}
+
diff --git a/bot3/autonomous/autonomous.gyp b/bot3/autonomous/autonomous.gyp
new file mode 100644
index 0000000..7e2d5da
--- /dev/null
+++ b/bot3/autonomous/autonomous.gyp
@@ -0,0 +1,51 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'auto_queue',
+ 'type': 'static_library',
+ 'sources': ['auto.q'],
+ 'variables': {
+ 'header_path': 'bot3/autonomous',
+ },
+ 'dependencies': [
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'includes': ['../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'auto_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'auto.cc',
+ ],
+ 'dependencies': [
+ 'auto_queue',
+ '<(AOS)/common/common.gyp:controls',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
+ '<(DEPTH)/frc971/frc971.gyp:common',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/common.gyp:timing',
+ '<(AOS)/common/util/util.gyp:trapezoid_profile',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/common.gyp:controls',
+ ],
+ },
+ {
+ 'target_name': 'auto',
+ 'type': 'executable',
+ 'sources': [
+ 'auto_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/atom_code/atom_code.gyp:init',
+ 'auto_queue',
+ 'auto_lib',
+ ],
+ },
+ ],
+}
diff --git a/bot3/control_loops/drivetrain/drivetrain.cc b/bot3/control_loops/drivetrain/drivetrain.cc
new file mode 100644
index 0000000..b0cb8b4
--- /dev/null
+++ b/bot3/control_loops/drivetrain/drivetrain.cc
@@ -0,0 +1,285 @@
+#include "bot3/control_loops/drivetrain/drivetrain.h"
+
+#include <stdio.h>
+#include <sched.h>
+#include <cmath>
+#include <memory>
+
+#include "aos/common/logging/logging.h"
+#include "aos/common/queue.h"
+#include "bot3/control_loops/drivetrain/drivetrain_motor_plant.h"
+#include "bot3/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/queues/GyroAngle.q.h"
+#include "frc971/queues/Piston.q.h"
+
+using frc971::sensors::gyro;
+using ::frc971::control_loops::shifters;
+
+namespace bot3 {
+namespace control_loops {
+
+// Width of the robot.
+const double width = 22.0 / 100.0 * 2.54;
+
+class DrivetrainMotorsSS {
+ public:
+ DrivetrainMotorsSS ()
+ : loop_(new StateFeedbackLoop<4, 2, 2>(MakeDrivetrainLoop())) {
+ _offset = 0;
+ _integral_offset = 0;
+ _left_goal = 0.0;
+ _right_goal = 0.0;
+ _raw_left = 0.0;
+ _raw_right = 0.0;
+ _control_loop_driving = false;
+ }
+ void SetGoal(double left, double left_velocity, double right, double right_velocity) {
+ _left_goal = left;
+ _right_goal = right;
+ loop_->R << left, left_velocity, right, right_velocity;
+ }
+ void SetRawPosition(double left, double right) {
+ _raw_right = right;
+ _raw_left = left;
+ loop_->Y << left, right;
+ }
+ void SetPosition(
+ double left, double right, double gyro, bool control_loop_driving) {
+ // Decay the offset quickly because this gyro is great.
+ _offset = (0.25) * (right - left - gyro * width) / 2.0 + 0.75 * _offset;
+ const double angle_error = (_right_goal - _left_goal) / width - (_raw_right - _offset - _raw_left - _offset) / width;
+ // TODO(aschuh): Add in the gyro.
+ _integral_offset = 0.0;
+ _offset = 0.0;
+ _gyro = gyro;
+ _control_loop_driving = control_loop_driving;
+ SetRawPosition(left, right);
+ LOG(DEBUG, "Left %f->%f Right %f->%f Gyro %f aerror %f ioff %f\n", left + _offset, _left_goal, right - _offset, _right_goal, gyro, angle_error, _integral_offset);
+ }
+
+ void Update(bool update_observer, bool stop_motors) {
+ loop_->Update(update_observer, stop_motors);
+ }
+
+ void SendMotors(Drivetrain::Output *output) {
+ if (output) {
+ output->left_voltage = loop_->U(0, 0);
+ output->right_voltage = loop_->U(1, 0);
+ }
+ }
+ void PrintMotors() const {
+ // LOG(DEBUG, "Left Power %f Right Power %f lg %f rg %f le %f re %f gyro %f\n", U[0], U[1], R[0], R[2], Y[0], Y[1], _gyro);
+ ::Eigen::Matrix<double, 4, 1> E = loop_->R - loop_->X_hat;
+ LOG(DEBUG, "E[0, 0]: %f E[1, 0] %f E[2, 0] %f E[3, 0] %f\n", E(0, 0), E(1, 0), E(2, 0), E(3, 0));
+ }
+
+ private:
+ ::std::unique_ptr<StateFeedbackLoop<4, 2, 2>> loop_;
+
+ double _integral_offset;
+ double _offset;
+ double _gyro;
+ double _left_goal;
+ double _right_goal;
+ double _raw_left;
+ double _raw_right;
+ bool _control_loop_driving;
+};
+
+class DrivetrainMotorsOL {
+ public:
+ DrivetrainMotorsOL() {
+ _old_wheel = 0.0;
+ _wheel = 0.0;
+ _throttle = 0.0;
+ _quickturn = false;
+ _highgear = true;
+ _neg_inertia_accumulator = 0.0;
+ _left_pwm = 0.0;
+ _right_pwm = 0.0;
+ }
+ void SetGoal(double wheel, double throttle, bool quickturn, bool highgear) {
+ _wheel = wheel;
+ _throttle = throttle;
+ _quickturn = quickturn;
+ _highgear = highgear;
+ _left_pwm = 0.0;
+ _right_pwm = 0.0;
+ }
+ void Update(void) {
+ double overPower;
+ float sensitivity = 1.7;
+ float angular_power;
+ float linear_power;
+ double wheel;
+
+ double neg_inertia = _wheel - _old_wheel;
+ _old_wheel = _wheel;
+
+ double wheelNonLinearity;
+ if (_highgear) {
+ wheelNonLinearity = 0.1; // used to be csvReader->TURN_NONLIN_HIGH
+ // Apply a sin function that's scaled to make it feel better.
+ const double angular_range = M_PI / 2.0 * wheelNonLinearity;
+ wheel = sin(angular_range * _wheel) / sin(angular_range);
+ wheel = sin(angular_range * wheel) / sin(angular_range);
+ } else {
+ wheelNonLinearity = 0.2; // used to be csvReader->TURN_NONLIN_LOW
+ // Apply a sin function that's scaled to make it feel better.
+ const double angular_range = M_PI / 2.0 * wheelNonLinearity;
+ wheel = sin(angular_range * _wheel) / sin(angular_range);
+ wheel = sin(angular_range * wheel) / sin(angular_range);
+ wheel = sin(angular_range * wheel) / sin(angular_range);
+ }
+
+ static const double kThrottleDeadband = 0.05;
+ if (::std::abs(_throttle) < kThrottleDeadband) {
+ _throttle = 0;
+ } else {
+ _throttle = copysign((::std::abs(_throttle) - kThrottleDeadband) /
+ (1.0 - kThrottleDeadband), _throttle);
+ }
+
+ double neg_inertia_scalar;
+ if (_highgear) {
+ neg_inertia_scalar = 8.0; // used to be csvReader->NEG_INTERTIA_HIGH
+ sensitivity = 1.22; // used to be csvReader->SENSE_HIGH
+ } else {
+ if (wheel * neg_inertia > 0) {
+ neg_inertia_scalar = 5; // used to be csvReader->NEG_INERTIA_LOW_MORE
+ } else {
+ if (::std::abs(wheel) > 0.65) {
+ neg_inertia_scalar = 5; // used to be csvReader->NEG_INTERTIA_LOW_LESS_EXT
+ } else {
+ neg_inertia_scalar = 5; // used to be csvReader->NEG_INTERTIA_LOW_LESS
+ }
+ }
+ sensitivity = 1.24; // used to be csvReader->SENSE_LOW
+ }
+ double neg_inertia_power = neg_inertia * neg_inertia_scalar;
+ _neg_inertia_accumulator += neg_inertia_power;
+
+ wheel = wheel + _neg_inertia_accumulator;
+ if (_neg_inertia_accumulator > 1) {
+ _neg_inertia_accumulator -= 1;
+ } else if (_neg_inertia_accumulator < -1) {
+ _neg_inertia_accumulator += 1;
+ } else {
+ _neg_inertia_accumulator = 0;
+ }
+
+ linear_power = _throttle;
+
+ if (_quickturn) {
+ double qt_angular_power = wheel;
+ if (::std::abs(linear_power) < 0.2) {
+ if (qt_angular_power > 1) qt_angular_power = 1.0;
+ if (qt_angular_power < -1) qt_angular_power = -1.0;
+ } else {
+ qt_angular_power = 0.0;
+ }
+ overPower = 1.0;
+ if (_highgear) {
+ sensitivity = 1.0;
+ } else {
+ sensitivity = 1.0;
+ }
+ angular_power = wheel;
+ } else {
+ overPower = 0.0;
+ angular_power = ::std::abs(_throttle) * wheel * sensitivity;
+ }
+
+ _right_pwm = _left_pwm = linear_power;
+ _left_pwm += angular_power;
+ _right_pwm -= angular_power;
+
+ if (_left_pwm > 1.0) {
+ _right_pwm -= overPower*(_left_pwm - 1.0);
+ _left_pwm = 1.0;
+ } else if (_right_pwm > 1.0) {
+ _left_pwm -= overPower*(_right_pwm - 1.0);
+ _right_pwm = 1.0;
+ } else if (_left_pwm < -1.0) {
+ _right_pwm += overPower*(-1.0 - _left_pwm);
+ _left_pwm = -1.0;
+ } else if (_right_pwm < -1.0) {
+ _left_pwm += overPower*(-1.0 - _right_pwm);
+ _right_pwm = -1.0;
+ }
+ }
+
+ void SendMotors(Drivetrain::Output *output) {
+ LOG(DEBUG, "left pwm: %f right pwm: %f wheel: %f throttle: %f\n",
+ _left_pwm, _right_pwm, _wheel, _throttle);
+ if (output) {
+ output->left_voltage = _left_pwm * 12.0;
+ output->right_voltage = _right_pwm * 12.0;
+ }
+ if (_highgear) {
+ shifters.MakeWithBuilder().set(false).Send();
+ } else {
+ shifters.MakeWithBuilder().set(true).Send();
+ }
+ }
+
+ private:
+ double _old_wheel;
+ double _wheel;
+ double _throttle;
+ bool _quickturn;
+ bool _highgear;
+ double _neg_inertia_accumulator;
+ double _left_pwm;
+ double _right_pwm;
+};
+
+void DrivetrainLoop::RunIteration(const Drivetrain::Goal *goal,
+ const Drivetrain::Position *position,
+ Drivetrain::Output *output,
+ Drivetrain::Status * /*status*/) {
+ // TODO(aschuh): These should be members of the class.
+ static DrivetrainMotorsSS dt_closedloop;
+ static DrivetrainMotorsOL dt_openloop;
+
+ bool bad_pos = false;
+ if (position == NULL) {
+ LOG(WARNING, "no position\n");
+ bad_pos = true;
+ }
+
+ double wheel = goal->steering;
+ double throttle = goal->throttle;
+ bool quickturn = goal->quickturn;
+ bool highgear = goal->highgear;
+
+ bool control_loop_driving = goal->control_loop_driving;
+ double left_goal = goal->left_goal;
+ double right_goal = goal->right_goal;
+
+ dt_closedloop.SetGoal(left_goal, goal->left_velocity_goal,
+ right_goal, goal->right_velocity_goal);
+ if (!bad_pos) {
+ const double left_encoder = position->left_encoder;
+ const double right_encoder = position->right_encoder;
+ if (gyro.FetchLatest()) {
+ dt_closedloop.SetPosition(left_encoder, right_encoder,
+ gyro->angle, control_loop_driving);
+ } else {
+ dt_closedloop.SetRawPosition(left_encoder, right_encoder);
+ }
+ }
+ dt_closedloop.Update(position, output == NULL);
+ //dt_closedloop.PrintMotors();
+ dt_openloop.SetGoal(wheel, throttle, quickturn, highgear);
+ dt_openloop.Update();
+ if (control_loop_driving) {
+ dt_closedloop.SendMotors(output);
+ } else {
+ dt_openloop.SendMotors(output);
+ }
+}
+
+} // namespace control_loops
+} // namespace bot3
diff --git a/bot3/control_loops/drivetrain/drivetrain.gyp b/bot3/control_loops/drivetrain/drivetrain.gyp
new file mode 100644
index 0000000..17e5a09
--- /dev/null
+++ b/bot3/control_loops/drivetrain/drivetrain.gyp
@@ -0,0 +1,68 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'drivetrain_loop',
+ 'type': 'static_library',
+ 'sources': ['drivetrain.q'],
+ 'variables': {
+ 'header_path': 'bot3/control_loops/drivetrain',
+ },
+ 'dependencies': [
+ '<(AOS)/common/common.gyp:control_loop_queues',
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/common.gyp:control_loop_queues',
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'includes': ['../../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'drivetrain_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'drivetrain.cc',
+ 'drivetrain_motor_plant.cc',
+ ],
+ 'dependencies': [
+ 'drivetrain_loop',
+ '<(AOS)/common/common.gyp:controls',
+ '<(DEPTH)/frc971/frc971.gyp:common',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(DEPTH)/frc971/queues/queues.gyp:queues',
+ ],
+ 'export_dependent_settings': [
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(AOS)/common/common.gyp:controls',
+ 'drivetrain_loop',
+ ],
+ },
+ {
+ 'target_name': 'drivetrain_lib_test',
+ 'type': 'executable',
+ 'sources': [
+ 'drivetrain_lib_test.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ 'drivetrain_loop',
+ 'drivetrain_lib',
+ '<(AOS)/common/common.gyp:queue_testutils',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(DEPTH)/frc971/queues/queues.gyp:queues',
+ ],
+ },
+ {
+ 'target_name': 'drivetrain',
+ 'type': 'executable',
+ 'sources': [
+ 'drivetrain_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/atom_code/atom_code.gyp:init',
+ 'drivetrain_lib',
+ 'drivetrain_loop',
+ ],
+ },
+ ],
+}
diff --git a/bot3/control_loops/drivetrain/drivetrain.h b/bot3/control_loops/drivetrain/drivetrain.h
new file mode 100644
index 0000000..03b3bd0
--- /dev/null
+++ b/bot3/control_loops/drivetrain/drivetrain.h
@@ -0,0 +1,32 @@
+#ifndef BOT3_CONTROL_LOOPS_DRIVETRAIN_H_
+#define BOT3_CONTROL_LOOPS_DRIVETRAIN_H_
+
+#include "aos/common/control_loop/ControlLoop.h"
+#include "bot3/control_loops/drivetrain/drivetrain.q.h"
+
+namespace bot3 {
+namespace control_loops {
+
+class DrivetrainLoop
+ : public aos::control_loops::ControlLoop<control_loops::Drivetrain> {
+ public:
+ // Constructs a control loop which can take a Drivetrain or defaults to the
+ // drivetrain at frc971::control_loops::drivetrain
+ explicit DrivetrainLoop(
+ control_loops::Drivetrain *my_drivetrain = &control_loops::drivetrain)
+ : aos::control_loops::ControlLoop<control_loops::Drivetrain>(
+ my_drivetrain) {}
+
+ protected:
+ // Executes one cycle of the control loop.
+ virtual void RunIteration(
+ const control_loops::Drivetrain::Goal *goal,
+ const control_loops::Drivetrain::Position *position,
+ control_loops::Drivetrain::Output *output,
+ control_loops::Drivetrain::Status *status);
+};
+
+} // namespace control_loops
+} // namespace bot3
+
+#endif // BOT3_CONTROL_LOOPS_DRIVETRAIN_H_
diff --git a/bot3/control_loops/drivetrain/drivetrain.q b/bot3/control_loops/drivetrain/drivetrain.q
new file mode 100644
index 0000000..d082e13
--- /dev/null
+++ b/bot3/control_loops/drivetrain/drivetrain.q
@@ -0,0 +1,40 @@
+package bot3.control_loops;
+
+import "aos/common/control_loop/control_loops.q";
+
+queue_group Drivetrain {
+ implements aos.control_loops.ControlLoop;
+
+ message Goal {
+ float steering;
+ float throttle;
+ bool highgear;
+ bool quickturn;
+ bool control_loop_driving;
+ float left_goal;
+ float left_velocity_goal;
+ float right_goal;
+ float right_velocity_goal;
+ };
+
+ message Position {
+ double left_encoder;
+ double right_encoder;
+ };
+
+ message Output {
+ float left_voltage;
+ float right_voltage;
+ };
+
+ message Status {
+ bool is_done;
+ };
+
+ queue Goal goal;
+ queue Position position;
+ queue Output output;
+ queue Status status;
+};
+
+queue_group Drivetrain drivetrain;
diff --git a/bot3/control_loops/drivetrain/drivetrain_lib_test.cc b/bot3/control_loops/drivetrain/drivetrain_lib_test.cc
new file mode 100644
index 0000000..931b9ad
--- /dev/null
+++ b/bot3/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -0,0 +1,187 @@
+#include <unistd.h>
+
+#include <memory>
+
+#include "gtest/gtest.h"
+#include "aos/common/queue.h"
+#include "aos/common/queue_testutils.h"
+#include "bot3/control_loops/drivetrain/drivetrain.q.h"
+#include "bot3/control_loops/drivetrain/drivetrain.h"
+#include "bot3/control_loops/drivetrain/drivetrain_motor_plant.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/queues/GyroAngle.q.h"
+
+
+using ::aos::time::Time;
+
+namespace bot3 {
+namespace control_loops {
+namespace testing {
+
+
+// Class which simulates the drivetrain and sends out queue messages containing the
+// position.
+class DrivetrainSimulation {
+ public:
+ // Constructs a motor simulation.
+ DrivetrainSimulation()
+ : drivetrain_plant_(
+ new StateFeedbackPlant<4, 2, 2>(MakeDrivetrainPlant())),
+ my_drivetrain_loop_(".frc971.control_loops.drivetrain",
+ 0x8a8dde77, ".frc971.control_loops.drivetrain.goal",
+ ".frc971.control_loops.drivetrain.position",
+ ".frc971.control_loops.drivetrain.output",
+ ".frc971.control_loops.drivetrain.status") {
+ Reinitialize();
+ }
+
+ // Resets the plant.
+ void Reinitialize() {
+ drivetrain_plant_->X(0, 0) = 0.0;
+ drivetrain_plant_->X(1, 0) = 0.0;
+ drivetrain_plant_->Y = drivetrain_plant_->C() * drivetrain_plant_->X;
+ last_left_position_ = drivetrain_plant_->Y(0, 0);
+ last_right_position_ = drivetrain_plant_->Y(1, 0);
+ }
+
+ // Returns the position of the drivetrain.
+ double GetLeftPosition() const {
+ return drivetrain_plant_->Y(0, 0);
+ }
+ double GetRightPosition() const {
+ return drivetrain_plant_->Y(1, 0);
+ }
+
+ // Sends out the position queue messages.
+ void SendPositionMessage() {
+ const double left_encoder = GetLeftPosition();
+ const double right_encoder = GetRightPosition();
+
+ ::aos::ScopedMessagePtr<control_loops::Drivetrain::Position> position =
+ my_drivetrain_loop_.position.MakeMessage();
+ position->left_encoder = left_encoder;
+ position->right_encoder = right_encoder;
+ position.Send();
+ }
+
+ // Simulates the drivetrain moving for one timestep.
+ void Simulate() {
+ last_left_position_ = drivetrain_plant_->Y(0, 0);
+ last_right_position_ = drivetrain_plant_->Y(1, 0);
+ EXPECT_TRUE(my_drivetrain_loop_.output.FetchLatest());
+ drivetrain_plant_->U << my_drivetrain_loop_.output->left_voltage,
+ my_drivetrain_loop_.output->right_voltage;
+ drivetrain_plant_->Update();
+ }
+
+ ::std::unique_ptr<StateFeedbackPlant<4, 2, 2>> drivetrain_plant_;
+ private:
+ Drivetrain my_drivetrain_loop_;
+ double last_left_position_;
+ double last_right_position_;
+};
+
+class DrivetrainTest : public ::testing::Test {
+ protected:
+ ::aos::common::testing::GlobalCoreInstance my_core;
+
+ // Create a new instance of the test queue so that it invalidates the queue
+ // that it points to. Otherwise, we will have a pointer to shared memory that
+ // is no longer valid.
+ Drivetrain my_drivetrain_loop_;
+
+ // Create a loop and simulation plant.
+ DrivetrainLoop drivetrain_motor_;
+ DrivetrainSimulation drivetrain_motor_plant_;
+
+ DrivetrainTest() : my_drivetrain_loop_(".frc971.control_loops.drivetrain",
+ 0x8a8dde77,
+ ".frc971.control_loops.drivetrain.goal",
+ ".frc971.control_loops.drivetrain.position",
+ ".frc971.control_loops.drivetrain.output",
+ ".frc971.control_loops.drivetrain.status"),
+ drivetrain_motor_(&my_drivetrain_loop_),
+ drivetrain_motor_plant_() {
+ // Flush the robot state queue so we can use clean shared memory for this
+ // test, also for the gyro.
+ ::aos::robot_state.Clear();
+ ::frc971::sensors::gyro.Clear();
+ SendDSPacket(true);
+ }
+
+ void SendDSPacket(bool enabled) {
+ ::aos::robot_state.MakeWithBuilder().enabled(enabled)
+ .autonomous(false)
+ .team_id(971).Send();
+ ::aos::robot_state.FetchLatest();
+ }
+
+ void VerifyNearGoal() {
+ my_drivetrain_loop_.goal.FetchLatest();
+ my_drivetrain_loop_.position.FetchLatest();
+ EXPECT_NEAR(my_drivetrain_loop_.goal->left_goal,
+ drivetrain_motor_plant_.GetLeftPosition(),
+ 1e-2);
+ EXPECT_NEAR(my_drivetrain_loop_.goal->right_goal,
+ drivetrain_motor_plant_.GetRightPosition(),
+ 1e-2);
+ }
+
+ virtual ~DrivetrainTest() {
+ ::aos::robot_state.Clear();
+ ::frc971::sensors::gyro.Clear();
+ }
+};
+
+// Tests that the drivetrain converges on a goal.
+TEST_F(DrivetrainTest, ConvergesCorrectly) {
+ my_drivetrain_loop_.goal.MakeWithBuilder().control_loop_driving(true)
+ .left_goal(-1.0)
+ .right_goal(1.0).Send();
+ for (int i = 0; i < 200; ++i) {
+ drivetrain_motor_plant_.SendPositionMessage();
+ drivetrain_motor_.Iterate();
+ drivetrain_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+}
+
+// Tests that it survives disabling.
+TEST_F(DrivetrainTest, SurvivesDisabling) {
+ my_drivetrain_loop_.goal.MakeWithBuilder().control_loop_driving(true)
+ .left_goal(-1.0)
+ .right_goal(1.0).Send();
+ for (int i = 0; i < 500; ++i) {
+ drivetrain_motor_plant_.SendPositionMessage();
+ drivetrain_motor_.Iterate();
+ drivetrain_motor_plant_.Simulate();
+ if (i > 20 && i < 200) {
+ SendDSPacket(false);
+ } else {
+ SendDSPacket(true);
+ }
+ }
+ VerifyNearGoal();
+}
+
+// Tests surviving bad positions.
+TEST_F(DrivetrainTest, SurvivesBadPosition) {
+ my_drivetrain_loop_.goal.MakeWithBuilder().control_loop_driving(true)
+ .left_goal(-1.0)
+ .right_goal(1.0).Send();
+ for (int i = 0; i < 500; ++i) {
+ if (i > 20 && i < 200) {
+ } else {
+ drivetrain_motor_plant_.SendPositionMessage();
+ }
+ drivetrain_motor_.Iterate();
+ drivetrain_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+}
+
+} // namespace testing
+} // namespace control_loops
+} // namespace bot3
diff --git a/bot3/control_loops/drivetrain/drivetrain_main.cc b/bot3/control_loops/drivetrain/drivetrain_main.cc
new file mode 100644
index 0000000..5ef00bf
--- /dev/null
+++ b/bot3/control_loops/drivetrain/drivetrain_main.cc
@@ -0,0 +1,11 @@
+#include "bot3/control_loops/drivetrain/drivetrain.h"
+
+#include "aos/atom_code/init.h"
+
+int main() {
+ ::aos::Init();
+ bot3::control_loops::DrivetrainLoop drivetrain;
+ drivetrain.Run();
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/bot3/control_loops/drivetrain/drivetrain_motor_plant.cc b/bot3/control_loops/drivetrain/drivetrain_motor_plant.cc
new file mode 100644
index 0000000..1a6989a
--- /dev/null
+++ b/bot3/control_loops/drivetrain/drivetrain_motor_plant.cc
@@ -0,0 +1,47 @@
+#include "bot3/control_loops/drivetrain/drivetrain_motor_plant.h"
+
+#include <vector>
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace bot3 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainPlantCoefficients() {
+ Eigen::Matrix<double, 4, 4> A;
+ A << 1.0, 0.0095410093, 0.0, -0.0000167223, 0.0, 0.9096302600, 0.0, -0.0032396985, 0.0, -0.0000167223, 1.0, 0.0095410093, 0.0, -0.0032396985, 0.0, 0.9096302600;
+ Eigen::Matrix<double, 4, 2> B;
+ B << 0.0000628338, 0.0000022892, 0.0123712263, 0.0004435007, 0.0000022892, 0.0000628338, 0.0004435007, 0.0123712263;
+ Eigen::Matrix<double, 2, 4> C;
+ C << 1, 0, 0, 0, 0, 0, 1, 0;
+ Eigen::Matrix<double, 2, 2> D;
+ D << 0, 0, 0, 0;
+ Eigen::Matrix<double, 2, 1> U_max;
+ U_max << 12.0, 12.0;
+ Eigen::Matrix<double, 2, 1> U_min;
+ U_min << -12.0, -12.0;
+ return StateFeedbackPlantCoefficients<4, 2, 2>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackController<4, 2, 2> MakeDrivetrainController() {
+ Eigen::Matrix<double, 4, 2> L;
+ L << 1.7496302600, -0.0032396985, 72.1296388532, -0.4369906587, -0.0032396985, 1.7496302600, -0.4369906586, 72.1296388532;
+ Eigen::Matrix<double, 2, 4> K;
+ K << 242.8102455120, 19.7898401032, -8.7045950610, -0.9720464423, -8.7045950610, -0.9720464423, 242.8102455120, 19.7898401032;
+ return StateFeedbackController<4, 2, 2>(L, K, MakeDrivetrainPlantCoefficients());
+}
+
+StateFeedbackPlant<4, 2, 2> MakeDrivetrainPlant() {
+ ::std::vector<StateFeedbackPlantCoefficients<4, 2, 2> *> plants(1);
+ plants[0] = new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDrivetrainPlantCoefficients());
+ return StateFeedbackPlant<4, 2, 2>(plants);
+}
+
+StateFeedbackLoop<4, 2, 2> MakeDrivetrainLoop() {
+ ::std::vector<StateFeedbackController<4, 2, 2> *> controllers(1);
+ controllers[0] = new StateFeedbackController<4, 2, 2>(MakeDrivetrainController());
+ return StateFeedbackLoop<4, 2, 2>(controllers);
+}
+
+} // namespace control_loops
+} // namespace bot3
diff --git a/bot3/control_loops/drivetrain/drivetrain_motor_plant.h b/bot3/control_loops/drivetrain/drivetrain_motor_plant.h
new file mode 100644
index 0000000..e511560
--- /dev/null
+++ b/bot3/control_loops/drivetrain/drivetrain_motor_plant.h
@@ -0,0 +1,20 @@
+#ifndef BOT3_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_MOTOR_PLANT_H_
+#define BOT3_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace bot3 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainPlantCoefficients();
+
+StateFeedbackController<4, 2, 2> MakeDrivetrainController();
+
+StateFeedbackPlant<4, 2, 2> MakeDrivetrainPlant();
+
+StateFeedbackLoop<4, 2, 2> MakeDrivetrainLoop();
+
+} // namespace control_loops
+} // namespace bot3
+
+#endif // BOT3_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_MOTOR_PLANT_H_
diff --git a/bot3/input/gyro_board_data.h b/bot3/input/gyro_board_data.h
new file mode 100644
index 0000000..a3b60e5
--- /dev/null
+++ b/bot3/input/gyro_board_data.h
@@ -0,0 +1,65 @@
+#ifndef BOT3_INPUT_GYRO_BOARD_DATA_H_
+#define BOT3_INPUT_GYRO_BOARD_DATA_H_
+
+#include "aos/common/byteorder.h"
+
+namespace bot3 {
+
+// The struct that the gyro board sends out with all of the data in it.
+struct GyroBoardData {
+ int64_t gyro_angle;
+
+ int32_t left_drive;
+ int32_t right_drive;
+ int32_t shooter_angle;
+ int32_t shooter;
+
+ int32_t capture_top_rise;
+ int32_t capture_top_fall;
+ int32_t capture_bottom_fall_delay;
+
+ uint8_t top_rise_count;
+
+ uint8_t top_fall_count;
+
+ uint8_t bottom_rise_count;
+
+ uint8_t bottom_fall_delay_count;
+ uint8_t bottom_fall_count;
+
+ union {
+ struct {
+ uint8_t top_disc : 1;
+ uint8_t bottom_disc : 1;
+ };
+ uint32_t digitals;
+ };
+
+ void NetworkToHost() {
+ // Apparently it sends the information out in little endian.
+#if 0
+ using ::aos::ntoh;
+
+ gyro_angle = ntoh(gyro_angle);
+
+ right_drive = ntoh(right_drive);
+ left_drive = ntoh(left_drive);
+ shooter_angle = ntoh(shooter_angle);
+ shooter = ntoh(shooter);
+ indexer = ntoh(indexer);
+ wrist = ntoh(wrist);
+
+ capture_top_rise = ntoh(capture_top_rise);
+ capture_top_fall = ntoh(capture_top_fall);
+ capture_bottom_fall_delay = ntoh(capture_bottom_fall_delay);
+ capture_wrist_rise = ntoh(capture_wrist_rise);
+ capture_shooter_angle_rise = ntoh(capture_shooter_angle_rise);
+
+ digitals = ntoh(digitals);
+#endif
+ }
+} __attribute__((__packed__));
+
+} // namespace bot3
+
+#endif // BOT3_INPUT_GYRO_BOARD_DATA_H_
diff --git a/bot3/input/gyro_board_reader.cc b/bot3/input/gyro_board_reader.cc
new file mode 100644
index 0000000..a5f961f
--- /dev/null
+++ b/bot3/input/gyro_board_reader.cc
@@ -0,0 +1,187 @@
+#include <libusb-1.0/libusb.h>
+#include <memory>
+
+#include "aos/common/inttypes.h"
+#include "aos/atom_code/init.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/control_loop/Timing.h"
+#include "aos/common/time.h"
+
+#include "bot3/control_loops/drivetrain/drivetrain.q.h"
+#include "bot3/input/gyro_board_data.h"
+#include "gyro_board/src/libusb-driver/libusb_wrap.h"
+#include "frc971/queues/GyroAngle.q.h"
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+using ::bot3::control_loops::drivetrain;
+using ::frc971::sensors::gyro;
+
+namespace bot3 {
+namespace {
+
+inline double drivetrain_translate(int32_t in) {
+ return static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*quad*/) *
+ (19.0 / 50.0) /*output reduction*/ * (64.0 / 24.0) /*encoder gears*/ *
+ (3.5 /*wheel diameter*/ * 2.54 / 100.0 * M_PI);
+}
+
+// TODO(daniel): This might have to change if I find out that the gear ratios are different.
+inline double shooter_translate(int32_t in) {
+ return static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*quad*/) *
+ (15.0 / 34.0) /*gears*/ * (2 * M_PI);
+}
+
+} // namespace
+
+class GyroBoardReader {
+ public:
+ GyroBoardReader()
+ : top_rise_count_(0),
+ last_top_rise_count_(0),
+ top_fall_count_(0),
+ last_top_fall_count_(0),
+ bottom_rise_count_(0),
+ last_bottom_rise_count_(0),
+ bottom_fall_delay_count_(0),
+ last_bottom_fall_delay_count_(0),
+ bottom_fall_count_(0),
+ last_bottom_fall_count_(0) {
+ }
+
+ void Run() {
+ LibUSB libusb;
+
+ dev_handle_ = ::std::unique_ptr<LibUSBDeviceHandle>(
+ libusb.FindDeviceWithVIDPID(kVid, kPid));
+ if (!dev_handle_) {
+ LOG(ERROR, "couldn't find device. exiting\n");
+ exit(1);
+ }
+
+ uint8_t data[64];
+ GyroBoardData *real_data;
+ static_assert(sizeof(*real_data) <= sizeof(data), "it doesn't fit");
+
+ uint8_t *data_pointer = data;
+ memcpy(&real_data, &data_pointer, sizeof(data_pointer));
+ while (true) {
+ if (false) {
+ // Theoretically need -3ms of offset. Using a slightly larger one to avoid
+ // missing the first control loop in the worst case.
+ ::aos::time::PhasedLoop10MS(
+ ::aos::time::Time::InSeconds(-0.0031).ToUSec());
+ LOG(DEBUG, "starting now\n");
+
+ // Read 2 to make sure that we get fresh data.
+ if (!ReadPacket(data, sizeof(data))) continue;
+ //LOG(DEBUG, "in between\n");
+ if (!ReadPacket(data, sizeof(data))) continue;
+ } else {
+ if (!ReadPacket(data, sizeof(data))) continue;
+
+ ProcessData(real_data);
+ }
+ }
+ }
+
+ private:
+ static const unsigned char kEndpoint = 0x81;
+ // in ms
+ // 0 is unlimited
+ static const unsigned int kReadTimeout = 1000;
+
+ // vendor ID
+ static const int32_t kVid = 0x1424;
+ // product ID
+ static const int32_t kPid = 0xd243;
+
+ // Returns whether it read a good packet.
+ bool ReadPacket(uint8_t *data, size_t data_size) {
+ int read_bytes;
+ int r = dev_handle_->interrupt_transfer(
+ kEndpoint, data, data_size, &read_bytes, kReadTimeout);
+
+ if (r != 0) {
+ if (r == LIBUSB_ERROR_TIMEOUT) {
+ LOG(ERROR, "read timed out\n");
+ return false;
+ }
+ LOG(FATAL, "libusb gave error %d\n", r);
+ }
+
+ if (read_bytes < static_cast<ssize_t>(sizeof(GyroBoardData))) {
+ LOG(ERROR, "read %d bytes instead of at least %zd\n",
+ read_bytes, sizeof(GyroBoardData));
+ return false;
+ }
+
+ return true;
+ }
+
+ void UpdateWrappingCounter(
+ uint8_t current, uint8_t *last, int32_t *counter) {
+ if (*last > current) {
+ *counter += 0x100;
+ }
+ *counter = (*counter & 0xffffff00) | current;
+ *last = current;
+ }
+
+ void ProcessData(GyroBoardData *data) {
+ data->NetworkToHost();
+ LOG(DEBUG, "processing a packet\n");
+ static ::aos::time::Time last_time = ::aos::time::Time::Now();
+ if ((last_time - ::aos::time::Time::Now()) >
+ ::aos::time::Time::InMS(0.00205)) {
+ LOG(INFO, "missed one\n");
+ }
+
+ gyro.MakeWithBuilder()
+ .angle(data->gyro_angle / 16.0 / 1000.0 / 180.0 * M_PI)
+ .Send();
+
+ UpdateWrappingCounter(data->top_rise_count,
+ &last_top_rise_count_, &top_rise_count_);
+ UpdateWrappingCounter(data->top_fall_count,
+ &last_top_fall_count_, &top_fall_count_);
+ UpdateWrappingCounter(data->bottom_rise_count,
+ &last_bottom_rise_count_, &bottom_rise_count_);
+ UpdateWrappingCounter(data->bottom_fall_delay_count,
+ &last_bottom_fall_delay_count_, &bottom_fall_delay_count_);
+ UpdateWrappingCounter(data->bottom_fall_count,
+ &last_bottom_fall_count_, &bottom_fall_count_);
+
+ drivetrain.position.MakeWithBuilder()
+ .right_encoder(drivetrain_translate(data->right_drive))
+ .left_encoder(-drivetrain_translate(data->left_drive))
+ .Send();
+
+ //TODO (danielp): Add stuff for bot3 shooter.
+ }
+
+ ::std::unique_ptr<LibUSBDeviceHandle> dev_handle_;
+
+ int32_t top_rise_count_;
+ uint8_t last_top_rise_count_;
+ int32_t top_fall_count_;
+ uint8_t last_top_fall_count_;
+ int32_t bottom_rise_count_;
+ uint8_t last_bottom_rise_count_;
+ int32_t bottom_fall_delay_count_;
+ uint8_t last_bottom_fall_delay_count_;
+ int32_t bottom_fall_count_;
+ uint8_t last_bottom_fall_count_;
+};
+
+} // namespace bot3
+
+int main() {
+ ::aos::Init();
+ ::bot3::GyroBoardReader reader;
+ reader.Run();
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/bot3/input/gyro_sensor_receiver.cc b/bot3/input/gyro_sensor_receiver.cc
new file mode 100644
index 0000000..6553e3a
--- /dev/null
+++ b/bot3/input/gyro_sensor_receiver.cc
@@ -0,0 +1,258 @@
+#include <libusb-1.0/libusb.h>
+#include <memory>
+
+#include "aos/common/inttypes.h"
+#include "aos/atom_code/init.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/time.h"
+#include "aos/common/sensors/sensor_unpacker.h"
+#include "aos/common/sensors/sensor_receiver.h"
+
+#include "bot3/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/input/gyro_board_data.h"
+#include "gyro_board/src/libusb-driver/libusb_wrap.h"
+#include "frc971/queues/GyroAngle.q.h"
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+using ::bot3::control_loops::drivetrain;
+using ::frc971::sensors::gyro;
+using ::frc971::GyroBoardData;
+
+namespace bot3 {
+namespace {
+
+inline double drivetrain_translate(int32_t in) {
+ return static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*quad*/) *
+ (19.0 / 50.0) /*output reduction*/ * (64.0 / 24.0) /*encoder gears*/ *
+ (3.5 /*wheel diameter*/ * 2.54 / 100.0 * M_PI);
+}
+
+inline 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);
+}
+
+inline 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);
+}
+
+inline 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);
+}
+
+inline double index_translate(int32_t in) {
+ return -static_cast<double>(in) / (128.0 /*cpr*/ * 4.0 /*quad*/) *
+ (1.0) /*gears*/ * (2 * M_PI);
+}
+
+} // namespace
+
+class GyroSensorUnpacker :
+ public ::aos::sensors::SensorUnpackerInterface<GyroBoardData> {
+ public:
+ GyroSensorUnpacker()
+ : top_rise_count_(0),
+ last_top_rise_count_(0),
+ top_fall_count_(0),
+ last_top_fall_count_(0),
+ bottom_rise_count_(0),
+ last_bottom_rise_count_(0),
+ bottom_fall_delay_count_(0),
+ last_bottom_fall_delay_count_(0),
+ bottom_fall_count_(0),
+ last_bottom_fall_count_(0),
+ wrist_rise_count_(0),
+ last_wrist_rise_count_(0),
+ shooter_angle_rise_count_(0),
+ last_shooter_angle_rise_count_(0) {
+ }
+
+ void UnpackFrom(GyroBoardData *data) {
+ data->NetworkToHost();
+ LOG(DEBUG, "processing a packet\n");
+
+ static ::aos::time::Time last_time = ::aos::time::Time::Now();
+ if ((last_time - ::aos::time::Time::Now()) >
+ ::aos::time::Time::InMS(0.00205)) {
+ LOG(INFO, "missed one\n");
+ }
+
+ gyro.MakeWithBuilder()
+ .angle(data->gyro_angle / 16.0 / 1000.0 / 180.0 * M_PI)
+ .Send();
+
+ UpdateWrappingCounter(data->top_rise_count,
+ &last_top_rise_count_, &top_rise_count_);
+ UpdateWrappingCounter(data->top_fall_count,
+ &last_top_fall_count_, &top_fall_count_);
+ UpdateWrappingCounter(data->bottom_rise_count,
+ &last_bottom_rise_count_, &bottom_rise_count_);
+ UpdateWrappingCounter(data->bottom_fall_delay_count,
+ &last_bottom_fall_delay_count_, &bottom_fall_delay_count_);
+ UpdateWrappingCounter(data->bottom_fall_count,
+ &last_bottom_fall_count_, &bottom_fall_count_);
+ UpdateWrappingCounter(data->wrist_rise_count,
+ &last_wrist_rise_count_, &wrist_rise_count_);
+ UpdateWrappingCounter(data->shooter_angle_rise_count,
+ &last_shooter_angle_rise_count_, &shooter_angle_rise_count_);
+
+ drivetrain.position.MakeWithBuilder()
+ .right_encoder(drivetrain_translate(data->right_drive))
+ .left_encoder(-drivetrain_translate(data->left_drive))
+ .Send();
+ }
+
+ private:
+ void UpdateWrappingCounter(
+ uint8_t current, uint8_t *last, int32_t *counter) {
+ if (*last > current) {
+ *counter += 0x100;
+ }
+ *counter = (*counter & 0xffffff00) | current;
+ *last = current;
+ }
+
+ int32_t top_rise_count_;
+ uint8_t last_top_rise_count_;
+ int32_t top_fall_count_;
+ uint8_t last_top_fall_count_;
+ int32_t bottom_rise_count_;
+ uint8_t last_bottom_rise_count_;
+ int32_t bottom_fall_delay_count_;
+ uint8_t last_bottom_fall_delay_count_;
+ int32_t bottom_fall_count_;
+ uint8_t last_bottom_fall_count_;
+ int32_t wrist_rise_count_;
+ uint8_t last_wrist_rise_count_;
+ int32_t shooter_angle_rise_count_;
+ uint8_t last_shooter_angle_rise_count_;
+};
+
+class GyroSensorReceiver :
+ public ::aos::sensors::SensorReceiver<GyroBoardData> {
+ public:
+ GyroSensorReceiver(
+ ::aos::sensors::SensorUnpackerInterface<GyroBoardData> *unpacker)
+ : ::aos::sensors::SensorReceiver<GyroBoardData>(unpacker),
+ start_time_(0, 0) {
+ static_assert(sizeof(GyroBoardData) <= kDataLength,
+ "the buffer will be too small");
+ }
+
+ private:
+ static const unsigned char kEndpoint = 0x81;
+ // in ms
+ // 0 is unlimited
+ static const unsigned int kReadTimeout = 1000;
+
+ // vendor ID
+ static const int32_t kVid = 0x1424;
+ // product ID
+ static const int32_t kPid = 0xd243;
+
+ // How big of a buffer to give the function.
+ static const size_t kDataLength = 64;
+
+ virtual void DoReceiveData() {
+ // Loop and then return once we get a good one.
+ while (true) {
+ completed_transfer_ = NULL;
+ while (completed_transfer_ == NULL) {
+ libusb_.HandleEvents();
+ }
+ LOG(DEBUG, "processing transfer %p\n", completed_transfer_);
+
+ if (completed_transfer_->read_bytes() <
+ static_cast<ssize_t>(sizeof(GyroBoardData))) {
+ LOG(ERROR, "read %d bytes instead of at least %zd\n",
+ completed_transfer_->read_bytes(), sizeof(GyroBoardData));
+ continue;
+ }
+
+ memcpy(&data()->values, completed_transfer_->data(),
+ sizeof(GyroBoardData));
+ if (data()->count == 0) {
+ start_time_ = ::aos::time::Time::Now();
+ data()->count = 1;
+ } else {
+ ::aos::time::Time delta_time = ::aos::time::Time::Now() - start_time_;
+ data()->count = static_cast<int32_t>(
+ (delta_time / ::aos::sensors::kSensorSendFrequency) + 0.5);
+ }
+ return;
+ }
+ }
+
+ virtual void Reset() {
+ transfer1_.reset();
+ transfer2_.reset();
+ dev_handle_ = ::std::unique_ptr<LibUSBDeviceHandle>(
+ libusb_.FindDeviceWithVIDPID(kVid, kPid));
+ if (!dev_handle_) {
+ LOG(ERROR, "couldn't find device. exiting\n");
+ exit(1);
+ }
+ transfer1_ = ::std::unique_ptr<libusb::Transfer>(
+ new libusb::Transfer(kDataLength, StaticTransferCallback, this));
+ transfer2_ = ::std::unique_ptr<libusb::Transfer>(
+ new libusb::Transfer(kDataLength, StaticTransferCallback, this));
+ transfer1_->FillInterrupt(dev_handle_.get(), kEndpoint, kReadTimeout);
+ transfer2_->FillInterrupt(dev_handle_.get(), kEndpoint, kReadTimeout);
+ transfer1_->Submit();
+ transfer2_->Submit();
+
+ data()->count = 0;
+ }
+
+ static void StaticTransferCallback(libusb::Transfer *transfer, void *self) {
+ static_cast<GyroSensorReceiver *>(self)->TransferCallback(transfer);
+ }
+ void TransferCallback(libusb::Transfer *transfer) {
+ if (transfer->status() == LIBUSB_TRANSFER_COMPLETED) {
+ LOG(DEBUG, "transfer %p completed\n", transfer);
+ completed_transfer_ = transfer;
+ } else if (transfer->status() == LIBUSB_TRANSFER_TIMED_OUT) {
+ LOG(WARNING, "transfer %p timed out\n", transfer);
+ } else if (transfer->status() == LIBUSB_TRANSFER_CANCELLED) {
+ LOG(DEBUG, "transfer %p cancelled\n", transfer);
+ } else {
+ LOG(FATAL, "transfer %p has status %d\n", transfer, transfer->status());
+ }
+ transfer->Submit();
+ }
+
+ virtual void Synchronized(::aos::time::Time start_time) {
+ // Subtract off how many packets it read while synchronizing from the time.
+ start_time_ = start_time -
+ ::aos::sensors::kSensorSendFrequency * data()->count;
+ }
+
+ ::std::unique_ptr<LibUSBDeviceHandle> dev_handle_;
+ ::std::unique_ptr<libusb::Transfer> transfer1_, transfer2_;
+ // Temporary variable for holding a completed transfer to communicate that
+ // information from the callback to the code that wants it.
+ libusb::Transfer *completed_transfer_;
+
+ ::aos::time::Time start_time_;
+
+ LibUSB libusb_;
+};
+
+} // namespace bot3
+
+int main() {
+ ::aos::Init();
+ ::bot3::GyroSensorUnpacker unpacker;
+ ::bot3::GyroSensorReceiver receiver(&unpacker);
+ while (true) {
+ receiver.RunIteration();
+ }
+ ::aos::Cleanup();
+}
diff --git a/bot3/input/input.gyp b/bot3/input/input.gyp
new file mode 100644
index 0000000..5beb3d0
--- /dev/null
+++ b/bot3/input/input.gyp
@@ -0,0 +1,65 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'joystick_reader',
+ 'type': 'executable',
+ 'sources': [
+ 'joystick_reader.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/atom_code/input/input.gyp:joystick_input',
+ '<(AOS)/atom_code/atom_code.gyp:init',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
+ '<(DEPTH)/frc971/queues/queues.gyp:queues',
+ '<(DEPTH)/bot3/autonomous/autonomous.gyp:auto_queue',
+ ],
+ },
+ {
+ 'target_name': 'gyro_sensor_receiver',
+ 'type': 'executable',
+ 'sources': [
+ 'gyro_sensor_receiver.cc',
+ ],
+ 'dependencies': [
+ '<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
+ '<(DEPTH)/frc971/queues/queues.gyp:queues',
+ '<(AOS)/atom_code/atom_code.gyp:init',
+ '<(DEPTH)/gyro_board/src/libusb-driver/libusb-driver.gyp:libusb_wrap',
+ '<(EXTERNALS):libusb',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/sensors/sensors.gyp:sensor_receiver',
+ ],
+ },
+ {
+ 'target_name': 'gyro_board_reader',
+ 'type': 'executable',
+ 'sources': [
+ 'gyro_board_reader.cc',
+ ],
+ 'dependencies': [
+ '<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
+ '<(DEPTH)/frc971/queues/queues.gyp:queues',
+ '<(AOS)/atom_code/atom_code.gyp:init',
+ '<(DEPTH)/gyro_board/src/libusb-driver/libusb-driver.gyp:libusb_wrap',
+ '<(EXTERNALS):libusb',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/common.gyp:timing',
+ ],
+ },
+ {
+ 'target_name': 'gyro_reader',
+ 'type': 'executable',
+ 'sources': [
+ 'gyro_reader.cc',
+ ],
+ 'dependencies': [
+ '<(DEPTH)/frc971/queues/queues.gyp:queues',
+ '<(AOS)/atom_code/atom_code.gyp:init',
+ '<(AOS)/build/aos.gyp:logging',
+ ],
+ },
+ ],
+}
diff --git a/bot3/input/joystick_reader.cc b/bot3/input/joystick_reader.cc
new file mode 100644
index 0000000..5efe8b4
--- /dev/null
+++ b/bot3/input/joystick_reader.cc
@@ -0,0 +1,206 @@
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+#include <math.h>
+
+#include "aos/atom_code/init.h"
+#include "aos/atom_code/input/joystick_input.h"
+#include "aos/common/logging/logging.h"
+
+#include "bot3/control_loops/drivetrain/drivetrain.q.h"
+#include "bot3/autonomous/auto.q.h"
+#include "frc971/queues/GyroAngle.q.h"
+#include "frc971/queues/Piston.q.h"
+#include "frc971/queues/CameraTarget.q.h"
+
+using ::bot3::control_loops::drivetrain;
+using ::frc971::control_loops::shifters;
+using ::frc971::sensors::gyro;
+// using ::frc971::vision::target_angle;
+
+using ::aos::input::driver_station::ButtonLocation;
+using ::aos::input::driver_station::JoystickAxis;
+using ::aos::input::driver_station::ControlBit;
+
+namespace bot3 {
+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 kFire(3, 11);
+const ButtonLocation kIntake(3, 10);
+const ButtonLocation kForceFire(3, 12);
+const ButtonLocation kForceIndexUp(3, 9), kForceIndexDown(3, 7);
+
+class Reader : public ::aos::input::JoystickInput {
+ public:
+ static const bool kWristAlwaysDown = false;
+
+ Reader() {
+ printf("\nRunning Bot3 JoystickReader!\n");
+ 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");
+ ::bot3::autonomous::autonomous.MakeWithBuilder()
+ .run_auto(true).Send();
+ } else if (data.NegEdge(ControlBit::kEnabled)) {
+ LOG(INFO, "Stopping auto mode\n");
+ ::bot3::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)) {
+ LOG(INFO, "Control loop driving is currently not supported by this robot.\n");
+#if 0
+ 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);
+#endif
+ }
+ 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;
+ }
+#if 0
+ ::aos::ScopedMessagePtr<frc971::control_loops::ShooterLoop::Goal> shooter_goal =
+ shooter.goal.MakeMessage();
+ shooter_goal->velocity = 0;
+ if (data.IsPressed(kPitShot1) && data.IsPressed(kPitShot2)) {
+ shooter_goal->velocity = 131;
+ } 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
+ wrist_up_position = 0.70;
+ } else {
+ LOG(WARNING, "camera frame too old\n");
+ // pretend like no button is pressed
+ }
+#endif
+ shooter_goal->velocity = 360;
+ } else if (data.IsPressed(kMediumShot)) {
+#if 0
+ shooter_goal->velocity = 375;
+ wrist_up_position = 0.70;
+ angle_adjust_goal = 0.564;
+#endif
+ // middle wheel on the back line (same as auto)
+ shooter_goal->velocity = 395;
+ } else if (data.IsPressed(kShortShot)) {
+ shooter_goal->velocity = 375;
+ }
+
+ //TODO (daniel) Modify this for hopper and shooter.
+ ::aos::ScopedMessagePtr<frc971::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);
+ index_goal->override_index = index_up || index_down;
+ 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;
+ }
+
+ index_goal.Send();
+ shooter_goal.Send();
+#endif
+ }
+ }
+};
+
+} // namespace joysticks
+} // namespace input
+} // namespace bot3
+
+int main() {
+ ::aos::Init();
+ ::bot3::input::joysticks::Reader reader;
+ reader.Run();
+ ::aos::Cleanup();
+}
diff --git a/bot3/output/CameraServer.cc b/bot3/output/CameraServer.cc
new file mode 100644
index 0000000..939731d
--- /dev/null
+++ b/bot3/output/CameraServer.cc
@@ -0,0 +1,93 @@
+#include <string.h>
+
+#include "aos/atom_code/output/HTTPServer.h"
+#include "aos/atom_code/output/evhttp_ctemplate_emitter.h"
+#include "aos/atom_code/output/ctemplate_cache.h"
+#include "aos/common/messages/RobotState.q.h"
+#include "ctemplate/template.h"
+#include "aos/atom_code/init.h"
+#include "aos/common/logging/logging.h"
+#include "aos/atom_code/configuration.h"
+
+#include "frc971/constants.h"
+
+RegisterTemplateFilename(ROBOT_HTML, "robot.html.tpl");
+
+namespace frc971 {
+
+class CameraServer : public aos::http::HTTPServer {
+ public:
+ CameraServer() : HTTPServer(::aos::configuration::GetRootDirectory(), 8080),
+ buf_(NULL) {
+ AddPage<CameraServer>("/robot.html", &CameraServer::RobotHTML, this);
+ }
+
+ private:
+ evbuffer *buf_;
+ bool Setup(evhttp_request *request, const char *content_type) {
+ if (evhttp_add_header(evhttp_request_get_output_headers(request),
+ "Content-Type", content_type) == -1) {
+ LOG(WARNING, "adding Content-Type failed\n");
+ evhttp_send_error(request, HTTP_INTERNAL, NULL);
+ return false;
+ }
+ if (buf_ == NULL) buf_ = evbuffer_new();
+ if (buf_ == NULL) {
+ LOG(WARNING, "evbuffer_new() failed\n");
+ evhttp_send_error(request, HTTP_INTERNAL, NULL);
+ return false;
+ }
+ return true;
+ }
+ void RobotHTML(evhttp_request *request) {
+ if (!Setup(request, "text/html")) return;
+
+ ctemplate::TemplateDictionary dict(ROBOT_HTML);
+ const char *host = evhttp_find_header(
+ evhttp_request_get_input_headers(request), "Host");
+ if (host == NULL) {
+ evhttp_send_error(request, HTTP_BADREQUEST, "no Host header");
+ return;
+ }
+ const char *separator = strchrnul(host, ':');
+ size_t length = separator - host;
+ // Don't include the last ':' (or the terminating '\0') or anything else
+ // after it.
+ dict.SetValue("HOST", ctemplate::TemplateString(host, length));
+
+ if (!aos::robot_state.FetchLatest()) {
+ LOG(WARNING, "getting a RobotState message failed\n");
+ evhttp_send_error(request, HTTP_INTERNAL, NULL);
+ return;
+ }
+ int center;
+ if (!constants::camera_center(¢er)) {
+ evhttp_send_error(request, HTTP_INTERNAL, NULL);
+ return;
+ }
+ dict.SetIntValue("CENTER", center);
+
+ aos::http::EvhttpCtemplateEmitter emitter(buf_);
+ if (!aos::http::get_template_cache()->
+ ExpandWithData(ROBOT_HTML, ctemplate::STRIP_WHITESPACE,
+ &dict, NULL, &emitter)) {
+ LOG(ERROR, "expanding the template failed\n");
+ evhttp_send_error(request, HTTP_INTERNAL, NULL);
+ return;
+ }
+ if (emitter.error()) {
+ evhttp_send_error(request, HTTP_INTERNAL, NULL);
+ return;
+ }
+ evhttp_send_reply(request, HTTP_OK, NULL, buf_);
+ }
+};
+
+} // namespace frc971
+
+int main() {
+ ::aos::InitNRT();
+ ::frc971::CameraServer server;
+ server.Run();
+ ::aos::Cleanup();
+}
diff --git a/bot3/output/atom_motor_writer.cc b/bot3/output/atom_motor_writer.cc
new file mode 100644
index 0000000..7d43b53
--- /dev/null
+++ b/bot3/output/atom_motor_writer.cc
@@ -0,0 +1,68 @@
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+
+#include "aos/atom_code/output/motor_output.h"
+#include "aos/common/logging/logging.h"
+#include "aos/atom_code/init.h"
+
+#include "frc971/queues/Piston.q.h"
+#include "bot3/control_loops/drivetrain/drivetrain.q.h"
+
+using ::bot3::control_loops::drivetrain;
+using ::frc971::control_loops::shifters;
+
+namespace bot3 {
+namespace output {
+
+class MotorWriter : public ::aos::MotorOutput {
+ // Maximum age of an output packet before the motors get zeroed instead.
+ static const int kOutputMaxAgeMS = 20;
+ static const int kEnableDrivetrain = true;
+
+ virtual void RunIteration() {
+ values_.digital_module = 0;
+ values_.pressure_switch_channel = 14;
+ values_.compressor_channel = 1;
+ values_.solenoid_module = 0;
+
+ drivetrain.output.FetchLatest();
+ if (drivetrain.output.IsNewerThanMS(kOutputMaxAgeMS) && kEnableDrivetrain) {
+ SetPWMOutput(2, drivetrain.output->right_voltage / 12.0, kTalonBounds);
+ 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);
+ } else {
+ DisablePWMOutput(2);
+ DisablePWMOutput(3);
+ DisablePWMOutput(5);
+ DisablePWMOutput(6);
+ if (kEnableDrivetrain) {
+ LOG(WARNING, "drivetrain not new enough\n");
+ }
+ }
+ shifters.FetchLatest();
+ if (shifters.get()) {
+ SetSolenoid(1, shifters->set);
+ SetSolenoid(2, !shifters->set);
+ }
+
+ /*if (shooter.output.IsNewerThanMS(kOutputMaxAgeMS)) {
+ SetPWMOutput(4, shooter.output->voltage / 12.0, kVictorBounds);
+ } else {
+ DisablePWMOutput(4);
+ LOG(WARNING, "shooter not new enough\n");
+ }*/
+ // TODO(danielp): Add stuff for intake and shooter.
+ }
+};
+
+} // namespace output
+} // namespace bot3
+
+int main() {
+ ::aos::Init();
+ ::bot3::output::MotorWriter writer;
+ writer.Run();
+ ::aos::Cleanup();
+}
diff --git a/bot3/output/output.gyp b/bot3/output/output.gyp
new file mode 100644
index 0000000..219f501
--- /dev/null
+++ b/bot3/output/output.gyp
@@ -0,0 +1,42 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'CameraServer',
+ 'type': 'executable',
+ 'sources': [
+ 'CameraServer.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/atom_code/output/output.gyp:http_server',
+ '<(DEPTH)/frc971/frc971.gyp:common',
+ '<(AOS)/atom_code/atom_code.gyp:init',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/messages/messages.gyp:aos_queues',
+ '<(AOS)/atom_code/atom_code.gyp:configuration',
+ ],
+ 'copies': [
+ {
+ 'destination': '<(rsync_dir)',
+ 'files': [
+ 'robot.html.tpl',
+ ],
+ },
+ ],
+ },
+ {
+ 'target_name': 'MotorWriter',
+ 'type': '<(aos_target)',
+ 'sources': [
+ 'atom_motor_writer.cc'
+ ],
+ 'dependencies': [
+ '<(AOS)/atom_code/output/output.gyp:motor_output',
+ '<(AOS)/atom_code/atom_code.gyp:init',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
+ '<(AOS)/common/common.gyp:controls',
+ '<(DEPTH)/frc971/queues/queues.gyp:queues',
+ ],
+ },
+ ],
+}
diff --git a/bot3/output/robot.html.tpl b/bot3/output/robot.html.tpl
new file mode 100644
index 0000000..7ae51a6
--- /dev/null
+++ b/bot3/output/robot.html.tpl
@@ -0,0 +1,56 @@
+<!DOCTYPE HTML>
+<html>
+ <head>
+ <title>971 Camera Code: Robot Stream</title>
+ <style type="text/css">
+ #body {
+ display: block;
+ margin: 0px;
+ margin-top: 0px;
+ margin-right: 0px;
+ margin-bottom: 0px;
+ margin-left: 0px;
+ }
+ #img {
+ position: absolute;
+ left: 50%;
+ top: 0%;
+ margin: 0 0 0 -320px;
+ }
+ #center {
+ left: 50%;
+ position: absolute;
+ width: 2px;
+ height: 100%;
+ background-color: red;
+ }
+ #middle {
+ top: 240px;
+ margin-top: -1px;
+ width: 100%;
+ position: absolute;
+ height: 2px;
+ background-color: red;
+ }
+ #footer {
+ top: 482px;
+ left: 10px;
+ position: absolute;
+ }
+ #center {
+ margin-left: {{CENTER}}px;
+ }
+ </style>
+ </head>
+ <body id="body">
+ <img id="img" src="http://{{HOST}}:9714" />
+ <div id="center"></div>
+ <div id="middle"></div>
+ <div id="footer">
+ <!--<form>
+ <input type="button" value="Camera Controls"
+ onclick="window.open('control.htm', 'Camera_Controls')">
+ </form>-->
+ </div>
+ </body>
+</html>
diff --git a/frc971/atom_code/atom_code.gyp b/frc971/atom_code/atom_code.gyp
index 780ca23..a2bdc8d 100644
--- a/frc971/atom_code/atom_code.gyp
+++ b/frc971/atom_code/atom_code.gyp
@@ -17,7 +17,7 @@
'../control_loops/shooter/shooter.gyp:shooter_lib_test',
'../control_loops/shooter/shooter.gyp:shooter',
'../autonomous/autonomous.gyp:auto',
- '../input/input.gyp:JoystickReader',
+ '../input/input.gyp:joystick_reader',
'../../vision/vision.gyp:OpenCVWorkTask',
'../../vision/vision.gyp:GoalMaster',
'../output/output.gyp:MotorWriter',
diff --git a/frc971/atom_code/scripts/start_list.txt b/frc971/atom_code/scripts/start_list.txt
index 0991e96..8b51057 100644
--- a/frc971/atom_code/scripts/start_list.txt
+++ b/frc971/atom_code/scripts/start_list.txt
@@ -1,6 +1,6 @@
BinaryLogReader
MotorWriter
-JoystickReader
+joystick_reader
drivetrain
CRIOLogReader
angle_adjust
diff --git a/frc971/input/gyro_reader.cc b/frc971/input/gyro_reader.cc
new file mode 100644
index 0000000..bdcd07e
--- /dev/null
+++ b/frc971/input/gyro_reader.cc
@@ -0,0 +1,51 @@
+#include <unistd.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+
+#include "aos/common/logging/logging.h"
+#include "aos/atom_code/init.h"
+
+#include "frc971/queues/GyroAngle.q.h"
+
+#define M_PI 3.14159265358979323846264338327
+
+using frc971::sensors::gyro;
+
+int main(){
+ aos::Init();
+ int fd = open("/dev/aschuh0", O_RDONLY);
+ int rate_limit = 0;
+ if (fd < 0) {
+ LOG(ERROR, "No Gyro found.\n");
+ } else {
+ LOG(INFO, "Gyro now connected\n");
+ }
+
+ while (true) {
+ int64_t gyro_value;
+ if (read(fd, (void *)&gyro_value, sizeof(gyro_value)) != sizeof(gyro_value)) {
+ LOG(ERROR, "Could not read gyro errno: %d\n", errno);
+ if (errno == ENODEV || errno == EBADF) {
+ close(fd);
+ while (1) {
+ usleep(1000);
+ fd = open("/dev/aschuh0", O_RDONLY);
+ if (fd > 0) {
+ LOG(INFO, "Found gyro again\n");
+ break;
+ }
+ }
+ }
+ continue;
+ }
+ rate_limit ++;
+ if (rate_limit > 10) {
+ LOG(DEBUG, "Gyro is %d\n", (int)(gyro_value / 16));
+ rate_limit = 0;
+ }
+ gyro.MakeWithBuilder().angle(gyro_value / 16.0 / 1000.0 / 180.0 * M_PI).Send();
+ }
+
+ aos::Cleanup();
+}
diff --git a/frc971/input/input.gyp b/frc971/input/input.gyp
index 2fa63f5..d3e25a1 100644
--- a/frc971/input/input.gyp
+++ b/frc971/input/input.gyp
@@ -1,10 +1,10 @@
{
'targets': [
{
- 'target_name': 'JoystickReader',
+ 'target_name': 'joystick_reader',
'type': 'executable',
'sources': [
- 'JoystickReader.cc',
+ 'joystick_reader.cc',
],
'dependencies': [
'<(AOS)/atom_code/input/input.gyp:joystick_input',
diff --git a/frc971/input/JoystickReader.cc b/frc971/input/joystick_reader.cc
similarity index 100%
rename from frc971/input/JoystickReader.cc
rename to frc971/input/joystick_reader.cc
diff --git a/frc971/output/AtomMotorWriter.cc b/frc971/output/atom_motor_writer.cc
similarity index 100%
rename from frc971/output/AtomMotorWriter.cc
rename to frc971/output/atom_motor_writer.cc
diff --git a/frc971/output/output.gyp b/frc971/output/output.gyp
index 90fccb2..68b3630 100644
--- a/frc971/output/output.gyp
+++ b/frc971/output/output.gyp
@@ -27,7 +27,7 @@
'target_name': 'MotorWriter',
'type': '<(aos_target)',
'sources': [
- 'AtomMotorWriter.cc'
+ 'atom_motor_writer.cc'
],
'dependencies': [
'<(AOS)/atom_code/output/output.gyp:motor_output',