Merge in Brian's latest tweaks and improvements, mostly just
to stay somewhat in sync with what he's doing.
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/input/JoystickInput.cpp b/aos/atom_code/input/JoystickInput.cpp
deleted file mode 100644
index 2c3b3b9..0000000
--- a/aos/atom_code/input/JoystickInput.cpp
+++ /dev/null
@@ -1,73 +0,0 @@
-#include "aos/atom_code/input/JoystickInput.h"
-
-#include "aos/common/Configuration.h"
-#include "aos/common/network/ReceiveSocket.h"
-#include "aos/common/messages/RobotState.q.h"
-
-namespace aos {
-
-void JoystickInput::SetupButtons() {
- for (int i = 0; i < 4; ++i) {
- old_buttons[i] = buttons[i];
- }
- buttons[0] = control_data_.stick0Buttons;
- buttons[1] = control_data_.stick1Buttons;
- buttons[2] = control_data_.stick2Buttons;
- buttons[3] = control_data_.stick3Buttons;
-
- // Put the ENABLED, AUTONOMOUS, and FMS_ATTACHED values into unused bits in
- // the values for joystick 0 so that PosEdge and NegEdge can be used with
- // them.
- // Windows only supports 12 buttons, so we know there will never be any more.
- // Not using MASK because it doesn't make it any cleaner.
- buttons[0] |= (control_data_.enabled << (ENABLED - 9)) |
- (control_data_.autonomous << (AUTONOMOUS - 9)) |
- (control_data_.fmsAttached << (FMS_ATTACHED - 9));
-
- for (int j = 0; j < 4; ++j) {
- for (int k = 1; k <= 12; ++k) {
- if (PosEdge(j, k)) {
- LOG(INFO, "PosEdge(%d, %d)\n", j, k);
- }
- if (NegEdge(j, k)) {
- LOG(INFO, "NegEdge(%d, %d)\n", j, k);
- }
- }
- }
- if (PosEdge(0, ENABLED)) LOG(INFO, "PosEdge(ENABLED)\n");
- if (NegEdge(0, ENABLED)) LOG(INFO, "NegEdge(ENABLED)\n");
- if (PosEdge(0, AUTONOMOUS)) LOG(INFO, "PosEdge(AUTONOMOUS)\n");
- if (NegEdge(0, AUTONOMOUS)) LOG(INFO, "NegEdge(AUTONOMOUS)\n");
- if (PosEdge(0, FMS_ATTACHED)) LOG(INFO, "PosEdge(FMS_ATTACHED)\n");
- if (NegEdge(0, FMS_ATTACHED)) LOG(INFO, "NegEdge(FMS_ATTACHED)\n");
-}
-
-void JoystickInput::Run() {
- ReceiveSocket sock(NetworkPort::kDS);
- while (true) {
- if (sock.Receive(&control_data_, sizeof(control_data_)) !=
- sizeof(control_data_)) {
- LOG(WARNING, "socket receive failed\n");
- continue;
- }
- SetupButtons();
- if (!robot_state.MakeWithBuilder()
- .enabled(Pressed(0, ENABLED))
- .autonomous(Pressed(0, AUTONOMOUS))
- .team_id(ntohs(control_data_.teamID))
- .Send()) {
- LOG(WARNING, "sending robot_state failed\n");
- }
- if (robot_state.FetchLatest()) {
- char state[1024];
- robot_state->Print(state, sizeof(state));
- LOG(DEBUG, "robot_state={%s}\n", state);
- } else {
- LOG(WARNING, "fetching robot_state failed\n");
- }
- RunIteration();
- }
-}
-
-} // namespace aos
-
diff --git a/aos/atom_code/input/JoystickInput.h b/aos/atom_code/input/JoystickInput.h
deleted file mode 100644
index 313ad0a..0000000
--- a/aos/atom_code/input/JoystickInput.h
+++ /dev/null
@@ -1,46 +0,0 @@
-#ifndef AOS_INPUT_JOYSTICK_INPUT_H_
-#define AOS_INPUT_JOYSTICK_INPUT_H_
-
-#include "FRCComm.h"
-
-namespace aos {
-
-// Class for implementing atom code that reads the joystick values from the
-// cRIO.
-// Designed for a subclass that implements RunIteration to be instantiated and
-// Runed.
-// TODO(brians): rewrite this with OO buttons/fms state etc
-class JoystickInput {
- private:
- uint16_t buttons[4], old_buttons[4];
- inline uint16_t MASK(int button) {
- return 1 << ((button > 8) ? (button - 9) : (button + 7));
- }
- void SetupButtons();
- protected:
- FRCCommonControlData control_data_;
-
- // Constants that retrieve data when used with joystick 0.
- static const int ENABLED = 13;
- static const int AUTONOMOUS = 14;
- static const int FMS_ATTACHED = 15;
- bool Pressed(int stick, int button) {
- return buttons[stick] & MASK(button);
- }
- bool PosEdge(int stick, int button) {
- return !(old_buttons[stick] & MASK(button)) && (buttons[stick] & MASK(button));
- }
- bool NegEdge(int stick, int button) {
- return (old_buttons[stick] & MASK(button)) && !(buttons[stick] & MASK(button));
- }
-
- virtual void RunIteration() = 0;
- public:
- // Enters an infinite loop that reads values and calls RunIteration.
- void Run();
-};
-
-} // namespace aos
-
-#endif
-
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..e0ff434 100755
--- a/aos/build/build.sh
+++ b/aos/build/build.sh
@@ -9,10 +9,11 @@
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.
+shift 4
+shift || true # We might not have a 5th argument if ACTION is empty.
export WIND_BASE=${WIND_BASE:-"/usr/local/powerpc-wrs-vxworks/wind_base"}
@@ -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
@@ -71,7 +72,7 @@
fi
${NINJA} -C ${OUTDIR}/Default ${NINJA_ACTION} "$@"
if [[ ${ACTION} == deploy || ${ACTION} == redeploy ]]; then
- [ ${PLATFORM} == atom ] && \
+ [[ ${PLATFORM} =~ .*atom ]] && \
rsync --progress -c -r \
${OUTDIR}/Default/outputs/* \
driver@`${AOS}/build/get_ip fitpc`:/home/driver/robot_code/bin
diff --git a/bot3/atom_code/atom_code.gyp b/bot3/atom_code/atom_code.gyp
new file mode 100644
index 0000000..1b1ba1e
--- /dev/null
+++ b/bot3/atom_code/atom_code.gyp
@@ -0,0 +1,31 @@
+{
+ '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',
+ #'../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_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..841aa7f
--- /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 "$@"
diff --git a/bot3/atom_code/scripts/start_list.txt b/bot3/atom_code/scripts/start_list.txt
new file mode 100644
index 0000000..deeae24
--- /dev/null
+++ b/bot3/atom_code/scripts/start_list.txt
@@ -0,0 +1,6 @@
+BinaryLogReader
+MotorWriter
+joystick_reader
+drivetrain
+auto
+gyro_sensor_receiver
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_sensor_receiver.cc b/bot3/input/gyro_sensor_receiver.cc
new file mode 100644
index 0000000..add717f
--- /dev/null
+++ b/bot3/input/gyro_sensor_receiver.cc
@@ -0,0 +1,79 @@
+#include "aos/atom_code/init.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/util/wrapping_counter.h"
+
+#include "bot3/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/queues/GyroAngle.q.h"
+#include "frc971/input/usb_receiver.h"
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+using ::bot3::control_loops::drivetrain;
+using ::frc971::sensors::gyro;
+using ::aos::util::WrappingCounter;
+using ::frc971::USBReceiver;
+
+namespace bot3 {
+namespace {
+
+//TODO (danielp): Figure out whether the bigger gear is on the
+// encoder or not.
+inline double drivetrain_translate(int32_t in) {
+ return static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*quad*/) *
+ (44.0 / 32.0 /*encoder gears*/) * // the encoders are on the wheels.
+ (3.5 /*wheel diameter*/ * 2.54 / 100 * M_PI);
+}
+
+// TODO (danielp): This needs to be modified eventually.
+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);
+}
+
+} // namespace
+
+class GyroSensorReceiver : public USBReceiver {
+ virtual void ProcessData() override {
+ if (data()->robot_id != 0) {
+ LOG(ERROR, "gyro board sent data for robot id %hhd!"
+ " dip switches are %x\n",
+ data()->robot_id, data()->base_status & 0xF);
+ return;
+ } else {
+ LOG(DEBUG, "processing a packet dip switches %x\n",
+ data()->base_status & 0xF);
+ }
+
+ gyro.MakeWithBuilder()
+ .angle(data()->gyro_angle / 16.0 / 1000.0 / 180.0 * M_PI)
+ .Send();
+
+ LOG(INFO, "Got encoder data: %lf, %lf\n",
+ drivetrain_translate(data()->main.right_drive),
+ drivetrain_translate(data()->main.left_drive));
+
+ drivetrain.position.MakeWithBuilder()
+ .right_encoder(drivetrain_translate(data()->main.right_drive))
+ .left_encoder(-drivetrain_translate(data()->main.left_drive))
+ .Send();
+ }
+
+ WrappingCounter top_rise_;
+ WrappingCounter top_fall_;
+ WrappingCounter bottom_rise_;
+ WrappingCounter bottom_fall_delay_;
+ WrappingCounter bottom_fall_;
+};
+
+} // namespace bot3
+
+int main() {
+ ::aos::Init();
+ ::bot3::GyroSensorReceiver receiver;
+ while (true) {
+ receiver.RunIteration();
+ }
+ ::aos::Cleanup();
+}
diff --git a/bot3/input/input.gyp b/bot3/input/input.gyp
new file mode 100644
index 0000000..b428dc2
--- /dev/null
+++ b/bot3/input/input.gyp
@@ -0,0 +1,54 @@
+{
+ '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',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/util/util.gyp:wrapping_counter',
+ 'usb_receiver',
+ ],
+ },
+ {
+ 'target_name': 'usb_receiver',
+ 'type': 'static_library',
+ 'sources': [
+ '<(DEPTH)/frc971/input/usb_receiver.cc',
+ ],
+ 'dependencies': [
+ '<(DEPTH)/gyro_board/src/libusb-driver/libusb-driver.gyp:libusb_wrap',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/util/util.gyp:wrapping_counter',
+ '<(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/util/util.gyp:wrapping_counter',
+ '<(AOS)/common/common.gyp:time',
+ ],
+ },
+ ],
+}
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..c3a9858
--- /dev/null
+++ b/bot3/output/atom_motor_writer.cc
@@ -0,0 +1,64 @@
+#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 = 1;
+ values_.compressor_channel = 1; //spike
+ values_.solenoid_module = 0;
+
+ drivetrain.output.FetchLatest();
+ if (drivetrain.output.IsNewerThanMS(kOutputMaxAgeMS) && kEnableDrivetrain) {
+ SetPWMOutput(3, drivetrain.output->right_voltage / 12.0, kTalonBounds);
+ SetPWMOutput(4, -drivetrain.output->left_voltage / 12.0, kTalonBounds);
+ } else {
+ DisablePWMOutput(3);
+ DisablePWMOutput(4);
+ if (kEnableDrivetrain) {
+ LOG(WARNING, "drivetrain not new enough\n");
+ }
+ }
+ shifters.FetchLatest();
+ if (shifters.get()) {
+ SetSolenoid(7, shifters->set);
+ SetSolenoid(8, !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/build.sh b/frc971/atom_code/build.sh
index b8129ab..711ced0 100755
--- a/frc971/atom_code/build.sh
+++ b/frc971/atom_code/build.sh
@@ -1,3 +1,3 @@
#!/bin/bash
-../../aos/build/build.sh atom atom_code.gyp no "$@"
+../../aos/build/build.sh atom atom_code.gyp no atom "$@"
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/crio/build.sh b/frc971/crio/build.sh
index b3fc031..9b41ad1 100755
--- a/frc971/crio/build.sh
+++ b/frc971/crio/build.sh
@@ -1,3 +1,3 @@
#!/bin/bash
-../../aos/build/build.sh crio crio.gyp no "$@"
+../../aos/build/build.sh crio crio.gyp no crio "$@"
diff --git a/frc971/input/input.gyp b/frc971/input/input.gyp
index 23dfda6..9a26bce 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 1a8b1c9..01cbf84 100644
--- a/frc971/output/output.gyp
+++ b/frc971/output/output.gyp
@@ -26,7 +26,7 @@
'target_name': 'MotorWriter',
'type': '<(aos_target)',
'sources': [
- 'AtomMotorWriter.cc'
+ 'atom_motor_writer.cc'
],
'dependencies': [
'<(AOS)/atom_code/output/output.gyp:motor_output',
diff --git a/gyro_board/src/usb/data_struct.h b/gyro_board/src/usb/data_struct.h
index 718d19c..317b0f5 100644
--- a/gyro_board/src/usb/data_struct.h
+++ b/gyro_board/src/usb/data_struct.h
@@ -115,6 +115,7 @@
};
uint16_t booleans;
};
+ uint32_t shooter_cycle_ticks;
} bot3;
};
};
diff --git a/gyro_board/src/usb/encoder.c b/gyro_board/src/usb/encoder.c
index 77344fb..82f1b52 100644
--- a/gyro_board/src/usb/encoder.c
+++ b/gyro_board/src/usb/encoder.c
@@ -1,3 +1,5 @@
+#include <string.h>
+
#include "fill_packet.h"
#include "encoder.h"
@@ -11,6 +13,9 @@
// How long (in ms) to wait after a falling edge on the bottom indexer sensor
// before reading the indexer encoder.
static const int kBottomFallDelayTime = 32;
+// How long to wait for a revolution of the shooter wheel (on the third robot)
+// before said wheel is deemed "stopped". (In secs)
+static const uint8_t kWheelStopThreshold = 1;
// The timer to use for timestamping sensor readings.
// This is a constant to avoid hard-coding it in a lot of places, but there ARE
@@ -84,6 +89,41 @@
}
}
+static inline void reset_TC(void) {
+ TIM2->TCR |= (1 << 1); // Put it into reset.
+ while (TIM2->TC != 0) { // Wait for reset.
+ continue;
+ }
+ TIM2->TCR = 1; // Take it out of reset + make sure it's enabled.
+}
+
+// TIM2
+volatile uint32_t shooter_cycle_ticks;
+void TIMER2_IRQHandler(void) {
+ // Apparently, this handler runs regardless of a match or capture event.
+ if (TIM2->IR & (1 << 4)) {
+ // Capture
+ TIM2->IR = (1 << 3); // Clear the interrupt.
+
+ shooter_cycle_ticks = TIM2->CR0;
+
+ reset_TC();
+ } else if (TIM2->IR & 1) {
+ // Match
+ TIM2->IR = 1; // Clear the interrupt
+
+ // Assume shooter is stopped.
+ shooter_cycle_ticks = 0;
+
+ // Disable timer.
+ TIM2->TCR = 0;
+ }
+
+ // It will only handle one interrupt per run.
+ // If there is another interrupt pending, it won't be cleared, and the ISR
+ // will be run again to handle it.
+}
+
// TODO(brians): Have this indicate some kind of error instead of just looping
// infinitely in the ISR because it never clears it.
static void NoGPIO(void) {}
@@ -297,30 +337,25 @@
capture_shooter_angle_rise = encoder2_val;
}
-// Count leading zeros.
-// Returns 0 if bit 31 is set etc.
-__attribute__((always_inline)) static __INLINE uint32_t __clz(uint32_t value) {
- uint32_t result;
- __asm__("clz %0, %1" : "=r" (result) : "r" (value));
- return result;
+// Third robot shooter.
+static void ShooterPhotoFall(void) {
+ GPIOINT->IO0IntClr = (1 << 23);
+ // We reset TC to make sure we don't get a crap
+ // value from CR0 when the capture interrupt occurs
+ // if the shooter is just starting up again, and so
+ // that the match interrupt thing works right.
+ reset_TC();
}
-inline static void IRQ_Dispatch(void) {
- // There is no need to add a loop here to handle multiple interrupts at the
- // same time because the processor has tail chaining of interrupts which we
- // can't really beat with our own loop.
- // It would actually be bad because a loop here would block EINT1/2 for longer
- // lengths of time.
- uint32_t index = __clz(GPIOINT->IO2IntStatR | GPIOINT->IO0IntStatR |
- (GPIOINT->IO2IntStatF << 28) | (GPIOINT->IO0IntStatF << 4));
-
- typedef void (*Handler)(void);
- const static Handler table[] = {
+typedef void (*Handler)(void);
+// Contains default pointers for ISR functions.
+// (These can be used without modifications on the comp/practice bots.)
+Handler ISRTable[] = {
Encoder5BFall, // index 0: P2.3 Fall #bit 31 //Encoder 5 B //Dio 10
Encoder5AFall, // index 1: P2.2 Fall #bit 30 //Encoder 5 A //Dio 9
Encoder4BFall, // index 2: P2.1 Fall #bit 29 //Encoder 4 B //Dio 8
Encoder4AFall, // index 3: P2.0 Fall #bit 28 //Encoder 4 A //Dio 7
- NoGPIO, // index 4: NO GPIO #bit 27
+ NoGPIO, // index 4: NO GPIO #bit 27
Encoder2AFall, // index 5: P0.22 Fall #bit 26 //Encoder 2 A
Encoder2BFall, // index 6: P0.21 Fall #bit 25 //Encoder 2 B
Encoder3AFall, // index 7: P0.20 Fall #bit 24 //Encoder 3 A
@@ -349,12 +384,31 @@
Encoder4BRise, // index 30: P2.1 Rise #bit 1 //Encoder 4 B //Dio 8
Encoder4ARise, // index 31: P2.0 Rise #bit 0 //Encoder 4 A //Dio 7
NoGPIO // index 32: NO BITS SET #False Alarm
- };
- table[index]();
+};
+
+// Count leading zeros.
+// Returns 0 if bit 31 is set etc.
+__attribute__((always_inline)) static __INLINE uint32_t __clz(uint32_t value) {
+ uint32_t result;
+ __asm__("clz %0, %1" : "=r" (result) : "r" (value));
+ return result;
+}
+inline static void IRQ_Dispatch(void) {
+ // There is no need to add a loop here to handle multiple interrupts at the
+ // same time because the processor has tail chaining of interrupts which we
+ // can't really beat with our own loop.
+ // It would actually be bad because a loop here would block EINT1/2 for longer
+ // lengths of time.
+
+ uint32_t index = __clz(GPIOINT->IO2IntStatR | GPIOINT->IO0IntStatR |
+ (GPIOINT->IO2IntStatF << 28) | (GPIOINT->IO0IntStatF << 4));
+
+ ISRTable[index]();
}
void EINT3_IRQHandler(void) {
IRQ_Dispatch();
}
+
int32_t encoder_val(int chan) {
int32_t val;
switch (chan) {
@@ -420,20 +474,7 @@
QEI->QEICONF = 0x00000004;
// Wrap back to 0 when we wrap the int and vice versa.
QEI->QEIMAXPOS = 0xFFFFFFFF;
-
- // Set up encoder 1.
- // Make GPIOs 2.11 and 2.12 trigger EINT1 and EINT2 (respectively).
- // PINSEL4[23:22] = {0 1}
- // PINSEL4[25:24] = {0 1}
- PINCON->PINSEL4 = (PINCON->PINSEL4 & ~(0x3 << 22)) | (0x1 << 22);
- PINCON->PINSEL4 = (PINCON->PINSEL4 & ~(0x3 << 24)) | (0x1 << 24);
- // Clear the interrupt flags for EINT1 and EINT2 (0x6 = 0b0110).
- SC->EXTMODE = 0x6;
- SC->EXTINT = 0x6;
- NVIC_EnableIRQ(EINT1_IRQn);
- NVIC_EnableIRQ(EINT2_IRQn);
- encoder1_val = 0;
-
+
// Set up encoder 2.
GPIOINT->IO0IntEnF |= (1 << 22); // Set GPIO falling interrupt.
GPIOINT->IO0IntEnR |= (1 << 22); // Set GPIO rising interrupt.
@@ -453,32 +494,70 @@
PINCON->PINSEL1 &= ~(0x3 << 8);
PINCON->PINSEL1 &= ~(0x3 << 6);
encoder3_val = 0;
-
- // Set up encoder 4.
- GPIOINT->IO2IntEnF |= (1 << 0); // Set GPIO falling interrupt.
- GPIOINT->IO2IntEnR |= (1 << 0); // Set GPIO rising interrupt.
- GPIOINT->IO2IntEnF |= (1 << 1); // Set GPIO falling interrupt.
- GPIOINT->IO2IntEnR |= (1 << 1); // Set GPIO rising interrupt.
- // Make sure they're in mode 00 (the default, aka nothing special).
- PINCON->PINSEL4 &= ~(0x3 << 0);
- PINCON->PINSEL4 &= ~(0x3 << 2);
- encoder4_val = 0;
-
- // Set up encoder 5.
- GPIOINT->IO2IntEnF |= (1 << 2); // Set GPIO falling interrupt.
- GPIOINT->IO2IntEnR |= (1 << 2); // Set GPIO rising interrupt.
- GPIOINT->IO2IntEnF |= (1 << 3); // Set GPIO falling interrupt.
- GPIOINT->IO2IntEnR |= (1 << 3); // Set GPIO rising interrupt.
- // Make sure they're in mode 00 (the default, aka nothing special).
- PINCON->PINSEL4 &= ~(0x3 << 4);
- PINCON->PINSEL4 &= ~(0x3 << 6);
- encoder5_val = 0;
-
+
// Enable interrupts from the GPIO pins.
NVIC_EnableIRQ(EINT3_IRQn);
if (is_bot3) {
+ // Modify robot handler table for third robot.
+ ISRTable[23] = ShooterPhotoFall;
+
+ // Set up timer for bot3 photosensor.
+ // Make sure timer two is powered.
+ SC->PCONP |= (1 << 22);
+ // We don't need all the precision the CCLK can provide.
+ // We'll use CCLK/8. (12.5 mhz).
+ SC->PCLKSEL1 |= (0x3 << 12);
+ // Use timer prescale to get that freq down to 500 hz.
+ TIM2->PR = 25000;
+ // Select capture 2.0 function on pin 0.4.
+ PINCON->PINSEL0 |= (0x3 << 8);
+ // Set timer to capture and interrupt on rising edge.
+ TIM2->CCR = 0x5;
+ // Set up match interrupt.
+ TIM2->MR0 = kWheelStopThreshold * 500;
+ TIM2->MCR = 1;
+ // Enable timer IRQ, and make it lower priority than the encoders.
+ NVIC_SetPriority(TIMER3_IRQn, 1);
+ NVIC_EnableIRQ(TIMER3_IRQn);
+ // Set up GPIO interrupt on other edge.
+ GPIOINT->IO0IntEnF |= (1 << 23);
+
} else { // is main robot
+ // Set up encoder 1.
+ // Make GPIOs 2.11 and 2.12 trigger EINT1 and EINT2 (respectively).
+ // PINSEL4[23:22] = {0 1}
+ // PINSEL4[25:24] = {0 1}
+ PINCON->PINSEL4 = (PINCON->PINSEL4 & ~(0x3 << 22)) | (0x1 << 22);
+ PINCON->PINSEL4 = (PINCON->PINSEL4 & ~(0x3 << 24)) | (0x1 << 24);
+ // Clear the interrupt flags for EINT1 and EINT2 (0x6 = 0b0110).
+ SC->EXTMODE = 0x6;
+ SC->EXTINT = 0x6;
+ NVIC_EnableIRQ(EINT1_IRQn);
+ NVIC_EnableIRQ(EINT2_IRQn);
+ encoder1_val = 0;
+
+ // Set up encoder 4.
+ GPIOINT->IO2IntEnF |= (1 << 0); // Set GPIO falling interrupt.
+ GPIOINT->IO2IntEnR |= (1 << 0); // Set GPIO rising interrupt.
+ GPIOINT->IO2IntEnF |= (1 << 1); // Set GPIO falling interrupt.
+ GPIOINT->IO2IntEnR |= (1 << 1); // Set GPIO rising interrupt.
+ // Make sure they're in mode 00 (the default, aka nothing special).
+ PINCON->PINSEL4 &= ~(0x3 << 0);
+ PINCON->PINSEL4 &= ~(0x3 << 2);
+ encoder4_val = 0;
+
+ // Set up encoder 5.
+ GPIOINT->IO2IntEnF |= (1 << 2); // Set GPIO falling interrupt.
+ GPIOINT->IO2IntEnR |= (1 << 2); // Set GPIO rising interrupt.
+ GPIOINT->IO2IntEnF |= (1 << 3); // Set GPIO falling interrupt.
+ GPIOINT->IO2IntEnR |= (1 << 3); // Set GPIO rising interrupt.
+ // Make sure they're in mode 00 (the default, aka nothing special).
+ PINCON->PINSEL4 &= ~(0x3 << 4);
+ PINCON->PINSEL4 &= ~(0x3 << 6);
+ encoder5_val = 0;
+
+
xTaskCreate(vDelayCapture,
(signed char *) "SENSORs",
configMINIMAL_STACK_SIZE + 100,
@@ -530,12 +609,18 @@
if (is_bot3) {
packet->robot_id = 1;
+
+ packet->main.left_drive = encoder3_val;
+ packet->main.right_drive = encoder2_val;
+
+ packet->bot3.shooter_cycle_ticks = shooter_cycle_ticks;
} else { // is main robot
packet->robot_id = 2;
- packet->main.shooter = encoder1_val;
packet->main.left_drive = encoder5_val;
packet->main.right_drive = encoder4_val;
+
+ packet->main.shooter = encoder1_val;
packet->main.indexer = encoder3_val;
packet->main.battery_voltage = analog(3);
packet->main.left_drive_hall = analog(1);