Merging in Brian's latest gyro board code.
Conflicts:
gyro_board/src/usb/encoder.c
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..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/aos/common/control_loop/ControlLoop-tmpl.h b/aos/common/control_loop/ControlLoop-tmpl.h
index 69c3121..5d22300 100644
--- a/aos/common/control_loop/ControlLoop-tmpl.h
+++ b/aos/common/control_loop/ControlLoop-tmpl.h
@@ -53,16 +53,16 @@
if (!control_loop_->position.IsNewerThanMS(kPositionTimeoutMs)) {
LOG(ERROR, "Stale position. %d ms > %d ms. Outputs disabled.\n",
msec_age, kPositionTimeoutMs);
- ZeroOutputs();
- return;
+ //ZeroOutputs();
+ //eturn;
} else {
LOG(ERROR, "Stale position. %d ms\n", msec_age);
}
} else {
LOG(ERROR, "Never had a position.\n");
if (fail_no_position) {
- ZeroOutputs();
- return;
+ // ZeroOutputs();
+ // return;
}
}
}
diff --git a/bot3/atom_code/atom_code.gyp b/bot3/atom_code/atom_code.gyp
new file mode 100644
index 0000000..d507b40
--- /dev/null
+++ b/bot3/atom_code/atom_code.gyp
@@ -0,0 +1,28 @@
+{
+ '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)/bot3/control_loops/shooter/shooter.gyp:shooter_lib_test',
+ '<(DEPTH)/bot3/control_loops/shooter/shooter.gyp:shooter',
+ '<(DEPTH)/bot3/autonomous/autonomous.gyp:auto',
+ '<(DEPTH)/bot3/input/input.gyp:joystick_reader',
+ '<(DEPTH)/bot3/output/output.gyp:MotorWriter',
+ '<(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..f8a0ba7
--- /dev/null
+++ b/bot3/atom_code/scripts/start_list.txt
@@ -0,0 +1,7 @@
+BinaryLogReader
+MotorWriter
+joystick_reader
+drivetrain
+auto
+shooter
+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..bf5f4ff
--- /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:constants',
+ '<(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..7cc5191
--- /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:constants',
+ '<(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/control_loops/shooter/shooter.cc b/bot3/control_loops/shooter/shooter.cc
new file mode 100644
index 0000000..25bfc74
--- /dev/null
+++ b/bot3/control_loops/shooter/shooter.cc
@@ -0,0 +1,83 @@
+#include "bot3/control_loops/shooter/shooter.h"
+#include "bot3/control_loops/shooter/shooter_motor.q.h"
+
+#include "aos/common/control_loop/control_loops.q.h"
+#include "aos/common/logging/logging.h"
+
+#include "bot3/control_loops/shooter/shooter_motor_plant.h"
+
+namespace bot3 {
+namespace control_loops {
+
+ShooterMotor::ShooterMotor(control_loops::ShooterLoop *my_shooter)
+ : aos::control_loops::ControlLoop<control_loops::ShooterLoop>(my_shooter),
+ loop_(new StateFeedbackLoop<1, 1, 1>(MakeShooterLoop())),
+ last_velocity_goal_(0) {
+ loop_->Reset();
+ U_add = loop_->B().inverse() * (loop_->A().Identity() - loop_->A());
+}
+
+/*static*/ const double ShooterMotor::dt = 0.01;
+
+void ShooterMotor::RunIteration(
+ const control_loops::ShooterLoop::Goal *goal,
+ const control_loops::ShooterLoop::Position *position,
+ control_loops::ShooterLoop::Output *output,
+ control_loops::ShooterLoop::Status *status) {
+ double velocity_goal = goal->velocity;
+ // Our position here is actually a velocity.
+ average_velocity_ =
+ (position == NULL ? loop_->X_hat(0, 0) : position->velocity);
+ double output_voltage = 0.0;
+
+/* if (index_loop.status.FetchLatest() || index_loop.status.get()) {
+ if (index_loop.status->is_shooting) {
+ if (velocity_goal != last_velocity_goal_ &&
+ velocity_goal < 130) {
+ velocity_goal = last_velocity_goal_;
+ }
+ }
+ } else {
+ LOG(WARNING, "assuming index isn't shooting\n");
+ }*/
+ last_velocity_goal_ = velocity_goal;
+
+ loop_->Y << average_velocity_;
+ loop_->R << velocity_goal;
+
+ loop_->Update(position, output == NULL, U_add * loop_->R);
+
+ // Kill power at low velocity goals.
+ if (velocity_goal < 1.0) {
+ loop_->U(0) = 0.0;
+ } else {
+ output_voltage = loop_->U(0);
+ }
+
+ LOG(DEBUG,
+ "PWM: %f, raw_velocity: %f, xhat[0]: %f, R[0]: %f\n",
+ output_voltage, average_velocity_, loop_->X_hat[0], loop_->R[0]);
+
+ status->average_velocity = average_velocity_;
+
+ // Determine if the velocity is close enough to the goal to be ready.
+ if (std::abs(velocity_goal - average_velocity_) < 10.0 &&
+ velocity_goal != 0.0) {
+ LOG(DEBUG, "Steady: ");
+ status->ready = true;
+ } else {
+ LOG(DEBUG, "Not ready: ");
+ status->ready = false;
+ }
+ LOG(DEBUG, "avg = %f goal = %f\n", average_velocity_, velocity_goal);
+
+ if (output) {
+ output->voltage = output_voltage;
+ output->intake = goal->intake;
+ output->push = goal->push;
+ LOG(DEBUG, "goal: %lf, volt: %lf, push:%d\n", goal->intake, output_voltage, goal->push);
+ }
+}
+
+} // namespace control_loops
+} // namespace bot3
diff --git a/bot3/control_loops/shooter/shooter.gyp b/bot3/control_loops/shooter/shooter.gyp
new file mode 100644
index 0000000..2be0439
--- /dev/null
+++ b/bot3/control_loops/shooter/shooter.gyp
@@ -0,0 +1,79 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'shooter_loop',
+ 'type': 'static_library',
+ 'sources': ['shooter_motor.q'],
+ 'variables': {
+ 'header_path': 'bot3/control_loops/shooter',
+ },
+ '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': 'shooter_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'shooter.cc',
+ 'shooter_motor_plant.cc',
+ ],
+ 'dependencies': [
+ 'shooter_loop',
+ '<(AOS)/common/common.gyp:controls',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
+ '<(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',
+ 'shooter_loop',
+ ],
+ },
+ {
+ 'target_name': 'shooter_lib_test',
+ 'type': 'executable',
+ 'sources': [
+ 'shooter_lib_test.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ 'shooter_loop',
+ 'shooter_lib',
+ '<(AOS)/common/common.gyp:queue_testutils',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ ],
+ },
+ {
+ 'target_name': 'shooter_csv',
+ 'type': 'executable',
+ 'sources': [
+ 'shooter_csv.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/common.gyp:timing',
+ 'shooter_loop',
+ ],
+ },
+ {
+ 'target_name': 'shooter',
+ 'type': 'executable',
+ 'sources': [
+ 'shooter_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/atom_code/atom_code.gyp:init',
+ 'shooter_lib',
+ 'shooter_loop',
+ ],
+ },
+ ],
+}
diff --git a/bot3/control_loops/shooter/shooter.h b/bot3/control_loops/shooter/shooter.h
new file mode 100644
index 0000000..b9514c3
--- /dev/null
+++ b/bot3/control_loops/shooter/shooter.h
@@ -0,0 +1,51 @@
+#ifndef BOT3_CONTROL_LOOPS_SHOOTER_H_
+#define BOT3_CONTROL_LOOPS_SHOOTER_H_
+
+#include <memory>
+
+#include "aos/common/control_loop/ControlLoop.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "bot3/control_loops/shooter/shooter_motor.q.h"
+#include "bot3/control_loops/shooter/shooter_motor_plant.h"
+
+namespace bot3 {
+namespace control_loops {
+
+class ShooterMotor
+ : public aos::control_loops::ControlLoop<control_loops::ShooterLoop> {
+ public:
+ explicit ShooterMotor(
+ control_loops::ShooterLoop *my_shooter = &control_loops::shooter);
+
+ // Control loop time step.
+ static const double dt;
+
+ // Maximum speed of the shooter wheel which the encoder is rated for in
+ // rad/sec.
+ static const double kMaxSpeed;
+
+ protected:
+ virtual void RunIteration(
+ const control_loops::ShooterLoop::Goal *goal,
+ const control_loops::ShooterLoop::Position *position,
+ control_loops::ShooterLoop::Output *output,
+ control_loops::ShooterLoop::Status *status);
+
+ private:
+ // The state feedback control loop to talk to.
+ ::std::unique_ptr<StateFeedbackLoop<1, 1, 1>> loop_;
+
+ double average_velocity_;
+
+ // For making sure it keeps spinning if we're shooting.
+ double last_velocity_goal_;
+
+ Eigen::Matrix<double, 1, 1> U_add;
+
+ DISALLOW_COPY_AND_ASSIGN(ShooterMotor);
+};
+
+} // namespace control_loops
+} // namespace bot3
+
+#endif // BOT3_CONTROL_LOOPS_SHOOTER_H_
diff --git a/bot3/control_loops/shooter/shooter_csv.cc b/bot3/control_loops/shooter/shooter_csv.cc
new file mode 100644
index 0000000..c929f7e
--- /dev/null
+++ b/bot3/control_loops/shooter/shooter_csv.cc
@@ -0,0 +1,50 @@
+#include "stdio.h"
+
+#include "aos/common/control_loop/Timing.h"
+#include "aos/common/time.h"
+#include "bot3/control_loops/shooter/shooter_motor.q.h"
+
+using ::bot3::control_loops::shooter;
+using ::aos::time::Time;
+
+int main(int argc, char * argv[]) {
+ FILE *data_file = NULL;
+ FILE *output_file = NULL;
+
+ if (argc == 2) {
+ data_file = fopen(argv[1], "w");
+ output_file = data_file;
+ } else {
+ printf("Logging to stdout instead\n");
+ output_file = stdout;
+ }
+
+ fprintf(data_file, "time, power, position");
+
+ ::aos::Init();
+
+ Time start_time = Time::Now();
+
+ while (true) {
+ ::aos::time::PhasedLoop10MS(2000);
+ shooter.goal.FetchLatest();
+ shooter.status.FetchLatest();
+ shooter.position.FetchLatest();
+ shooter.output.FetchLatest();
+ if (shooter.output.get() &&
+ shooter.position.get()) {
+ fprintf(output_file, "\n%f, %f, %f",
+ (shooter.position->sent_time - start_time).ToSeconds(),
+ shooter.output->voltage,
+ shooter.position->position);
+ }
+ }
+
+ if (data_file) {
+ fclose(data_file);
+ }
+
+ ::aos::Cleanup();
+ return 0;
+}
+
diff --git a/bot3/control_loops/shooter/shooter_lib_test.cc b/bot3/control_loops/shooter/shooter_lib_test.cc
new file mode 100644
index 0000000..445ef57
--- /dev/null
+++ b/bot3/control_loops/shooter/shooter_lib_test.cc
@@ -0,0 +1,174 @@
+#include <unistd.h>
+
+#include <memory>
+
+#include "gtest/gtest.h"
+#include "aos/common/queue.h"
+#include "aos/common/queue_testutils.h"
+#include "bot3/control_loops/shooter/shooter_motor.q.h"
+#include "bot3/control_loops/shooter/shooter.h"
+#include "frc971/constants.h"
+
+using ::aos::time::Time;
+
+namespace bot3 {
+namespace control_loops {
+namespace testing {
+
+// Class which simulates the shooter and sends out queue messages with the
+// position.
+class ShooterMotorSimulation {
+ public:
+ // Constructs a shooter simulation. I'm not sure how the construction of
+ // the queue (my_shooter_loop_) actually works (same format as wrist).
+ ShooterMotorSimulation()
+ : shooter_plant_(new StateFeedbackPlant<1, 1, 1>(MakeShooterPlant())),
+ my_shooter_loop_(".bot3.control_loops.shooter",
+ 0x78d8e372, ".bot3.control_loops.shooter.goal",
+ ".bot3.control_loops.shooter.position",
+ ".bot3.control_loops.shooter.output",
+ ".bot3.control_loops.shooter.status") {
+ }
+
+ // Sends a queue message with the position of the shooter.
+ void SendPositionMessage() {
+ ::aos::ScopedMessagePtr<control_loops::ShooterLoop::Position> position =
+ my_shooter_loop_.position.MakeMessage();
+ position->velocity = shooter_plant_->Y(0, 0);
+ position.Send();
+ }
+
+ // Simulates shooter for a single timestep.
+ void Simulate() {
+ EXPECT_TRUE(my_shooter_loop_.output.FetchLatest());
+ shooter_plant_->U << my_shooter_loop_.output->voltage;
+ shooter_plant_->Update();
+ }
+
+ ::std::unique_ptr<StateFeedbackPlant<1, 1, 1>> shooter_plant_;
+
+ private:
+ ShooterLoop my_shooter_loop_;
+};
+
+class ShooterTest : public ::testing::Test {
+ protected:
+ ShooterTest() : my_shooter_loop_(".bot3.control_loops.shooter",
+ 0x78d8e372,
+ ".bot3.control_loops.shooter.goal",
+ ".bot3.control_loops.shooter.position",
+ ".bot3.control_loops.shooter.output",
+ ".bot3.control_loops.shooter.status"),
+ shooter_motor_(&my_shooter_loop_),
+ shooter_motor_plant_() {
+ // Flush the robot state queue so we can use clean shared memory for this
+ // test.
+ ::aos::robot_state.Clear();
+ SendDSPacket(true);
+ }
+
+ virtual ~ShooterTest() {
+ ::aos::robot_state.Clear();
+ }
+
+ // Update the robot state. Without this, the Iteration of the control loop
+ // will stop all the motors and the shooter won't go anywhere.
+ void SendDSPacket(bool enabled) {
+ ::aos::robot_state.MakeWithBuilder().enabled(enabled)
+ .autonomous(false)
+ .team_id(971).Send();
+ ::aos::robot_state.FetchLatest();
+ }
+
+ void VerifyNearGoal() {
+ my_shooter_loop_.goal.FetchLatest();
+ my_shooter_loop_.status.FetchLatest();
+ EXPECT_NEAR(my_shooter_loop_.goal->velocity,
+ my_shooter_loop_.status->average_velocity,
+ 10.0);
+ }
+
+ // Bring up and down Core.
+ ::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 pointed to
+ // shared memory that is no longer valid.
+ ShooterLoop my_shooter_loop_;
+
+ // Create a control loop and simulation.
+ ShooterMotor shooter_motor_;
+ ShooterMotorSimulation shooter_motor_plant_;
+};
+
+// Tests that the shooter does nothing when the goal is zero.
+TEST_F(ShooterTest, DoesNothing) {
+ my_shooter_loop_.goal.MakeWithBuilder().velocity(0.0).Send();
+ SendDSPacket(true);
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ VerifyNearGoal();
+ my_shooter_loop_.output.FetchLatest();
+ EXPECT_EQ(my_shooter_loop_.output->voltage, 0.0);
+}
+
+// Tests that the shooter spins up to speed and that it then spins down
+// without applying any power.
+TEST_F(ShooterTest, SpinUpAndDown) {
+ my_shooter_loop_.goal.MakeWithBuilder().velocity(450.0).Send();
+ bool is_done = false;
+ while (!is_done) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(true);
+ EXPECT_TRUE(my_shooter_loop_.status.FetchLatest());
+ is_done = my_shooter_loop_.status->ready;
+ }
+ VerifyNearGoal();
+
+ my_shooter_loop_.goal.MakeWithBuilder().velocity(0.0).Send();
+ for (int i = 0; i < 100; ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(true);
+ EXPECT_TRUE(my_shooter_loop_.output.FetchLatest());
+ EXPECT_EQ(0.0, my_shooter_loop_.output->voltage);
+ }
+}
+
+// Tests that the shooter can spin up nicely while missing position packets.
+TEST_F(ShooterTest, MissingPositionMessages) {
+ my_shooter_loop_.goal.MakeWithBuilder().velocity(200.0).Send();
+ for (int i = 0; i < 100; ++i) {
+ if (i % 7) {
+ shooter_motor_plant_.SendPositionMessage();
+ }
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+
+ VerifyNearGoal();
+}
+
+// Tests that the shooter can spin up nicely while disabled for a while.
+TEST_F(ShooterTest, Disabled) {
+ my_shooter_loop_.goal.MakeWithBuilder().velocity(200.0).Send();
+ for (int i = 0; i < 100; ++i) {
+ if (i % 7) {
+ shooter_motor_plant_.SendPositionMessage();
+ }
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(i > 50);
+ }
+
+ VerifyNearGoal();
+}
+
+} // namespace testing
+} // namespace control_loops
+} // namespace bot3
diff --git a/bot3/control_loops/shooter/shooter_main.cc b/bot3/control_loops/shooter/shooter_main.cc
new file mode 100644
index 0000000..9907cfe
--- /dev/null
+++ b/bot3/control_loops/shooter/shooter_main.cc
@@ -0,0 +1,11 @@
+#include "bot3/control_loops/shooter/shooter.h"
+
+#include "aos/atom_code/init.h"
+
+int main() {
+ ::aos::Init();
+ bot3::control_loops::ShooterMotor shooter;
+ shooter.Run();
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/bot3/control_loops/shooter/shooter_motor.q b/bot3/control_loops/shooter/shooter_motor.q
new file mode 100644
index 0000000..d581c5d
--- /dev/null
+++ b/bot3/control_loops/shooter/shooter_motor.q
@@ -0,0 +1,46 @@
+package bot3.control_loops;
+
+import "aos/common/control_loop/control_loops.q";
+
+queue_group ShooterLoop {
+ implements aos.control_loops.ControlLoop;
+
+ message Goal {
+ // Goal velocity in rad/sec
+ double velocity;
+ // PWM output to intake.
+ double intake;
+ // Whether to activate pusher piston.
+ bool push;
+ };
+
+ message Status {
+ // True if the shooter is up to speed.
+ bool ready;
+ // The average velocity over the last 0.1 seconds.
+ double average_velocity;
+ // True if Frisbees are being fired into the shooter.
+ bool firing;
+ };
+
+ message Position {
+ // The speed of the shooter wheel measured in rad/sec.
+ double velocity;
+ };
+
+ message Output {
+ // Output to shooter, Volts.
+ double voltage;
+ // PWM output to intake.
+ double intake;
+ // Whether to activate pusher piston.
+ bool push;
+ };
+
+ queue Goal goal;
+ queue Position position;
+ queue Output output;
+ queue Status status;
+};
+
+queue_group ShooterLoop shooter;
diff --git a/bot3/control_loops/shooter/shooter_motor_plant.cc b/bot3/control_loops/shooter/shooter_motor_plant.cc
new file mode 100644
index 0000000..79fd6be
--- /dev/null
+++ b/bot3/control_loops/shooter/shooter_motor_plant.cc
@@ -0,0 +1,47 @@
+#include "frc971/control_loops/shooter/shooter_motor_plant.h"
+
+#include <vector>
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace bot3 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<1, 1, 1> MakeShooterPlantCoefficients() {
+ Eigen::Matrix<double, 1, 1> A;
+ A << 0.989057756738;
+ Eigen::Matrix<double, 1, 1> B;
+ B << 0.533205953514;
+ Eigen::Matrix<double, 1, 1> C;
+ C << 1;
+ Eigen::Matrix<double, 1, 1> D;
+ D << 0;
+ Eigen::Matrix<double, 1, 1> U_max;
+ U_max << 12.0;
+ Eigen::Matrix<double, 1, 1> U_min;
+ U_min << -12.0;
+ return StateFeedbackPlantCoefficients<1, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackController<1, 1, 1> MakeShooterController() {
+ Eigen::Matrix<double, 1, 1> L;
+ L << 0.539057756738;
+ Eigen::Matrix<double, 1, 1> K;
+ K << 0.354567977894;
+ return StateFeedbackController<1, 1, 1>(L, K, MakeShooterPlantCoefficients());
+}
+
+StateFeedbackPlant<1, 1, 1> MakeShooterPlant() {
+ ::std::vector<StateFeedbackPlantCoefficients<1, 1, 1> *> plants(1);
+ plants[0] = new StateFeedbackPlantCoefficients<1, 1, 1>(MakeShooterPlantCoefficients());
+ return StateFeedbackPlant<1, 1, 1>(plants);
+}
+
+StateFeedbackLoop<1, 1, 1> MakeShooterLoop() {
+ ::std::vector<StateFeedbackController<1, 1, 1> *> controllers(1);
+ controllers[0] = new StateFeedbackController<1, 1, 1>(MakeShooterController());
+ return StateFeedbackLoop<1, 1, 1>(controllers);
+}
+
+} // namespace control_loops
+} // namespace bot3
diff --git a/bot3/control_loops/shooter/shooter_motor_plant.h b/bot3/control_loops/shooter/shooter_motor_plant.h
new file mode 100644
index 0000000..1ab9702
--- /dev/null
+++ b/bot3/control_loops/shooter/shooter_motor_plant.h
@@ -0,0 +1,20 @@
+#ifndef BOT3_CONTROL_LOOPS_SHOOTER_SHOOTER_MOTOR_PLANT_H_
+#define BOT3_CONTROL_LOOPS_SHOOTER_SHOOTER_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace bot3 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<1, 1, 1> MakeShooterPlantCoefficients();
+
+StateFeedbackController<1, 1, 1> MakeShooterController();
+
+StateFeedbackPlant<1, 1, 1> MakeShooterPlant();
+
+StateFeedbackLoop<1, 1, 1> MakeShooterLoop();
+
+} // namespace control_loops
+} // namespace bot3
+
+#endif // BOT3_CONTROL_LOOPS_SHOOTER_SHOOTER_MOTOR_PLANT_H_
diff --git a/bot3/control_loops/update_shooter.sh b/bot3/control_loops/update_shooter.sh
new file mode 100755
index 0000000..26e7ae3
--- /dev/null
+++ b/bot3/control_loops/update_shooter.sh
@@ -0,0 +1,5 @@
+#!/bin/bash
+#
+# Updates the shooter controller.
+
+../../frc971/control_loops/python/shooter.py shooter/shooter_motor_plant.h shooter/shooter_motor_plant.cc
diff --git a/bot3/input/gyro_sensor_receiver.cc b/bot3/input/gyro_sensor_receiver.cc
new file mode 100644
index 0000000..0331d24
--- /dev/null
+++ b/bot3/input/gyro_sensor_receiver.cc
@@ -0,0 +1,77 @@
+#include <inttypes.h>
+
+#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 "bot3/control_loops/shooter/shooter_motor.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 ::bot3::control_loops::shooter;
+using ::frc971::sensors::gyro;
+using ::aos::util::WrappingCounter;
+using ::frc971::USBReceiver;
+
+namespace bot3 {
+namespace {
+
+inline double drivetrain_translate(int32_t in) {
+ return static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*quad*/) *
+ (32.0 / 44.0 /*encoder gears*/) * // the encoders are on the wheels.
+ (3.5 /*wheel diameter*/ * 2.54 / 100 * M_PI);
+}
+
+inline double shooter_translate(int32_t in) {
+ return 1.0 / (static_cast<double>(in) / (10 ^ 8)/*ticks per sec*/)
+ * (2 * M_PI);
+}
+
+inline double gyro_translate(int64_t in) {
+ return in / 16.0 / 1000.0 / (180.0 / M_PI);
+}
+
+} // namespace
+
+class GyroSensorReceiver : public USBReceiver {
+ public:
+ GyroSensorReceiver() : USBReceiver(1) {}
+
+ virtual void ProcessData(const ::aos::time::Time &/*timestamp*/) override {
+ gyro.MakeWithBuilder()
+ .angle(gyro_translate(data()->gyro_angle))
+ .Send();
+
+ LOG(DEBUG, "right drive: %f, left drive: %f\n",
+ drivetrain_translate(data()->main.shooter_angle),
+ drivetrain_translate(data()->main.indexer));
+ drivetrain.position.MakeWithBuilder()
+ .right_encoder(drivetrain_translate(data()->main.wrist))
+ .left_encoder(drivetrain_translate(data()->main.shooter))
+ .Send();
+ /*drivetrain.position.MakeWithBuilder()
+ .right_encoder(0)
+ .left_encoder(0)
+ .Send();*/
+
+ shooter.position.MakeWithBuilder()
+ .velocity(shooter_translate(data()->bot3.shooter_cycle_ticks));
+ }
+};
+
+} // namespace bot3
+
+int main() {
+ ::aos::Init(frc971::USBReceiver::kRelativePriority);
+ ::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..b43fc48
--- /dev/null
+++ b/bot3/input/input.gyp
@@ -0,0 +1,56 @@
+{
+ '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)/bot3/control_loops/shooter/shooter.gyp:shooter_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)/bot3/control_loops/shooter/shooter.gyp:shooter_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..199d1e6
--- /dev/null
+++ b/bot3/input/joystick_reader.cc
@@ -0,0 +1,238 @@
+#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/control_loops/shooter/shooter_motor.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 ::bot3::control_loops::shooter;
+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 kPush(3, 9);
+
+const ButtonLocation kFire(3, 3);
+const ButtonLocation kIntake(3, 4);
+
+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;
+ }
+
+ // 3, 4, 9, 9 fires, 3 pickups
+
+ shooter.status.FetchLatest();
+ bool push = false;
+ double velocity = 0.0;
+ double intake = 0.0;
+ if (data.IsPressed(kPush) && shooter.status->ready) {
+ push = true;
+ }
+ if (data.IsPressed(kFire)) {
+ velocity = 500;
+ }
+ else if (data.IsPressed(ButtonLocation(3, 1))) {
+ velocity = 50;
+ }
+ else if (data.IsPressed(ButtonLocation(3, 2))) {
+ velocity = 250;
+ }
+ else if (data.IsPressed(ButtonLocation(3, 5))) {
+ velocity = 300;
+ }
+ else if (data.IsPressed(ButtonLocation(3, 7))) {
+ velocity = 350;
+ }
+ else if (data.IsPressed(ButtonLocation(3, 8))) {
+ velocity = 400;
+ }
+ else if (data.IsPressed(ButtonLocation(3, 10))) {
+ velocity = 450;
+ }
+ if (data.IsPressed(kIntake)) {
+ intake = 0.8;
+ }
+ shooter.goal.MakeWithBuilder().intake(intake).velocity(velocity).push(push).Send();
+#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/atom_motor_writer.cc b/bot3/output/atom_motor_writer.cc
new file mode 100644
index 0000000..a16dfc4
--- /dev/null
+++ b/bot3/output/atom_motor_writer.cc
@@ -0,0 +1,137 @@
+#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"
+#include "bot3/control_loops/shooter/shooter_motor.q.h"
+
+using ::bot3::control_loops::drivetrain;
+using ::bot3::control_loops::shooter;
+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(4, drivetrain.output->right_voltage / 12.0, kTalonBounds);
+ SetPWMOutput(3, -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);
+ }
+
+ shooter.output.FetchLatest();
+ if (shooter.output.IsNewerThanMS(kOutputMaxAgeMS)) {
+ SetPWMOutput(1, LinearizeVictor(shooter.output->voltage / 12.0), kVictorBounds);
+ SetPWMOutput(7, shooter.output->intake, kVictorBounds);
+ SetSolenoid(4, shooter.output->push);
+ LOG(DEBUG, "shooter: %lf, intake: %lf, push: %d", shooter.output->voltage, shooter.output->intake, shooter.output->push);
+ } else {
+ DisablePWMOutput(2);
+ DisablePWMOutput(1);
+ LOG(WARNING, "shooter not new enough\n");
+ }
+ // TODO(danielp): Add stuff for intake and shooter.
+ }
+
+ // This linearizes the value from the victor.
+ // Copied form the 2012 code.
+ double LinearizeVictor(double goal_speed) {
+ // These values were derived by putting the robot up on blocks, and driving it
+ // at various speeds. The power required to drive at these speeds was then
+ // recorded and fit with gnuplot.
+ const double deadband_value = 0.082;
+ // If we are outside the range such that the motor is actually moving,
+ // subtract off the constant offset from the deadband. This makes the
+ // function odd and intersect the origin, making the fitting easier to do.
+ if (goal_speed > deadband_value) {
+ goal_speed -= deadband_value;
+ } else if (goal_speed < -deadband_value) {
+ goal_speed += deadband_value;
+ } else {
+ goal_speed = 0.0;
+ }
+ goal_speed = goal_speed / (1.0 - deadband_value);
+
+ double goal_speed2 = goal_speed * goal_speed;
+ double goal_speed3 = goal_speed2 * goal_speed;
+ double goal_speed4 = goal_speed3 * goal_speed;
+ double goal_speed5 = goal_speed4 * goal_speed;
+ double goal_speed6 = goal_speed5 * goal_speed;
+ double goal_speed7 = goal_speed6 * goal_speed;
+
+ // Constants for the 5th order polynomial
+ double victor_fit_e1 = 0.437239;
+ double victor_fit_c1 = -1.56847;
+ double victor_fit_a1 = (- (125.0 * victor_fit_e1 + 125.0
+ * victor_fit_c1 - 116.0) / 125.0);
+ double answer_5th_order = (victor_fit_a1 * goal_speed5
+ + victor_fit_c1 * goal_speed3
+ + victor_fit_e1 * goal_speed);
+
+ // Constants for the 7th order polynomial
+ double victor_fit_c2 = -5.46889;
+ double victor_fit_e2 = 2.24214;
+ double victor_fit_g2 = -0.042375;
+ double victor_fit_a2 = (- (125.0 * (victor_fit_c2 + victor_fit_e2
+ + victor_fit_g2) - 116.0) / 125.0);
+ double answer_7th_order = (victor_fit_a2 * goal_speed7
+ + victor_fit_c2 * goal_speed5
+ + victor_fit_e2 * goal_speed3
+ + victor_fit_g2 * goal_speed);
+
+
+ // Average the 5th and 7th order polynomials, and add a bit of linear power in
+ // as well. The average turns out to nicely fit the data in gnuplot with nice
+ // smooth curves, and the linear power gives it a bit more punch at low speeds
+ // again. Stupid victors.
+ double answer = 0.85 * 0.5 * (answer_7th_order + answer_5th_order)
+ + .15 * goal_speed * (1.0 - deadband_value);
+
+ // Add the deadband power back in to make it so that the motor starts moving
+ // when any power is applied. This is what the fitting assumes.
+ if (answer > 0.001) {
+ answer += deadband_value;
+ } else if (answer < -0.001) {
+ answer -= deadband_value;
+ }
+
+ return answer;
+ }
+
+};
+
+} // 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..12a128c
--- /dev/null
+++ b/bot3/output/output.gyp
@@ -0,0 +1,20 @@
+{
+ 'targets': [
+ {
+ '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',
+ '<(DEPTH)/bot3/control_loops/shooter/shooter.gyp:shooter_loop',
+ '<(AOS)/common/common.gyp:controls',
+ '<(DEPTH)/frc971/queues/queues.gyp:queues',
+ ],
+ },
+ ],
+}
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/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 754ba62..9a8cac8 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -17,7 +17,7 @@
if namespaces:
self._namespaces = namespaces
else:
- self._namespaces = ['frc971', 'control_loops']
+ self._namespaces = ['bot3', 'control_loops']
self._namespace_start = '\n'.join(
['namespace %s {' % name for name in self._namespaces])
@@ -26,7 +26,7 @@
['} // namespace %s' % name for name in reversed(self._namespaces)])
def _HeaderGuard(self, header_file):
- return ('FRC971_CONTROL_LOOPS_' +
+ return ('BOT3_CONTROL_LOOPS_' +
header_file.upper().replace('.', '_').replace('/', '_') +
'_')
@@ -89,7 +89,7 @@
def WriteCC(self, header_file_name, cc_file):
"""Writes the cc file to the file named cc_file."""
with open(cc_file, 'w') as fd:
- fd.write('#include \"frc971/control_loops/%s\"\n' % header_file_name)
+ fd.write('#include \"bot3/control_loops/%s\"\n' % header_file_name)
fd.write('\n')
fd.write('#include <vector>\n')
fd.write('\n')
diff --git a/frc971/control_loops/python/shooter.py b/frc971/control_loops/python/shooter.py
index 83beb90..27ecc16 100755
--- a/frc971/control_loops/python/shooter.py
+++ b/frc971/control_loops/python/shooter.py
@@ -4,51 +4,57 @@
import sys
from matplotlib import pylab
import control_loop
+import slycot
class Shooter(control_loop.ControlLoop):
def __init__(self):
super(Shooter, self).__init__("Shooter")
# Stall Torque in N m
- self.stall_torque = 0.49819248
+ self.stall_torque = 2.42211227883219
# Stall Current in Amps
- self.stall_current = 85
+ self.stall_current = 133
# Free Speed in RPM
- self.free_speed = 19300.0 - 1500.0
+ self.free_speed = 4650.0
# Free Current in Amps
- self.free_current = 1.4
+ self.free_current = 2.7
# Moment of inertia of the shooter wheel in kg m^2
self.J = 0.0032
# Resistance of the motor, divided by 2 to account for the 2 motors
- self.R = 12.0 / self.stall_current / 2
+ self.R = 12.0 / self.stall_current
# Motor velocity constant
self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
(12.0 - self.R * self.free_current))
# Torque constant
self.Kt = self.stall_torque / self.stall_current
# Gear ratio
- self.G = 11.0 / 34.0
+ self.G = 40.0 / 34.0
# Control loop time step
self.dt = 0.01
# State feedback matrices
self.A_continuous = numpy.matrix(
- [[0, 1],
- [0, -self.Kt / self.Kv / (self.J * self.G * self.G * self.R)]])
+ [[-self.Kt / self.Kv / (self.J * self.G * self.G * self.R)]])
self.B_continuous = numpy.matrix(
- [[0],
- [self.Kt / (self.J * self.G * self.R)]])
- self.C = numpy.matrix([[1, 0]])
+ [[self.Kt / (self.J * self.G * self.R)]])
+ self.C = numpy.matrix([[1]])
self.D = numpy.matrix([[0]])
- self.ContinuousToDiscrete(self.A_continuous, self.B_continuous,
- self.dt, self.C)
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous, self.B_continuous,
+ self.dt)
- self.PlaceControllerPoles([.6, .981])
+ self.InitializeState()
- self.rpl = .45
- self.ipl = 0.07
- self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
- self.rpl - 1j * self.ipl])
+ self.PlaceControllerPoles([.8])
+ # LQR stuff for optimization, if needed.
+ #print self.K
+ #self.R_LQR = numpy.matrix([[1.5]])
+ #self.P = slycot.sb02od(1, 1, self.A, self.B, self.C * self.C.T, self.R, 'D')[0]
+ #self.K = (numpy.linalg.inv(self.R_LQR + self.B.T * self.P * self.B)
+ # * self.B.T * self.P * self.A)
+ #print numpy.linalg.eig(self.A - self.B * self.K)
+
+
+ self.PlaceObserverPoles([0.45])
self.U_max = numpy.matrix([[12.0]])
self.U_min = numpy.matrix([[-12.0]])
@@ -72,56 +78,47 @@
last_x = shooter_data[i, 2]
sim_delay = 1
- pylab.plot(range(sim_delay, shooter_data.shape[0] + sim_delay),
- simulated_x, label='Simulation')
- pylab.plot(range(shooter_data.shape[0]), real_x, label='Reality')
- pylab.plot(range(shooter_data.shape[0]), x_vel, label='Velocity')
- pylab.legend()
- pylab.show()
+# pylab.plot(range(sim_delay, shooter_data.shape[0] + sim_delay),
+# simulated_x, label='Simulation')
+# pylab.plot(range(shooter_data.shape[0]), real_x, label='Reality')
+# pylab.plot(range(shooter_data.shape[0]), x_vel, label='Velocity')
+# pylab.legend()
+# pylab.show()
# Simulate the closed loop response of the system to a step input.
shooter = Shooter()
close_loop_x = []
close_loop_U = []
- velocity_goal = 300
- R = numpy.matrix([[0.0], [velocity_goal]])
- for _ in pylab.linspace(0,1.99,200):
+ velocity_goal = 400
+ R = numpy.matrix([[velocity_goal]])
+ goal = False
+ for i in pylab.linspace(0,1.99,200):
# Iterate the position up.
- R = numpy.matrix([[R[0, 0] + 10.5], [velocity_goal]])
- # Prevents the position goal from going beyond what is necessary.
- velocity_weight_scalar = 0.35
- max_reference = (
- (shooter.U_max[0, 0] - velocity_weight_scalar *
- (velocity_goal - shooter.X_hat[1, 0]) * shooter.K[0, 1]) /
- shooter.K[0, 0] +
- shooter.X_hat[0, 0])
- min_reference = (
- (shooter.U_min[0, 0] - velocity_weight_scalar *
- (velocity_goal - shooter.X_hat[1, 0]) * shooter.K[0, 1]) /
- shooter.K[0, 0] +
- shooter.X_hat[0, 0])
- R[0, 0] = numpy.clip(R[0, 0], min_reference, max_reference)
- U = numpy.clip(shooter.K * (R - shooter.X_hat),
+ R = numpy.matrix([[velocity_goal]])
+ U = numpy.clip(shooter.K * (R - shooter.X_hat) +
+ (numpy.identity(shooter.A.shape[0]) - shooter.A) * R / shooter.B,
shooter.U_min, shooter.U_max)
shooter.UpdateObserver(U)
shooter.Update(U)
- close_loop_x.append(shooter.X[1, 0])
+ close_loop_x.append(shooter.X[0, 0])
close_loop_U.append(U[0, 0])
+ if (abs(R[0, 0] - shooter.X[0, 0]) < R[0, 0]* 0.01 and (not goal)):
+ goal = True
+ print i
#pylab.plotfile("shooter.csv", (0,1))
- #pylab.plot(pylab.linspace(0,1.99,200), close_loop_U, 'ro')
+ pylab.plot(pylab.linspace(0,1.99,200), close_loop_U)
#pylab.plotfile("shooter.csv", (0,2))
- pylab.plot(pylab.linspace(0,1.99,200), close_loop_x, 'ro')
+ pylab.plot(pylab.linspace(0,1.99,200), close_loop_x)
pylab.show()
# Simulate spin down.
spin_down_x = [];
- R = numpy.matrix([[50.0], [0.0]])
for _ in xrange(150):
U = 0
shooter.UpdateObserver(U)
shooter.Update(U)
- spin_down_x.append(shooter.X[1, 0])
+ spin_down_x.append(shooter.X[0, 0])
#pylab.plot(range(150), spin_down_x)
#pylab.show()
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 38ca89c..034ee9e 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -288,11 +288,15 @@
// update_observer is whether or not to use the values in Y.
// stop_motors is whether or not to output all 0s.
- void Update(bool update_observer, bool stop_motors) {
+ // U_add is for the bot3 shooter velocity control, where a constant
+ // must be added to get a good U.
+ void Update(bool update_observer, bool stop_motors,
+ Eigen::Matrix<double, number_of_inputs, 1> U_add =
+ Eigen::Matrix<double, number_of_inputs, 1>::Zero()) {
if (stop_motors) {
U.setZero();
} else {
- U = U_uncapped = K() * (R - X_hat);
+ U = U_uncapped = K() * (R - X_hat) + U_add;
CapU();
}
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 8d5ad90..b0bcb20 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 975084a..0f54fe2 100644
--- a/gyro_board/src/usb/data_struct.h
+++ b/gyro_board/src/usb/data_struct.h
@@ -129,6 +129,7 @@
};
uint16_t booleans;
};
+ uint32_t shooter_cycle_ticks;
} bot3;
};
};
diff --git a/gyro_board/src/usb/digital.c b/gyro_board/src/usb/digital.c
index c468173..83ef169 100644
--- a/gyro_board/src/usb/digital.c
+++ b/gyro_board/src/usb/digital.c
@@ -30,5 +30,9 @@
int is_bot3;
void digital_init(void) {
- is_bot3 = 0;
+ if (dip_switch(0) || dip_switch(1) || dip_switch(2) || dip_switch(3)) {
+ is_bot3 = 1;
+ } else {
+ is_bot3 = 0;
+ }
}
diff --git a/gyro_board/src/usb/encoder.c b/gyro_board/src/usb/encoder.c
index c9155d4..c735126 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"
@@ -75,6 +77,38 @@
}
}
+static inline void reset_TC(void) {
+ TIM2->TCR |= (1 << 1); // Put it into reset.
+ 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) {}
@@ -288,30 +322,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
@@ -340,12 +369,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) {
@@ -394,20 +442,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.
@@ -427,32 +462,67 @@
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);
+ // Rate of clock signal is just CCLK.
+ SC->PCLKSEL1 |= (1 << 12);
+ // 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 * (10 ^ 8);
+ 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,
@@ -502,12 +572,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;
NVIC_DisableIRQ(EINT3_IRQn);