copied everything over from 2012 and removed all of the actual robot code except the drivetrain stuff
git-svn-id: https://robotics.mvla.net/svn/frc971/2013/trunk/src@4078 f308d9b7-e957-4cde-b6ac-9a88185e7312
diff --git a/frc971/atom_code/atom_code.gyp b/frc971/atom_code/atom_code.gyp
new file mode 100644
index 0000000..d727036
--- /dev/null
+++ b/frc971/atom_code/atom_code.gyp
@@ -0,0 +1,28 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'All',
+ 'type': 'none',
+ 'dependencies': [
+ '<(AOS)/build/aos_all.gyp:Atom',
+ '../control_loops/control_loops.gyp:DriveTrain',
+ '../input/input.gyp:JoystickReader',
+ '../input/input.gyp:SensorReader',
+ '../input/input.gyp:GyroReader',
+ '../input/input.gyp:AutoMode',
+ '../output/output.gyp:MotorWriter',
+ '../output/output.gyp:CameraServer',
+ 'camera/camera.gyp:frc971',
+ ],
+ 'copies': [
+ {
+ 'destination': '<(rsync_dir)',
+ 'files': [
+ 'scripts/aos_module.ko',
+ 'scripts/start_list.txt',
+ ],
+ },
+ ],
+ },
+ ],
+}
diff --git a/frc971/atom_code/build.sh b/frc971/atom_code/build.sh
new file mode 100755
index 0000000..d1dff19
--- /dev/null
+++ b/frc971/atom_code/build.sh
@@ -0,0 +1,3 @@
+#!/bin/bash
+
+../../aos/build/build.sh atom atom_code.gyp no $1
diff --git a/frc971/atom_code/camera/camera.gyp b/frc971/atom_code/camera/camera.gyp
new file mode 100644
index 0000000..6dc9530
--- /dev/null
+++ b/frc971/atom_code/camera/camera.gyp
@@ -0,0 +1,30 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'frc971_camera',
+ 'variables': {
+ 'srcdirs': ['.'],
+ 'manifest': 'frc971_camera.mf',
+ },
+ 'dependencies': [
+ '<(AOS)/atom_code/camera/camera.gyp:aos_camera',
+ '<(DEPTH)/frc971/queues/queues.gyp:frc971_queues_so',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/atom_code/camera/camera.gyp:aos_camera',
+ '<(DEPTH)/frc971/queues/queues.gyp:frc971_queues_so',
+ ],
+ 'includes': ['../../../aos/build/java.gypi'],
+ },
+ {
+ 'target_name': 'frc971',
+ 'variables': {
+ 'main_jar': 'frc971_camera',
+ },
+ 'dependencies': [
+ 'frc971_camera',
+ ],
+ 'includes': ['../../../aos/build/onejar.gypi'],
+ },
+ ],
+}
diff --git a/frc971/atom_code/camera/frc971_camera.mf b/frc971/atom_code/camera/frc971_camera.mf
new file mode 100644
index 0000000..e3929a2
--- /dev/null
+++ b/frc971/atom_code/camera/frc971_camera.mf
@@ -0,0 +1,2 @@
+Main-Class: org.spartanrobotics.camera.Test
+
diff --git a/frc971/atom_code/camera/org/spartanrobotics/camera/QueueReader.java b/frc971/atom_code/camera/org/spartanrobotics/camera/QueueReader.java
new file mode 100644
index 0000000..347b0c2
--- /dev/null
+++ b/frc971/atom_code/camera/org/spartanrobotics/camera/QueueReader.java
@@ -0,0 +1,16 @@
+package org.spartanrobotics.camera;
+
+import aos.NativeLoader;
+import aos.QueueLogHandler;
+import frc971.control_loops.Piston_q;
+
+public class QueueReader {
+
+ public static void main(String[] args) {
+ QueueLogHandler.UseForAll();
+ NativeLoader.load("frc971_queues_so");
+ System.out.println(Piston_q.shifters.FetchLatest());
+ System.out.println(Piston_q.shifters.getSet());
+ }
+
+}
diff --git a/frc971/atom_code/camera/org/spartanrobotics/camera/QueueTest.java b/frc971/atom_code/camera/org/spartanrobotics/camera/QueueTest.java
new file mode 100644
index 0000000..a71f447
--- /dev/null
+++ b/frc971/atom_code/camera/org/spartanrobotics/camera/QueueTest.java
@@ -0,0 +1,15 @@
+package org.spartanrobotics.camera;
+
+import aos.NativeLoader;
+import aos.QueueLogHandler;
+import frc971.control_loops.Piston_q;
+
+public class QueueTest {
+
+ public static void main(String[] args) {
+ QueueLogHandler.UseForAll();
+ NativeLoader.load("frc971_queues_so");
+ Piston_q.shifters.SafeMakeWithBuilder().set(false).Send();
+ }
+
+}
diff --git a/frc971/atom_code/camera/org/spartanrobotics/camera/Test.java b/frc971/atom_code/camera/org/spartanrobotics/camera/Test.java
new file mode 100644
index 0000000..86c06bc
--- /dev/null
+++ b/frc971/atom_code/camera/org/spartanrobotics/camera/Test.java
@@ -0,0 +1,44 @@
+package org.spartanrobotics.camera;
+
+import java.io.IOException;
+import com.googlecode.javacv.cpp.opencv_core;
+import com.googlecode.javacv.cpp.opencv_core.CvPoint;
+import com.googlecode.javacv.cpp.opencv_core.CvScalar;
+import com.googlecode.javacv.cpp.opencv_core.IplImage;
+import com.googlecode.javacv.cpp.opencv_imgproc;
+
+import aos.CameraProcessor;
+import aos.DebugServer;
+import aos.ImageGetter;
+import aos.ServableImage;
+import aos.Thresholder;
+
+public class Test extends CameraProcessor {
+ private final DebugServer server = new DebugServer(9719);
+ private final IplImage hsv = IplImage.create(ImageGetter.width, ImageGetter.height, opencv_core.IPL_DEPTH_8U, 3);
+ private final ServableImage thresholded = new ServableImage(ImageGetter.width, ImageGetter.height, opencv_core.IPL_DEPTH_8U, 1);
+
+ private Test(String[] args) throws IOException {
+ super(args);
+
+ server.addImage("/start", start);
+ server.addImage("/thresholded", thresholded, new DebugServer.Palette().add(0, 0, 0, 0).add(0, 0xFF, 0, 0xFF).add(0, 0, 0xFF, 0xFF));
+ }
+
+ @Override
+ protected void RunIteration() {
+ server.setTimestamp(getter.getTimestamp());
+
+ opencv_imgproc.cvCvtColor(start.getImage(), hsv, opencv_imgproc.CV_RGB2HSV);
+ Thresholder.threshold(hsv, thresholded.getImage(), 0, 100, 160, 0, 255, 0, 255);
+ thresholded.recordSnapshot(1);
+ if (thresholded.isDebugging()) {
+ opencv_core.cvCircle(thresholded.getImage(), new CvPoint(200, 200), 50, new CvScalar(2, 2, 2, 2), 5, 8, 0);
+ }
+ thresholded.releaseImage();
+ }
+
+ public static void main(String[] args) throws IOException {
+ new Test(args).Run();
+ }
+}
diff --git a/frc971/atom_code/scripts/aos_module.ko b/frc971/atom_code/scripts/aos_module.ko
new file mode 100644
index 0000000..cdaddb7
--- /dev/null
+++ b/frc971/atom_code/scripts/aos_module.ko
Binary files differ
diff --git a/frc971/atom_code/scripts/start_list.txt b/frc971/atom_code/scripts/start_list.txt
new file mode 100644
index 0000000..6bbcd70
--- /dev/null
+++ b/frc971/atom_code/scripts/start_list.txt
@@ -0,0 +1,15 @@
+MotorWriter
+JoystickReader
+SensorReader
+GyroReader
+DriveTrain
+WristMotor
+IntakeRollers
+HorizontalRoller
+VerticalRoller
+AutoMode
+BinaryLogReader
+CRIOLogReader
+CameraReader
+CameraServer
+CameraHTTPStreamer
diff --git a/frc971/constants.cpp b/frc971/constants.cpp
new file mode 100644
index 0000000..6c16097
--- /dev/null
+++ b/frc971/constants.cpp
@@ -0,0 +1,75 @@
+#include "frc971/constants.h"
+
+#include <stddef.h>
+#include <inttypes.h>
+
+#include "aos/common/messages/RobotState.q.h"
+#include "aos/atom_code/output/MotorOutput.h"
+
+namespace frc971 {
+namespace constants {
+
+namespace {
+
+const double kCompHorizontal = -1.77635 + 0.180;
+const double kPracticeHorizontal = -1.77635 + -0.073631;
+const int kCompCameraCenter = -2;
+const int kPracticeCameraCenter = -5;
+
+struct Values {
+ // what horizontal_offset returns
+ double horizontal;
+ // what camera_center returns
+ int camera_center;
+ // what drivetrain_motor_controllers returns
+ char drivetrain_controllers;
+};
+Values *values = NULL;
+// Attempts to retrieve a new Values instance and stores it in values if
+// necessary.
+// Returns a valid Values instance or NULL.
+const Values *GetValues() {
+ if (values == NULL) {
+ LOG(INFO, "creating a Constants for team %"PRIu16"\n",
+ aos::robot_state->team_id);
+ switch (aos::robot_state->team_id) {
+ case kCompTeamNumber:
+ values = new Values{kCompHorizontal, kCompCameraCenter,
+ aos::MotorOutput::TALON};
+ break;
+ case kPracticeTeamNumber:
+ values = new Values{kPracticeHorizontal, kPracticeCameraCenter,
+ aos::MotorOutput::VICTOR};
+ break;
+ default:
+ LOG(ERROR, "unknown team #%"PRIu16"\n",
+ aos::robot_state->team_id);
+ return NULL;
+ }
+ }
+ return values;
+}
+
+} // namespace
+
+bool horizontal_offset(double *horizontal) {
+ const Values *const values = GetValues();
+ if (values == NULL) return false;
+ *horizontal = values->horizontal;
+ return true;
+}
+bool camera_center(int *center) {
+ const Values *const values = GetValues();
+ if (values == NULL) return false;
+ *center = values->camera_center;
+ return true;
+}
+bool drivetrain_motor_controllers(char *controllers) {
+ const Values *const values = GetValues();
+ if (values == NULL) return false;
+ *controllers = values->drivetrain_controllers;
+ return true;
+}
+
+} // namespace constants
+} // namespace frc971
diff --git a/frc971/constants.h b/frc971/constants.h
new file mode 100644
index 0000000..ab360e0
--- /dev/null
+++ b/frc971/constants.h
@@ -0,0 +1,29 @@
+#include <stdint.h>
+
+namespace frc971 {
+namespace constants {
+
+// Has all of the numbers that change for both robots and makes it easy to
+// retrieve the values for the current one.
+//
+// All of the public functions to retrieve various values take a pointer to
+// store their output value into and assume that aos::robot_state->get() is
+// not null and is correct. They return true on success.
+
+const uint16_t kCompTeamNumber = 971;
+const uint16_t kPracticeTeamNumber = 5971;
+
+// Sets *horizontal to how many radians from the hall effect transition point
+// to horizontal for the wrist.
+bool horizontal_offset(double *horizontal);
+// Sets *center to how many pixels off center the vertical line
+// on the camera view is.
+bool camera_center(int *center);
+// Sets *talons to whether or not the drivetrain is using talons (otherwise it's
+// using victors).
+// Sets *controllers to what type of motor controllers to use for the drivetrain
+// (one of MotorOutput's constants).
+bool drivetrain_motor_controllers(char *controllers);
+
+} // namespace constants
+} // namespace frc971
diff --git a/frc971/control_loops/DriveTrain.cc b/frc971/control_loops/DriveTrain.cc
new file mode 100644
index 0000000..b627cc0
--- /dev/null
+++ b/frc971/control_loops/DriveTrain.cc
@@ -0,0 +1,298 @@
+#include "frc971/control_loops/DriveTrain.h"
+
+#include <stdio.h>
+#include <sched.h>
+#include <cmath>
+
+#include "aos/aos_core.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/queue.h"
+#include "frc971/control_loops/DriveTrain.mat"
+#include "frc971/control_loops/DriveTrain.q.h"
+#include "frc971/queues/GyroAngle.q.h"
+#include "frc971/queues/Piston.q.h"
+
+using frc971::sensors::gyro;
+
+namespace frc971 {
+namespace control_loops {
+
+// Width of the robot.
+const double width = 22.0 / 100.0 * 2.54;
+
+class DrivetrainMotorsSS : public MatrixClass {
+ public:
+ DrivetrainMotorsSS (void) {
+ MATRIX_INIT;
+ _offset = 0;
+ _integral_offset = 0;
+ _left_goal = 0.0;
+ _right_goal = 0.0;
+ _raw_left = 0.0;
+ _raw_right = 0.0;
+ }
+ void SetGoal(double left, double left_velocity, double right, double right_velocity) {
+ _left_goal = left;
+ _right_goal = right;
+ R << left + _integral_offset * width / 2.0, left_velocity, right - _integral_offset * width / 2.0, right_velocity;
+ }
+ void SetRawPosition(double left, double right) {
+ _raw_right = right;
+ _raw_left = left;
+ Y << left + _offset + _integral_offset, right - _offset + _integral_offset;
+ }
+ 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;
+ if (!control_loop_driving) {
+ _integral_offset = 0.0;
+ } else if (std::abs(angle_error) < M_PI / 10.0) {
+ _integral_offset -= angle_error * 0.010;
+ } else {
+ _integral_offset *= 0.97;
+ }
+ _gyro = gyro;
+ 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);
+ }
+ double UnDeadband(double value) {
+ const double positive_deadband_power = 0.15 * 12;
+ const double negative_deadband_power = 0.09 * 12;
+ if (value > 0) {
+ value += positive_deadband_power;
+ }
+ if (value < 0) {
+ value -= negative_deadband_power;
+ }
+ if (value > 12.0) {
+ value = 12.0;
+ }
+ if (value < -12.0) {
+ value = -12.0;
+ }
+ return value;
+ }
+
+ void SendMotors(Drivetrain::Output *status) {
+ status->left_voltage = UnDeadband(U[0]);
+ status->right_voltage = UnDeadband(U[1]);
+ }
+ 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);
+ LOG(DEBUG, "lg %f rg %f le %f re %f gyro %f off %f\n", R[0], R[2], Y[0], Y[1], _gyro * 180.0 / M_PI, _offset);
+ }
+
+ private:
+ double _integral_offset;
+ double _offset;
+ double _gyro;
+ double _left_goal;
+ double _right_goal;
+ double _raw_left;
+ double _raw_right;
+};
+
+class DrivetrainMotorsOL {
+ public:
+ DrivetrainMotorsOL() {
+ _old_wheel = 0.0;
+ quick_stop_accumulator = 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.7; // 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.4; // 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);
+ }
+
+ double neg_inertia_scalar;
+ if (_highgear) {
+ neg_inertia_scalar = 20.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 = 16; // used to be csvReader->NEG_INERTIA_LOW_MORE
+ } else {
+ if (fabs(wheel) > 0.65) {
+ neg_inertia_scalar = 16; // 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
+
+ if (fabs(_throttle) > 0.1) { // used to be csvReader->SENSE_CUTTOFF
+ sensitivity = 1 - (1 - sensitivity) / fabs(_throttle);
+ }
+ }
+ 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;
+
+ const double quickstop_scalar = 6;
+ if (_quickturn) {
+ double qt_angular_power = wheel;
+ const double alpha = 0.1;
+ if (fabs(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;
+ }
+ quick_stop_accumulator = (1 - alpha) * quick_stop_accumulator + alpha * qt_angular_power * quickstop_scalar;
+ overPower = 1.0;
+ if (_highgear) {
+ sensitivity = 1.0;
+ } else {
+ sensitivity = 1.0;
+ }
+ angular_power = wheel;
+ } else {
+ overPower = 0.0;
+ angular_power = fabs(_throttle) * wheel * sensitivity;
+ angular_power -= quick_stop_accumulator;
+ if (quick_stop_accumulator > 1) {
+ quick_stop_accumulator -= 1;
+ } else if (quick_stop_accumulator < -1) {
+ quick_stop_accumulator += 1;
+ } else {
+ quick_stop_accumulator = 0;
+ }
+ }
+
+ _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, qa %f\n",
+ _left_pwm, _right_pwm, _wheel, _throttle, quick_stop_accumulator);
+ 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;
+ double quick_stop_accumulator;
+};
+
+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 pos\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, 0.0, right_goal, 0.0);
+ 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(!bad_pos, bad_pos || (output == NULL));
+ 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 frc971
+
+AOS_RUN_LOOP(frc971::control_loops::DrivetrainLoop)
diff --git a/frc971/control_loops/DriveTrain.h b/frc971/control_loops/DriveTrain.h
new file mode 100644
index 0000000..61ea524
--- /dev/null
+++ b/frc971/control_loops/DriveTrain.h
@@ -0,0 +1,32 @@
+#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_H_
+#define FRC971_CONTROL_LOOPS_DRIVETRAIN_H_
+
+#include "aos/common/control_loop/ControlLoop.h"
+#include "frc971/control_loops/DriveTrain.q.h"
+
+namespace frc971 {
+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 frc971
+
+#endif // FRC971_CONTROL_LOOPS_DRIVETRAIN_H_
diff --git a/frc971/control_loops/DriveTrain.mat b/frc971/control_loops/DriveTrain.mat
new file mode 100644
index 0000000..fcc6f45
--- /dev/null
+++ b/frc971/control_loops/DriveTrain.mat
@@ -0,0 +1,12 @@
+#include "frc971/control_loops/StateFeedbackLoop.h"
+
+typedef StateFeedbackLoop<4, 2> MatrixClass;
+#define MATRIX_INIT A << 1.0000000000, 0.0095410093, 0.0000000000, -0.0000167223, 0.0000000000, 0.9096302600, 0.0000000000, -0.0032396985, 0.0000000000, -0.0000167223, 1.0000000000, 0.0095410093, 0.0000000000, -0.0032396985, 0.0000000000, 0.9096302600; \
+B << 0.0000628338, 0.0000022892, 0.0123712263, 0.0004435007, 0.0000022892, 0.0000628338, 0.0004435007, 0.0123712263; \
+C << 1.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000, 0.0000000000; \
+D << 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000; \
+L << 1.7496302600, -0.0032396985, 72.1296388532, -0.4369906587, -0.0032396985, 1.7496302600, -0.4369906586, 72.1296388532; \
+K << 242.8102455120, 19.7898401032, -8.7045950610, -0.9720464423, -8.7045950610, -0.9720464423, 242.8102455120, 19.7898401032; \
+U_max << 12.0000000000, 12.0000000000; \
+U_min << -12.0000000000, -12.0000000000; \
+
diff --git a/frc971/control_loops/DriveTrain.q b/frc971/control_loops/DriveTrain.q
new file mode 100644
index 0000000..f56c115
--- /dev/null
+++ b/frc971/control_loops/DriveTrain.q
@@ -0,0 +1,38 @@
+package frc971.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 right_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/frc971/control_loops/StateFeedbackLoop.h b/frc971/control_loops/StateFeedbackLoop.h
new file mode 100644
index 0000000..ad37f49
--- /dev/null
+++ b/frc971/control_loops/StateFeedbackLoop.h
@@ -0,0 +1,131 @@
+#ifndef FRC971_CONTROL_LOOPS_STATEFEEDBACKLOOP_H_
+#define FRC971_CONTROL_LOOPS_STATEFEEDBACKLOOP_H_
+
+// wikipedia article is <http://en.wikipedia.org/wiki/State_observer>
+
+#include "Eigen/Dense"
+
+template <int number_of_states, int number_of_outputs>
+class StateFeedbackPlant {
+ public:
+ Eigen::Matrix<double, number_of_states, 1> X;
+ Eigen::Matrix<double, number_of_outputs, 1> Y;
+ Eigen::Matrix<double, number_of_outputs, 1> U;
+ Eigen::Matrix<double, number_of_outputs, 1> U_max;
+ Eigen::Matrix<double, number_of_outputs, 1> U_min;
+ Eigen::Matrix<double, number_of_states, number_of_states> A;
+ Eigen::Matrix<double, number_of_states, number_of_outputs> B;
+ Eigen::Matrix<double, number_of_outputs, number_of_states> C;
+ Eigen::Matrix<double, number_of_outputs, number_of_outputs> D;
+ // TODO(aschuh): These following 2 lines are here because MATRIX_INIT
+ // assumes that you have a controller as well as a plant.
+ Eigen::Matrix<double, number_of_states, number_of_outputs> L;
+ Eigen::Matrix<double, number_of_outputs, number_of_states> K;
+
+ StateFeedbackPlant() {
+ X.setZero();
+ Y.setZero();
+ U.setZero();
+ }
+
+ virtual ~StateFeedbackPlant() {}
+
+ // If U is outside the hardware range, limit it before the plant tries to use
+ // it.
+ virtual void CapU() {
+ for (int i = 0; i < number_of_outputs; ++i) {
+ if (U[i] > U_max[i]) {
+ U[i] = U_max[i];
+ } else if (U[i] < U_min[i]) {
+ U[i] = U_min[i];
+ }
+ }
+ }
+ // Computes the new X and Y given the control input.
+ void Update() {
+ CapU();
+ X = A * X + B * U;
+ Y = C * X + D * U;
+ }
+
+ protected:
+ // these are accessible from non-templated subclasses
+ static const int number_of_states_var = number_of_states;
+ static const int number_of_outputs_var = number_of_outputs;
+};
+
+template <int number_of_states, int number_of_outputs>
+class StateFeedbackLoop {
+ public:
+ Eigen::Matrix<double, number_of_states, 1> X;
+ Eigen::Matrix<double, number_of_states, 1> X_hat;
+ Eigen::Matrix<double, number_of_outputs, 1> Y;
+ Eigen::Matrix<double, number_of_states, 1> R;
+ Eigen::Matrix<double, number_of_outputs, 1> U;
+ Eigen::Matrix<double, number_of_outputs, 1> U_max;
+ Eigen::Matrix<double, number_of_outputs, 1> U_min;
+ Eigen::Matrix<double, number_of_outputs, 1> U_ff;
+ Eigen::Matrix<double, number_of_states, number_of_states> A;
+ Eigen::Matrix<double, number_of_states, number_of_outputs> B;
+ // K in wikipedia article
+ Eigen::Matrix<double, number_of_outputs, number_of_states> C;
+ Eigen::Matrix<double, number_of_outputs, number_of_outputs> D;
+ // B in wikipedia article
+ Eigen::Matrix<double, number_of_states, number_of_outputs> L;
+ // C in wikipedia article
+ Eigen::Matrix<double, number_of_outputs, number_of_states> K;
+
+ StateFeedbackLoop() {
+ // You have to initialize all the matrices to 0 or else all their elements
+ // are undefined.
+ X.setZero();
+ X_hat.setZero();
+ Y.setZero();
+ R.setZero();
+ U.setZero();
+ U_ff.setZero();
+ }
+ virtual ~StateFeedbackLoop() {}
+
+ virtual void FeedForward() {
+ for (int i = 0; i < number_of_outputs; ++i) {
+ U_ff[i] = 0.0;
+ }
+ }
+ // If U is outside the hardware range, limit it before it
+ // gets added to the observer.
+ virtual void CapU() {
+ for (int i = 0; i < number_of_outputs; ++i) {
+ if (U[i] > U_max[i]) {
+ U[i] = U_max[i];
+ } else if (U[i] < U_min[i]) {
+ U[i] = U_min[i];
+ }
+ }
+ }
+ // 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) {
+ if (stop_motors) {
+ for (int i = 0; i < number_of_outputs; ++i) {
+ U[i] = 0.0;
+ }
+ } else {
+ // new power = constant * (goal - current prediction)
+ U.noalias() = K * (R - X_hat);
+ CapU();
+ }
+
+ if (update_observer) {
+ X_hat = (A - L * C) * X_hat + L * Y + B * U;
+ } else {
+ X_hat = A * X_hat + B * U;
+ }
+ }
+
+ protected:
+ // these are accessible from non-templated subclasses
+ static const int number_of_states_var = number_of_states;
+ static const int number_of_outputs_var = number_of_outputs;
+};
+#endif // FRC971_CONTROL_LOOPS_STATEFEEDBACKLOOP_H_
diff --git a/frc971/control_loops/control_loops.gyp b/frc971/control_loops/control_loops.gyp
new file mode 100644
index 0000000..a9ddb12
--- /dev/null
+++ b/frc971/control_loops/control_loops.gyp
@@ -0,0 +1,43 @@
+{
+ 'variables': {
+ 'loop_files': [
+ 'DriveTrain.q',
+ ]
+ },
+ 'targets': [
+ {
+ 'target_name': 'control_loops',
+ 'type': 'static_library',
+ 'sources': ['<@(loop_files)'],
+ 'variables': {
+ 'header_path': 'frc971/control_loops',
+ },
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ '<(AOS)/common/common.gyp:control_loop_queues',
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/build/aos.gyp:libaos',
+ '<(AOS)/common/common.gyp:control_loop_queues',
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'includes': ['../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'DriveTrain',
+ 'type': 'executable',
+ 'sources': [
+ 'DriveTrain.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/common.gyp:controls',
+ '<(AOS)/build/aos.gyp:libaos',
+ 'control_loops',
+ '<(DEPTH)/frc971/queues/queues.gyp:queues',
+ '<(EXTERNALS):eigen',
+ ],
+ },
+ ],
+}
diff --git a/frc971/control_loops/matlab/drivetrain_controller.m b/frc971/control_loops/matlab/drivetrain_controller.m
new file mode 100644
index 0000000..11a5a4b
--- /dev/null
+++ b/frc971/control_loops/matlab/drivetrain_controller.m
@@ -0,0 +1,231 @@
+close all;
+load 'drivetrain_spin_low'
+load 'drivetrain_strait_low'
+m = 68;
+rb = 0.617998644 / 2.0;
+J = 7;
+stall_current = 133.0;
+R = 12.0 / stall_current / 4 / 0.43;
+Km = (12.0 - R * 2.7) / (4650.0 / 60.0 * 2.0 * pi);
+Kt = 0.008;
+r = 0.04445; % 3.5 inches diameter
+G_low = 60.0 / 15.0 * 50.0 / 15.0;
+G_high = 45.0 / 30.0 * 50.0 / 15.0;
+dt = 0.01;
+
+G = G_low;
+
+msp = (1.0 / m + rb ^ 2.0 / J);
+msn = (1.0 / m - rb ^ 2.0 / J);
+tc = -Km * Kt * G ^ 2.0 / (R * r ^ 2.0);
+mp = G * Kt / (R * r);
+
+A = [0 1 0 0; 0 msp*tc 0 msn*tc; 0 0 0 1; 0 msn*tc 0 msp*tc];
+B = [0 0; msp * mp msn * mp; 0 0; msn * mp msp * mp];
+C = [1 0 0 0; 0 0 1 0];
+D = [0 0; 0 0];
+
+dm = c2d(ss(A, B, C, D), dt);
+
+hp = .8;
+lp = .85;
+K = place(dm.a, dm.b, [hp, hp, lp, lp]);
+
+hlp = 0.07;
+llp = 0.09;
+L = place(dm.a', dm.c', [hlp, hlp, llp, llp])';
+
+% Plot what we computed
+
+fd = fopen('/home/aschuh/frc971/2012/trunk/src/atom_code/control_loops/Drivetrain.mat', 'w');
+n = 1;
+sm = [];
+writeMatHeader(fd, size(dm.a, 1), size(dm.b, 2));
+writeMat(fd, dm.a, 'A');
+writeMat(fd, dm.b, 'B');
+writeMat(fd, dm.c, 'C');
+writeMat(fd, dm.d, 'D');
+writeMat(fd, L, 'L');
+writeMat(fd, K, 'K');
+writeMat(fd, [12; 12], 'U_max');
+writeMat(fd, [-12; -12], 'U_min');
+writeMatFooter(fd);
+fclose(fd);
+
+full_model = dss([dm.a (-dm.b * K); eye(4) (dm.a - dm.b * K - L * dm.c)], [0, 0; 0, 0; 0, 0; 0, 0; L], [C, [0, 0, 0, 0; 0, 0, 0, 0]], 0, eye(8), 0.01);
+
+n = 1;
+sm_strait = [];
+t = drivetrain_strait_low(1, 1) + dt * (n - 1);
+x = [drivetrain_strait_low(1, 2); 0; drivetrain_strait_low(1, 3); 0];
+while t < drivetrain_strait_low(end, 1)
+ sm_strait(n, 1) = t;
+ sm_strait(n, 2) = (x(1,1) + x(3,1)) / 2.0;
+ t = t + dt;
+ x = dm.a * x + dm.b * [drivetrain_strait_low(n, 4); drivetrain_strait_low(n, 5)];
+ n = n + 1;
+end
+
+figure;
+plot(drivetrain_strait_low(:, 1), (drivetrain_strait_low(:, 2) + drivetrain_strait_low(:, 3)) / 2.0, sm_strait(:, 1), sm_strait(:, 2));
+legend('actual', 'sim');
+
+n = 1;
+sm_spin = [];
+t = drivetrain_spin_low(1, 1) + dt * (n - 1);
+x = [drivetrain_spin_low(1, 2); 0; drivetrain_spin_low(1, 3); 0];
+while t < drivetrain_spin_low(end, 1)
+ sm_spin(n, 1) = t;
+ sm_spin(n, 2) = (x(1,1) - x(3,1)) / 2.0;
+ t = t + dt;
+ x = dm.a * x + dm.b * [drivetrain_spin_low(n, 4); drivetrain_spin_low(n, 5)];
+ n = n + 1;
+end
+
+figure;
+plot(drivetrain_spin_low(:, 1), (drivetrain_spin_low(:, 2) - drivetrain_spin_low(:, 3)) / 2.0, sm_spin(:, 1), sm_spin(:, 2));
+legend('actual', 'sim');
+
+%figure;
+%nyquist(full_model);
+
+
+%%
+t = 0;
+x = [0; 0; 0; 0;];
+while t < logging(end, 1)
+ sm(n, 1) = t;
+ sm(n, 2) = x(1,1);
+ sm(n, 3) = x(3,1);
+ t = t + dt;
+ x = dm.a * x + dm.b * [12.0; 12.0];
+ n = n + 1;
+end
+
+figure;
+plot(logging(:, 1), logging(:, 2), sm(:, 1), sm(:, 2));
+legend('actual', 'sim');
+
+%% Simulation of a small turn angle with a large distance to travel
+tf = 2;
+x = [0; 0; 0.1; 0;];
+r = [10; 0; 10; 0];
+
+smt = zeros(tf / dt, 8);
+t = 0;
+xhat = x;
+n = 1;
+% 1 means scale
+% 2 means just limit to 12 volts
+% 3 means preserve the difference in power
+captype = 1;
+while n <= size(smt, 1)
+ smt(n, 1) = t;
+ smt(n, 2) = x(1,1);
+ smt(n, 3) = x(3,1);
+ t = t + dt;
+
+ u = K * (r - xhat);
+ smt(n, 4) = u(1,1);
+ smt(n, 5) = u(2,1);
+
+ if captype == 1
+ if sum(abs(u) > 12.0)
+ % We have a problem!
+ % Check to see if it's a big steering power problem,
+ % or a big drive error.
+ turnPower = (u(1, 1) - u(2, 1));
+ drivePower = (u(1, 1) + u(2, 1));
+ scaleFactor = 12.0 / max(abs(u));
+ smt(n, 8) = 1.0 / scaleFactor;
+ % Only start scaling the turn power up if we are far out of
+ % range.
+ if abs(turnPower) < 0.5 * abs(drivePower)
+ % Turn power is swamped.
+ deltaTurn = turnPower / 2.0 / scaleFactor * 0.5;
+ u(1, 1) = u(1, 1) + deltaTurn;
+ u(2, 1) = u(2, 1) - deltaTurn;
+ scaleFactor = 12.0 / max(abs(u));
+ else
+ if 0.5 * abs(turnPower) > abs(drivePower)
+ % Drive power is swamped.
+ deltaDrive = drivePower / 2.0 / scaleFactor * 0.5;
+ u(1, 1) = u(1, 1) + deltaDrive;
+ u(2, 1) = u(2, 1) + deltaDrive;
+ scaleFactor = 12.0 / max(abs(u));
+ end
+ end
+ u = u * scaleFactor;
+ end
+ else
+ if captype == 2
+ if u(1, 1) > 12.0
+ u(1, 1) = 12.0;
+ end
+ if u(1, 1) < -12.0
+ u(1, 1) = -12.0;
+ end
+ if u(2, 1) > 12.0
+ u(2, 1) = 12.0;
+ end
+ if u(2, 1) < -12.0
+ u(2, 1) = -12.0;
+ end
+ else
+ if captype == 3
+ if u(1, 1) > 12.0
+ u(2, 1) = u(2, 1) - (u(1, 1) - 12.0);
+ else
+ if u(1, 1) < -12.0
+ u(2, 1) = u(2, 1) - (u(1, 1) + 12.0);
+ end
+ end
+ if u(2, 1) > 12.0
+ u(1, 1) = u(1, 1) - (u(2, 1) - 12.0);
+ else
+ if u(2, 1) < -12.0
+ u(1, 1) = u(1, 1) - (u(2, 1) + 12.0);
+ end
+ end
+ if u(1, 1) > 12.0
+ u(1, 1) = 12.0;
+ end
+ if u(1, 1) < -12.0
+ u(1, 1) = -12.0;
+ end
+ if u(2, 1) > 12.0
+ u(2, 1) = 12.0;
+ end
+ if u(2, 1) < -12.0
+ u(2, 1) = -12.0;
+ end
+ end
+ end
+
+ end
+ smt(n, 6) = u(1,1);
+ smt(n, 7) = u(2,1);
+ xhat = dm.a * xhat + dm.b * u + L * (dm.c * x - dm.c * xhat);
+ x = dm.a * x + dm.b * u;
+
+ n = n + 1;
+end
+
+figure;
+subplot(6, 1, 1);
+plot(smt(:, 1), smt(:, 2) + smt(:, 3));
+legend('dist');
+subplot(6, 1, 2);
+plot(smt(:, 1), smt(:, 2) - smt(:, 3));
+legend('angle');
+subplot(3, 1, 2);
+plot(smt(:, 1), smt(:, 4), smt(:, 1), smt(:, 5));
+legend('lu', 'ru');
+subplot(3, 1, 3);
+plot(smt(:, 1), smt(:, 6), smt(:, 1), smt(:, 7));
+legend('lu_{real}', 'ru_{real}');
+
+%figure;
+%plot(smt(:, 1), smt(:, 8))
+%legend('Scale Factor');
+
diff --git a/frc971/control_loops/matlab/drivetrain_spin_fast.csv b/frc971/control_loops/matlab/drivetrain_spin_fast.csv
new file mode 100644
index 0000000..5768209
--- /dev/null
+++ b/frc971/control_loops/matlab/drivetrain_spin_fast.csv
@@ -0,0 +1,215 @@
+12038,8.037056,-7.571709,1.000000
+12039,8.037056,-7.571709,1.000000
+12040,8.037056,-7.571709,1.000000
+12041,8.037254,-7.572700,1.000000
+12042,8.044792,-7.577064,1.000000
+12043,8.053718,-7.583610,1.000000
+12044,8.066016,-7.593330,1.000000
+12045,8.078909,-7.602652,1.000000
+12046,8.094183,-7.613760,1.000000
+12047,8.109655,-7.624670,1.000000
+12048,8.126118,-7.637167,1.000000
+12049,8.141789,-7.650060,1.000000
+12050,8.156070,-7.662556,1.000000
+12051,8.174716,-7.677433,1.000000
+12052,8.190188,-7.690128,1.000000
+12053,8.208635,-7.704608,1.000000
+12054,8.225694,-7.717303,1.000000
+12055,8.242951,-7.729800,1.000000
+12056,8.261200,-7.745073,1.000000
+12057,8.276077,-7.758363,1.000000
+12058,8.289565,-7.771058,1.000000
+12059,8.308012,-7.787720,1.000000
+12060,8.327253,-7.805771,1.000000
+12061,8.345105,-7.819457,1.000000
+12062,8.360379,-7.830565,1.000000
+12063,8.377041,-7.844252,1.000000
+12064,8.396083,-7.860914,1.000000
+12065,8.413539,-7.877378,1.000000
+12066,8.430597,-7.895230,1.000000
+12067,8.445871,-7.910107,1.000000
+12068,8.460153,-7.923397,1.000000
+12069,8.478600,-7.938274,1.000000
+12070,8.497444,-7.954341,1.000000
+12071,8.516288,-7.970408,1.000000
+12072,8.535132,-7.987268,1.000000
+12073,8.549215,-8.002541,1.000000
+12074,8.563299,-8.018410,1.000000
+12075,8.581944,-8.037651,1.000000
+12076,8.605152,-8.057288,1.000000
+12077,8.623203,-8.070578,1.000000
+12078,8.639865,-8.087637,1.000000
+12079,8.658709,-8.105489,1.000000
+12080,8.676958,-8.122945,1.000000
+12087,8.815213,-8.259811,1.000000
+12088,8.835247,-8.279250,1.000000
+12089,8.852702,-8.298094,1.000000
+12090,8.876307,-8.320310,1.000000
+12091,8.897333,-8.338758,1.000000
+12092,8.922921,-8.360577,1.000000
+12093,8.948311,-8.383983,1.000000
+12094,8.968147,-8.403621,1.000000
+12095,8.988974,-8.426035,1.000000
+12096,9.009405,-8.447656,1.000000
+12097,9.035191,-8.474633,1.000000
+12098,9.055027,-8.494865,1.000000
+12099,9.076450,-8.516288,1.000000
+12100,9.101641,-8.540686,1.000000
+12101,9.121675,-8.560323,1.000000
+12102,9.144090,-8.583729,1.000000
+12103,9.171066,-8.609119,1.000000
+12104,9.195663,-8.630542,1.000000
+12105,9.218672,-8.653155,1.000000
+12106,9.240690,-8.671602,1.000000
+12107,9.265881,-8.693421,1.000000
+12108,9.290676,-8.717621,1.000000
+12109,9.315868,-8.741424,1.000000
+12110,9.343043,-8.767210,1.000000
+12111,9.365655,-8.788038,1.000000
+12112,9.392632,-8.813626,1.000000
+12113,9.417427,-8.836635,1.000000
+12114,9.441230,-8.857066,1.000000
+12115,9.468801,-8.881464,1.000000
+12116,9.499150,-8.906656,1.000000
+12117,9.521564,-8.928475,1.000000
+12118,9.551120,-8.953666,1.000000
+12119,9.574724,-8.974692,1.000000
+12120,9.604081,-9.003454,1.000000
+12121,9.631455,-9.030629,1.000000
+12122,9.656646,-9.054829,1.000000
+12123,9.686400,-9.084979,1.000000
+12124,9.711789,-9.111559,1.000000
+12125,9.737576,-9.136552,1.000000
+12126,9.761776,-9.158570,1.000000
+12127,9.788554,-9.182770,1.000000
+12128,9.815927,-9.208754,1.000000
+12129,9.845681,-9.235334,1.000000
+12130,9.872261,-9.259534,1.000000
+12131,9.896460,-9.282543,1.000000
+12132,9.924627,-9.308528,1.000000
+12133,9.954976,-9.336893,1.000000
+12134,9.982746,-9.362283,1.000000
+12135,10.007739,-9.385293,1.000000
+12136,10.038881,-9.414055,1.000000
+12137,10.065064,-9.437262,1.000000
+12138,10.097595,-9.464437,1.000000
+12139,10.123977,-9.488042,1.000000
+12140,10.155912,-9.517201,1.000000
+12141,10.180905,-9.539813,1.000000
+12142,10.213039,-9.568377,1.000000
+12143,10.238429,-9.592973,1.000000
+12144,10.270166,-9.622529,1.000000
+12145,10.296548,-9.646728,1.000000
+12146,10.326897,-9.673705,1.000000
+12147,10.356253,-9.700285,1.000000
+12148,10.388784,-9.728650,1.000000
+12149,10.419133,-9.755230,1.000000
+12150,10.449878,-9.783992,1.000000
+12151,10.481020,-9.814935,1.000000
+12152,10.508592,-9.843499,1.000000
+12153,10.536759,-9.875633,1.000000
+12154,10.568099,-9.908560,1.000000
+12155,10.594679,-9.934347,1.000000
+12156,10.624036,-9.961323,1.000000
+12157,10.653195,-9.988300,1.000000
+12158,10.682949,-10.015078,1.000000
+12159,10.716074,-10.045824,1.000000
+12160,10.745828,-10.074189,1.000000
+12161,10.773003,-10.098587,1.000000
+12162,10.804542,-10.127944,1.000000
+12163,10.831518,-10.152342,1.000000
+12164,10.865041,-10.183881,1.000000
+12165,10.893406,-10.211254,1.000000
+12166,10.924945,-10.240413,1.000000
+12167,10.960054,-10.269968,1.000000
+12168,10.989411,-10.294168,1.000000
+12169,11.020950,-10.322334,1.000000
+12170,11.057249,-10.352881,1.000000
+12171,11.086210,-10.379263,1.000000
+12172,11.116162,-10.407231,1.000000
+12173,11.144130,-10.434605,1.000000
+12174,11.176859,-10.466342,1.000000
+12175,11.212564,-10.498873,1.000000
+12176,11.240929,-10.524461,1.000000
+12177,11.271872,-10.553818,1.000000
+12178,11.306387,-10.587737,1.000000
+12179,11.337926,-10.620863,1.000000
+12180,11.364704,-10.651410,1.000000
+12186,11.552945,-10.839849,1.000000
+12187,11.580517,-10.865041,1.000000
+12188,11.612056,-10.895588,1.000000
+12189,11.642603,-10.925738,1.000000
+12190,11.676125,-10.957674,1.000000
+12191,11.707268,-10.987031,1.000000
+12192,11.736823,-11.013214,1.000000
+12193,11.773122,-11.045943,1.000000
+12194,11.802479,-11.072920,1.000000
+12195,11.834415,-11.103268,1.000000
+12196,11.868532,-11.136394,1.000000
+12197,11.900865,-11.167338,1.000000
+12198,11.931808,-11.197488,1.000000
+12199,11.964141,-11.227440,1.000000
+12200,11.997068,-11.256797,1.000000
+12201,12.026623,-11.283576,1.000000
+12202,12.063121,-11.316701,1.000000
+12203,12.092676,-11.344273,1.000000
+12204,12.128778,-11.377200,1.000000
+12205,12.158333,-11.404574,1.000000
+12206,12.193641,-11.437699,1.000000
+12207,12.223394,-11.465668,1.000000
+12208,12.259297,-11.499785,1.000000
+12209,12.292621,-11.532514,1.000000
+12210,12.321581,-11.563458,1.000000
+12211,12.357087,-11.601345,1.000000
+12212,12.389618,-11.637049,1.000000
+12213,12.418776,-11.669580,1.000000
+12214,12.454481,-11.707466,1.000000
+12215,12.482648,-11.737021,1.000000
+12216,12.517757,-11.769155,1.000000
+12217,12.547511,-11.796528,1.000000
+12218,12.584802,-11.830844,1.000000
+12219,12.614754,-11.859209,1.000000
+12220,12.647284,-11.890153,1.000000
+12221,12.679815,-11.920502,1.000000
+12222,12.715520,-11.954818,1.000000
+12223,12.749042,-11.986158,1.000000
+12224,12.781969,-12.017896,1.000000
+12225,12.811723,-12.045071,1.000000
+12226,12.844849,-12.075816,1.000000
+12227,12.878371,-12.106363,1.000000
+12228,12.913084,-12.137902,1.000000
+12229,12.947995,-12.169639,1.000000
+12230,12.982707,-12.200385,1.000000
+12231,13.019602,-12.233510,1.000000
+12232,13.051736,-12.264057,1.000000
+12233,13.086448,-12.296191,1.000000
+12234,13.117590,-12.326143,1.000000
+12235,13.156072,-12.363236,1.000000
+12236,13.186222,-12.393982,1.000000
+12237,13.219745,-12.428496,1.000000
+12238,13.255846,-12.467771,1.000000
+12239,13.285401,-12.499310,1.000000
+12240,13.320907,-12.536998,1.000000
+12241,13.353834,-12.570322,1.000000
+12242,13.387754,-12.601861,1.000000
+12243,13.419689,-12.629234,1.000000
+12244,13.457774,-12.663550,1.000000
+12245,13.492090,-12.695089,1.000000
+12246,13.522438,-12.723652,1.000000
+12247,13.558738,-12.758563,1.000000
+12248,13.591269,-12.790895,1.000000
+12249,13.621419,-12.819261,1.000000
+12250,13.659305,-12.854965,1.000000
+12251,13.691241,-12.883132,1.000000
+12252,13.726350,-12.914076,1.000000
+12253,13.760864,-12.944623,1.000000
+12254,13.798751,-12.979335,-1.000000
+12255,13.829694,-13.008295,-1.000000
+12256,13.866391,-13.044000,-1.000000
+12257,13.894557,-13.069985,-1.000000
+12258,13.918360,-13.087242,-1.000000
+12259,13.925898,-13.091209,-1.000000
+12260,13.924311,-13.085060,-1.000000
+12261,13.919551,-13.075340,-1.000000
+12262,13.915980,-13.064629,-1.000000
+12263,13.913005,-13.050942,-1.000000
\ No newline at end of file
diff --git a/frc971/control_loops/matlab/drivetrain_spin_low.csv b/frc971/control_loops/matlab/drivetrain_spin_low.csv
new file mode 100644
index 0000000..a96acbe
--- /dev/null
+++ b/frc971/control_loops/matlab/drivetrain_spin_low.csv
@@ -0,0 +1,280 @@
+120.220000,16.662837,-14.612612,12.000000,-12.000000
+120.230000,16.662837,-14.612612,12.000000,-12.000000
+120.240000,16.662837,-14.612612,12.000000,-12.000000
+120.250000,16.662837,-14.612612,12.000000,-12.000000
+120.260000,16.662837,-14.612612,12.000000,-12.000000
+120.270000,16.665217,-14.617571,12.000000,-12.000000
+120.280000,16.673152,-14.624117,12.000000,-12.000000
+120.290000,16.682474,-14.632051,12.000000,-12.000000
+120.300000,16.692194,-14.640977,12.000000,-12.000000
+120.310000,16.702112,-14.650895,12.000000,-12.000000
+120.320000,16.713220,-14.660020,12.000000,-12.000000
+120.330000,16.724526,-14.671326,12.000000,-12.000000
+120.340000,16.736626,-14.682632,12.000000,-12.000000
+120.350000,16.748527,-14.693939,12.000000,-12.000000
+120.360000,16.761024,-14.705245,12.000000,-12.000000
+120.370000,16.773719,-14.716552,12.000000,-12.000000
+120.380000,16.786017,-14.728255,12.000000,-12.000000
+120.390000,16.799109,-14.740156,12.000000,-12.000000
+120.400000,16.812002,-14.751661,12.000000,-12.000000
+120.410000,16.824895,-14.763364,12.000000,-12.000000
+120.420000,16.837193,-14.775861,12.000000,-12.000000
+120.430000,16.850087,-14.787960,12.000000,-12.000000
+120.440000,0.0,-0.0,12.000000,-12.000000
+120.450000,0.0,-0.0,12.000000,-12.000000
+120.460000,0.0,-0.0,12.000000,-12.000000
+120.470000,0.0,-0.0,12.000000,-12.000000
+120.480000,16.913759,-14.847864,12.000000,-12.000000
+120.490000,16.926653,-14.860361,12.000000,-12.000000
+120.500000,16.939546,-14.873453,12.000000,-12.000000
+120.510000,16.952439,-14.886147,12.000000,-12.000000
+120.520000,16.965134,-14.899041,12.000000,-12.000000
+120.530000,16.978821,-14.911537,12.000000,-12.000000
+120.540000,16.991714,-14.924232,12.000000,-12.000000
+120.550000,17.004409,-14.936729,12.000000,-12.000000
+120.560000,17.018294,-14.950019,12.000000,-12.000000
+120.570000,17.030592,-14.963110,12.000000,-12.000000
+120.580000,17.043882,-14.976400,12.000000,-12.000000
+120.590000,17.057569,-14.989690,12.000000,-12.000000
+120.600000,17.071850,-15.003178,12.000000,-12.000000
+120.610000,17.085537,-15.016270,12.000000,-12.000000
+120.620000,17.099422,-15.029758,12.000000,-12.000000
+120.630000,17.113109,-15.043048,12.000000,-12.000000
+120.640000,17.127192,-15.056735,12.000000,-12.000000
+120.650000,17.141672,-15.070818,12.000000,-12.000000
+120.660000,17.155756,-15.084505,12.000000,-12.000000
+120.670000,17.170434,-15.098192,12.000000,-12.000000
+120.680000,17.185113,-15.112275,12.000000,-12.000000
+120.690000,17.199990,-15.126359,12.000000,-12.000000
+120.700000,17.214073,-15.140442,12.000000,-12.000000
+120.710000,17.228553,-15.155120,12.000000,-12.000000
+120.720000,17.243430,-15.170196,12.000000,-12.000000
+120.730000,17.258307,-15.184279,12.000000,-12.000000
+120.740000,17.273183,-15.198561,12.000000,-12.000000
+120.750000,17.288259,-15.212842,12.000000,-12.000000
+120.760000,17.303731,-15.227521,12.000000,-12.000000
+120.770000,17.319202,-15.242398,12.000000,-12.000000
+120.780000,17.334674,-15.256878,12.000000,-12.000000
+120.790000,17.350345,-15.272151,12.000000,-12.000000
+120.800000,17.365618,-15.287425,12.000000,-12.000000
+120.810000,17.381288,-15.302500,12.000000,-12.000000
+120.820000,17.396959,-15.317972,12.000000,-12.000000
+120.830000,17.412431,-15.333444,12.000000,-12.000000
+120.840000,17.428101,-15.349313,12.000000,-12.000000
+120.850000,17.443176,-15.364388,12.000000,-12.000000
+120.860000,17.458846,-15.379265,12.000000,-12.000000
+120.870000,17.474318,-15.394340,12.000000,-12.000000
+120.880000,17.489988,-15.409812,12.000000,-12.000000
+120.890000,17.505064,-15.424887,12.000000,-12.000000
+120.900000,17.520932,-15.439962,12.000000,-12.000000
+120.910000,17.536404,-15.455236,12.000000,-12.000000
+120.920000,17.552074,-15.469914,12.000000,-12.000000
+120.930000,17.567745,-15.485188,12.000000,-12.000000
+120.940000,17.584208,-15.501255,12.000000,-12.000000
+120.950000,17.600275,-15.515735,12.000000,-12.000000
+120.960000,17.616541,-15.531207,12.000000,-12.000000
+120.970000,17.633004,-15.546282,12.000000,-12.000000
+120.980000,17.649865,-15.561555,12.000000,-12.000000
+120.990000,17.666527,-15.576234,12.000000,-12.000000
+121.000000,17.682792,-15.591111,12.000000,-12.000000
+121.010000,17.699256,-15.606384,12.000000,-12.000000
+121.020000,17.715521,-15.621658,12.000000,-12.000000
+121.030000,17.731786,-15.636534,12.000000,-12.000000
+121.040000,17.747853,-15.652006,12.000000,-12.000000
+121.050000,17.763722,-15.666883,12.000000,-12.000000
+121.060000,17.779789,-15.682355,12.000000,-12.000000
+121.070000,17.796253,-15.698025,12.000000,-12.000000
+121.080000,17.812716,-15.713696,12.000000,-12.000000
+121.090000,17.828585,-15.728771,12.000000,-12.000000
+121.100000,17.845247,-15.744441,12.000000,-12.000000
+121.110000,17.861512,-15.760310,12.000000,-12.000000
+121.120000,17.877778,-15.775980,12.000000,-12.000000
+121.130000,17.894241,-15.791650,12.000000,-12.000000
+121.140000,17.910507,-15.807519,12.000000,-12.000000
+121.150000,17.926970,-15.822991,12.000000,-12.000000
+121.160000,17.943037,-15.838066,12.000000,-12.000000
+121.170000,17.959104,-15.853736,12.000000,-12.000000
+121.180000,17.975568,-15.869605,12.000000,-12.000000
+121.190000,17.992032,-15.884680,12.000000,-12.000000
+121.200000,18.008694,-15.900747,12.000000,-12.000000
+121.210000,18.024364,-15.916814,12.000000,-12.000000
+121.220000,18.040828,-15.932286,12.000000,-12.000000
+121.230000,18.057291,-15.947559,12.000000,-12.000000
+121.240000,18.073755,-15.963825,12.000000,-12.000000
+121.250000,18.090615,-15.979495,12.000000,-12.000000
+121.260000,18.107476,-15.995959,12.000000,-12.000000
+121.270000,18.123939,-16.011232,12.000000,-12.000000
+121.280000,18.140998,-16.027299,12.000000,-12.000000
+121.290000,18.157462,-16.042573,12.000000,-12.000000
+121.300000,18.174719,-16.058838,12.000000,-12.000000
+121.310000,18.191183,-16.074707,12.000000,-12.000000
+121.320000,18.208242,-16.090575,12.000000,-12.000000
+121.330000,18.225102,-16.106444,12.000000,-12.000000
+121.340000,18.242161,-16.122511,12.000000,-12.000000
+121.350000,18.258823,-16.138181,12.000000,-12.000000
+121.360000,18.276080,-16.154248,12.000000,-12.000000
+121.370000,18.292742,-16.169918,12.000000,-12.000000
+121.380000,18.309801,-16.185588,12.000000,-12.000000
+121.390000,18.326463,-16.201655,12.000000,-12.000000
+121.400000,18.343521,-16.217524,12.000000,-12.000000
+121.410000,18.359985,-16.232996,12.000000,-12.000000
+121.420000,18.377242,-16.249063,12.000000,-12.000000
+121.430000,18.394301,-16.265130,12.000000,-12.000000
+121.440000,18.411558,-16.280800,12.000000,-12.000000
+121.450000,0.0,-0.0,12.000000,-12.000000
+121.460000,0.0,-0.0,12.000000,-12.000000
+121.470000,0.0,-0.0,12.000000,-12.000000
+121.480000,18.480587,-16.345068,12.000000,-12.000000
+121.490000,18.497447,-16.360738,12.000000,-12.000000
+121.500000,18.514506,-16.377202,12.000000,-12.000000
+121.510000,18.531564,-16.392475,12.000000,-12.000000
+121.520000,18.548623,-16.408542,12.000000,-12.000000
+121.530000,18.565880,-16.424609,12.000000,-12.000000
+121.540000,18.582542,-16.441073,12.000000,-12.000000
+121.550000,18.599601,-16.456743,12.000000,-12.000000
+121.560000,18.616858,-16.473009,12.000000,-12.000000
+121.570000,18.633520,-16.489274,12.000000,-12.000000
+121.580000,18.650381,-16.505738,12.000000,-12.000000
+121.590000,18.666646,-16.522003,12.000000,-12.000000
+121.600000,18.683705,-16.537673,12.000000,-12.000000
+121.610000,18.700764,-16.553542,12.000000,-12.000000
+121.620000,18.718219,-16.570204,12.000000,-12.000000
+121.630000,18.734683,-16.585676,12.000000,-12.000000
+121.640000,18.752138,-16.602140,12.000000,-12.000000
+121.650000,18.768602,-16.618008,12.000000,-12.000000
+121.660000,18.785661,-16.634075,12.000000,-12.000000
+121.670000,18.802521,-16.649745,12.000000,-12.000000
+121.680000,18.819977,-16.666011,12.000000,-12.000000
+121.690000,18.836440,-16.681879,12.000000,-12.000000
+121.700000,18.853301,-16.697748,12.000000,-12.000000
+121.710000,18.870161,-16.713617,12.000000,-12.000000
+121.720000,18.887815,-16.729882,12.000000,-12.000000
+121.730000,18.904675,-16.745949,12.000000,-12.000000
+121.740000,18.921932,-16.762214,12.000000,-12.000000
+121.750000,18.938594,-16.777686,12.000000,-12.000000
+121.760000,18.955653,-16.793555,12.000000,-12.000000
+121.770000,18.972910,-16.809423,12.000000,-12.000000
+121.780000,18.990564,-16.825689,12.000000,-12.000000
+121.790000,19.007623,-16.841359,12.000000,-12.000000
+121.800000,19.025078,-16.857426,12.000000,-12.000000
+121.810000,19.041740,-16.873096,12.000000,-12.000000
+121.820000,19.058998,-16.889163,12.000000,-12.000000
+121.830000,19.076056,-16.905032,12.000000,-12.000000
+121.840000,19.094107,-16.921694,12.000000,-12.000000
+121.850000,19.110372,-16.937364,12.000000,-12.000000
+121.860000,19.128423,-16.953828,12.000000,-12.000000
+121.870000,19.144886,-16.969300,12.000000,-12.000000
+121.880000,19.162540,-16.986160,12.000000,-12.000000
+121.890000,19.179004,-17.001433,12.000000,-12.000000
+121.900000,19.196459,-17.017897,12.000000,-12.000000
+121.910000,19.213716,-17.033964,12.000000,-12.000000
+121.920000,19.231172,-17.050428,12.000000,-12.000000
+121.930000,19.248231,-17.066098,12.000000,-12.000000
+121.940000,19.265885,-17.082562,12.000000,-12.000000
+121.950000,19.283340,-17.099025,12.000000,-12.000000
+121.960000,19.301192,-17.115886,12.000000,-12.000000
+121.970000,19.318251,-17.131754,12.000000,-12.000000
+121.980000,19.335905,-17.148218,12.000000,-12.000000
+121.990000,19.352765,-17.164285,12.000000,-12.000000
+122.000000,19.370022,-17.180947,12.000000,-12.000000
+122.010000,19.387081,-17.197411,12.000000,-12.000000
+122.020000,19.404933,-17.214073,12.000000,-12.000000
+122.030000,19.421199,-17.229942,12.000000,-12.000000
+122.040000,19.439448,-17.247000,12.000000,-12.000000
+122.050000,19.455514,-17.262472,12.000000,-12.000000
+122.060000,19.472772,-17.279333,12.000000,-12.000000
+122.070000,19.489632,-17.295400,12.000000,-12.000000
+122.080000,19.507087,-17.311665,12.000000,-12.000000
+122.090000,19.523948,-17.327930,12.000000,-12.000000
+122.100000,19.541403,-17.343799,12.000000,-12.000000
+122.110000,19.558462,-17.360064,12.000000,-12.000000
+122.120000,19.576116,-17.376925,12.000000,-12.000000
+122.130000,19.592976,-17.392992,12.000000,-12.000000
+122.140000,19.610432,-17.409654,12.000000,-12.000000
+122.150000,19.627491,-17.425125,12.000000,-12.000000
+122.160000,19.644946,-17.441788,12.000000,-12.000000
+122.170000,19.661608,-17.457458,12.000000,-12.000000
+122.180000,19.679262,-17.473525,12.000000,-12.000000
+122.190000,19.696321,-17.489195,12.000000,-12.000000
+122.200000,19.713379,-17.505262,12.000000,-12.000000
+122.210000,19.730438,-17.520932,12.000000,-12.000000
+122.220000,19.748687,-17.537198,12.000000,-12.000000
+122.230000,19.765349,-17.552868,12.000000,-12.000000
+122.240000,19.783003,-17.569728,12.000000,-12.000000
+122.250000,19.799665,-17.585398,12.000000,-12.000000
+122.260000,19.817517,-17.601862,12.000000,-12.000000
+122.270000,19.834378,-17.617136,12.000000,-12.000000
+122.280000,19.851635,-17.633401,12.000000,-12.000000
+122.290000,19.869090,-17.649270,12.000000,-12.000000
+122.300000,19.886347,-17.665535,12.000000,-12.000000
+122.310000,19.903604,-17.681404,12.000000,-12.000000
+122.320000,19.921258,-17.698066,12.000000,-12.000000
+122.330000,19.938119,-17.713934,12.000000,-12.000000
+122.340000,19.955177,-17.729803,12.000000,-12.000000
+122.350000,19.972435,-17.746068,12.000000,-12.000000
+122.360000,19.989692,-17.762532,12.000000,-12.000000
+122.370000,20.005957,-17.778400,12.000000,-12.000000
+122.380000,20.023611,-17.794864,12.000000,-12.000000
+122.390000,20.040075,-17.810534,12.000000,-12.000000
+122.400000,20.057530,-17.826998,12.000000,-12.000000
+122.410000,20.074589,-17.842867,12.000000,-12.000000
+122.420000,20.092243,-17.859925,12.000000,-12.000000
+122.430000,20.109103,-17.875199,12.000000,-12.000000
+122.440000,20.126360,-17.891663,12.000000,-12.000000
+122.450000,0.0,-0.0,12.000000,-12.000000
+122.460000,0.0,-0.0,12.000000,-12.000000
+122.470000,0.0,-0.0,12.000000,-12.000000
+122.480000,20.195587,-17.957319,12.000000,-12.000000
+122.490000,20.212249,-17.973188,12.000000,-12.000000
+122.500000,20.229506,-17.989651,12.000000,-12.000000
+122.510000,20.246565,-18.005520,12.000000,-12.000000
+122.520000,20.263822,-18.022182,12.000000,-12.000000
+122.530000,20.281079,-18.038051,12.000000,-12.000000
+122.540000,20.298733,-18.054911,12.000000,-12.000000
+122.550000,20.314998,-18.070185,12.000000,-12.000000
+122.560000,20.333049,-18.087243,12.000000,-12.000000
+122.570000,20.350108,-18.102914,12.000000,-12.000000
+122.580000,20.367761,-18.119576,12.000000,-12.000000
+122.590000,20.385018,-18.135643,12.000000,-12.000000
+122.600000,20.402276,-18.152106,12.000000,-12.000000
+122.610000,20.419929,-18.167777,12.000000,-12.000000
+122.620000,20.437782,-18.184835,12.000000,-12.000000
+122.630000,20.455039,-18.200704,12.000000,-12.000000
+122.640000,20.472296,-18.216969,12.000000,-12.000000
+122.650000,20.489355,-18.233036,12.000000,-12.000000
+122.660000,20.507604,-18.249698,12.000000,-12.000000
+122.670000,20.524861,-18.265170,12.000000,-12.000000
+122.680000,20.542118,-18.281634,12.000000,-12.000000
+122.690000,20.559177,-18.297106,12.000000,-12.000000
+122.700000,20.576235,-18.313371,12.000000,-12.000000
+122.710000,20.593492,-18.329240,12.000000,-12.000000
+122.720000,20.610948,-18.346100,12.000000,-12.000000
+122.730000,20.628205,-18.361770,12.000000,-12.000000
+122.740000,20.646454,-18.378631,12.000000,-12.000000
+122.750000,20.662918,-18.393904,12.000000,-12.000000
+122.760000,20.681166,-18.410765,12.000000,-12.000000
+122.770000,20.697829,-18.426435,12.000000,-12.000000
+122.780000,20.715681,-18.442700,12.000000,-12.000000
+122.790000,20.732739,-18.458767,12.000000,-12.000000
+122.800000,20.749997,-18.474636,12.000000,-12.000000
+122.810000,20.767254,-18.491100,12.000000,-12.000000
+122.820000,20.784709,-18.507762,12.000000,-12.000000
+122.830000,20.802958,-18.525217,12.000000,-12.000000
+122.840000,20.820017,-18.541681,12.000000,-12.000000
+122.850000,20.835290,-18.555962,12.000000,-12.000000
+122.860000,20.852547,-18.572228,12.000000,-12.000000
+122.870000,0.0,-0.0,12.000000,-12.000000
+122.880000,0.0,-0.0,12.000000,-12.000000
+122.890000,0.0,-0.0,12.000000,-12.000000
+122.900000,0.0,-0.0,12.000000,-12.000000
+122.910000,20.938833,-18.652959,12.000000,-12.000000
+122.920000,20.956289,-18.669026,12.000000,-12.000000
+122.930000,20.973744,-18.685292,12.000000,-12.000000
+122.940000,20.991596,-18.702350,12.000000,-12.000000
+122.950000,21.008258,-18.718021,12.000000,-12.000000
+122.960000,21.025912,-18.734683,12.000000,-12.000000
+122.970000,21.042772,-18.750948,12.000000,-12.000000
+122.980000,21.061021,-18.768205,12.000000,-12.000000
+122.990000,21.077882,-18.783875,12.000000,-12.000000
+123.000000,21.094941,-18.800141,12.000000,-12.000000
+123.010000,21.111999,-18.816406,12.000000,-12.000000
diff --git a/frc971/control_loops/matlab/drivetrain_spin_low.mat b/frc971/control_loops/matlab/drivetrain_spin_low.mat
new file mode 100644
index 0000000..457083f
--- /dev/null
+++ b/frc971/control_loops/matlab/drivetrain_spin_low.mat
Binary files differ
diff --git a/frc971/control_loops/matlab/drivetrain_strait_low.csv b/frc971/control_loops/matlab/drivetrain_strait_low.csv
new file mode 100644
index 0000000..6966f5b
--- /dev/null
+++ b/frc971/control_loops/matlab/drivetrain_strait_low.csv
@@ -0,0 +1,103 @@
+11.590000,15.318369,-16.398228,12.000000,12.000000
+11.600000,15.318369,-16.398228,12.000000,12.000000
+11.610000,15.318369,-16.398228,12.000000,12.000000
+11.620000,15.318369,-16.398228,12.000000,12.000000
+11.630000,15.318369,-16.398228,12.000000,12.000000
+11.640000,15.321146,-16.395649,12.000000,12.000000
+11.650000,15.329278,-16.388707,12.000000,12.000000
+11.660000,15.338403,-16.380772,12.000000,12.000000
+11.670000,15.347924,-16.372045,12.000000,12.000000
+11.680000,15.358635,-16.362325,12.000000,12.000000
+11.690000,15.369545,-16.351812,12.000000,12.000000
+11.700000,15.381050,-16.341101,12.000000,12.000000
+11.710000,15.392555,-16.329993,12.000000,12.000000
+11.720000,15.404258,-16.319083,12.000000,12.000000
+11.730000,15.415961,-16.308173,12.000000,12.000000
+11.740000,15.428061,-16.294685,12.000000,12.000000
+11.750000,15.440954,-16.283379,12.000000,12.000000
+11.760000,15.453847,-16.268502,12.000000,12.000000
+11.770000,15.466939,-16.255014,12.000000,12.000000
+11.780000,15.479038,-16.241327,12.000000,12.000000
+11.790000,15.491138,-16.228235,12.000000,12.000000
+11.800000,15.503635,-16.216136,12.000000,12.000000
+11.810000,15.515933,-16.204234,12.000000,12.000000
+11.820000,15.528628,-16.192134,12.000000,12.000000
+11.830000,15.541521,-16.179043,12.000000,12.000000
+11.840000,15.554811,-16.165951,12.000000,12.000000
+11.850000,15.568299,-16.152859,12.000000,12.000000
+11.860000,15.581589,-16.139569,12.000000,12.000000
+11.870000,15.595673,-16.126081,12.000000,12.000000
+11.880000,15.610153,-16.112791,12.000000,12.000000
+11.890000,15.624435,-16.098708,12.000000,12.000000
+11.900000,15.638716,-16.084228,12.000000,12.000000
+11.910000,15.653395,-16.069946,12.000000,12.000000
+11.920000,15.668272,-16.055466,12.000000,12.000000
+11.930000,15.683347,-16.040787,12.000000,12.000000
+11.940000,15.698620,-16.026307,12.000000,12.000000
+11.950000,15.714092,-16.011430,12.000000,12.000000
+11.960000,15.728969,-15.996554,12.000000,12.000000
+11.970000,15.744243,-15.981478,12.000000,12.000000
+11.980000,15.759715,-15.966205,12.000000,12.000000
+11.990000,15.774988,-15.950931,12.000000,12.000000
+12.000000,15.789865,-15.935459,12.000000,12.000000
+12.010000,15.805535,-15.920186,12.000000,12.000000
+12.020000,15.821205,-15.904516,12.000000,12.000000
+12.030000,15.836876,-15.888845,12.000000,12.000000
+12.040000,15.852744,-15.873175,12.000000,12.000000
+12.050000,15.868613,-15.857703,12.000000,12.000000
+12.060000,15.884680,-15.841835,12.000000,12.000000
+12.070000,15.900747,-15.826164,12.000000,12.000000
+12.080000,15.916615,-15.810296,12.000000,12.000000
+12.090000,15.932881,-15.794427,12.000000,12.000000
+12.100000,15.949146,-15.778162,12.000000,12.000000
+12.110000,15.965213,-15.762293,12.000000,12.000000
+12.120000,15.981677,-15.746226,12.000000,12.000000
+12.130000,0.0,-0.0,12.000000,12.000000
+12.140000,0.0,-0.0,12.000000,12.000000
+12.150000,0.0,-0.0,12.000000,12.000000
+12.160000,16.048127,-15.680173,12.000000,12.000000
+12.170000,16.064194,-15.663709,12.000000,12.000000
+12.180000,16.081649,-15.647047,12.000000,12.000000
+12.190000,16.098113,-15.630385,12.000000,12.000000
+12.200000,16.114973,-15.613525,12.000000,12.000000
+12.210000,16.131834,-15.597061,12.000000,12.000000
+12.220000,16.148892,-15.580399,12.000000,12.000000
+12.230000,16.165951,-15.563340,12.000000,12.000000
+12.240000,16.183010,-15.546877,12.000000,12.000000
+12.250000,16.200069,-15.530016,12.000000,12.000000
+12.260000,16.217326,-15.512958,12.000000,12.000000
+12.270000,16.234583,-15.495700,12.000000,12.000000
+12.280000,16.251840,-15.478840,12.000000,12.000000
+12.290000,16.268899,-15.461980,12.000000,12.000000
+12.300000,16.286156,-15.445119,12.000000,12.000000
+12.310000,16.303215,-15.428061,12.000000,12.000000
+12.320000,16.320273,-15.411002,12.000000,12.000000
+12.330000,16.337729,-15.393943,12.000000,12.000000
+12.340000,16.354589,-15.376884,12.000000,12.000000
+12.350000,16.371648,-15.359825,12.000000,12.000000
+12.360000,16.389103,-15.342767,12.000000,12.000000
+12.370000,16.406162,-15.325708,12.000000,12.000000
+12.380000,16.423221,-15.308649,12.000000,12.000000
+12.390000,16.440676,-15.291590,12.000000,12.000000
+12.400000,16.457934,-15.274333,12.000000,12.000000
+12.410000,16.475191,-15.257671,12.000000,12.000000
+12.420000,16.492646,-15.240216,12.000000,12.000000
+12.430000,16.509903,-15.222959,12.000000,12.000000
+12.440000,16.527557,-15.205900,12.000000,12.000000
+12.450000,16.544616,-15.189040,12.000000,12.000000
+12.460000,16.561873,-15.171782,12.000000,12.000000
+12.470000,16.579130,-15.154922,12.000000,12.000000
+12.480000,16.596982,-15.137665,12.000000,12.000000
+12.490000,16.614239,-15.120209,12.000000,12.000000
+12.500000,16.631100,-15.103151,12.000000,12.000000
+12.510000,16.648159,-15.085894,12.000000,12.000000
+12.520000,16.666011,-15.068636,12.000000,12.000000
+12.530000,16.683069,-15.051379,12.000000,12.000000
+12.540000,16.700327,-15.034321,12.000000,12.000000
+12.550000,16.717782,-15.017262,12.000000,12.000000
+12.560000,16.734841,-15.000203,12.000000,12.000000
+12.570000,16.752495,-14.982946,12.000000,12.000000
+12.580000,16.769752,-14.965887,12.000000,12.000000
+12.590000,16.787009,-14.948828,12.000000,12.000000
+12.600000,16.804464,-14.931175,12.000000,12.000000
+12.610000,16.821920,-14.914314,12.000000,12.000000
diff --git a/frc971/control_loops/matlab/drivetrain_strait_low.mat b/frc971/control_loops/matlab/drivetrain_strait_low.mat
new file mode 100644
index 0000000..e6a4973
--- /dev/null
+++ b/frc971/control_loops/matlab/drivetrain_strait_low.mat
Binary files differ
diff --git a/frc971/control_loops/matlab/drivetrain_strait_low_wgaps.csv b/frc971/control_loops/matlab/drivetrain_strait_low_wgaps.csv
new file mode 100644
index 0000000..0091dd2
--- /dev/null
+++ b/frc971/control_loops/matlab/drivetrain_strait_low_wgaps.csv
@@ -0,0 +1,185 @@
+31783,-2.373349,-1.651328,1.000000
+31784,-2.373349,-1.651328,1.000000
+31785,-2.373349,-1.651328,1.000000
+31786,-2.373349,-1.651328,1.000000
+31787,-2.370771,-1.651328,1.000000
+31788,-2.364622,-1.649344,1.000000
+31789,-2.356489,-1.639624,1.000000
+31790,-2.348158,-1.632880,1.000000
+31791,-2.336257,-1.622367,1.000000
+31792,-2.325744,-1.612449,1.000000
+31793,-2.312652,-1.599755,1.000000
+31794,-2.301941,-1.589440,1.000000
+31795,-2.288651,-1.577142,1.000000
+31796,-2.278138,-1.567224,1.000000
+31797,-2.263856,-1.553934,1.000000
+31798,-2.251558,-1.542231,1.000000
+31799,-2.236681,-1.526957,1.000000
+31800,-2.223589,-1.513072,1.000000
+31801,-2.211093,-1.499386,1.000000
+31802,-2.198200,-1.486294,1.000000
+31803,-2.185505,-1.473004,1.000000
+31804,-2.173603,-1.462491,1.000000
+31805,-2.160115,-1.450193,1.000000
+31806,-2.145238,-1.436506,1.000000
+31807,-2.131353,-1.423613,1.000000
+31808,-2.117666,-1.410521,1.000000
+31809,-2.103980,-1.397231,1.000000
+31810,-2.089500,-1.383148,1.000000
+31811,-2.076011,-1.369660,1.000000
+31812,-2.063316,-1.357163,1.000000
+31813,-2.047646,-1.341493,1.000000
+31814,-2.034356,-1.328600,1.000000
+31815,-2.017496,-1.312533,1.000000
+31816,-2.002619,-1.297854,1.000000
+31817,-1.987544,-1.282977,1.000000
+31818,-1.974055,-1.269886,1.000000
+31819,-1.957195,-1.253819,1.000000
+31820,-1.941525,-1.238347,1.000000
+31821,-1.927441,-1.224859,1.000000
+31822,-1.910383,-1.208197,1.000000
+31823,-1.896299,-1.194311,1.000000
+31824,-1.879042,-1.177253,1.000000
+31825,-1.862578,-1.161384,1.000000
+31826,-1.846908,-1.145515,1.000000
+31827,-1.829254,-1.128060,1.000000
+31828,-1.814378,-1.114175,1.000000
+31829,-1.798906,-1.098306,1.000000
+31830,-1.782442,-1.081644,1.000000
+31831,-1.765978,-1.065776,1.000000
+31832,-1.751697,-1.051097,1.000000
+31833,-1.735233,-1.035229,1.000000
+31834,-1.718571,-1.019162,1.000000
+31835,-1.700124,-1.000913,1.000000
+31836,-1.683461,-0.984846,1.000000
+31837,-1.666799,-0.968382,1.000000
+31838,-1.650137,-0.952117,1.000000
+31839,-1.633277,-0.935851,1.000000
+31840,-1.616020,-0.919189,1.000000
+31841,-1.599358,-0.902527,1.000000
+31842,-1.582696,-0.885469,1.000000
+31843,-1.565835,-0.868608,1.000000
+31844,-1.548975,-0.851748,1.000000
+31845,-1.533305,-0.836474,1.000000
+31846,-1.516643,-0.820209,1.000000
+31847,-1.497997,-0.801762,1.000000
+31848,-1.481137,-0.784901,1.000000
+31849,-1.463880,-0.767843,1.000000
+31850,-1.446424,-0.750784,1.000000
+31851,-1.430952,-0.735709,1.000000
+31852,-1.411711,-0.717261,1.000000
+31853,-1.396240,-0.701789,1.000000
+31854,-1.377197,-0.682747,1.000000
+31855,-1.359543,-0.665688,1.000000
+31856,-1.342286,-0.648630,1.000000
+31857,-1.325029,-0.631174,1.000000
+31864,-1.204626,-0.512358,1.000000
+31865,-1.187171,-0.495299,1.000000
+31866,-1.169517,-0.478240,1.000000
+31867,-1.152260,-0.460785,1.000000
+31868,-1.134606,-0.443726,1.000000
+31869,-1.117150,-0.426667,1.000000
+31870,-1.097116,-0.407228,1.000000
+31871,-1.081446,-0.391756,1.000000
+31872,-1.062205,-0.373111,1.000000
+31873,-1.044551,-0.355457,1.000000
+31874,-1.027096,-0.337803,1.000000
+31875,-1.009045,-0.320546,1.000000
+31876,-0.993574,-0.304876,1.000000
+31877,-0.974134,-0.286032,1.000000
+31878,-0.958861,-0.270560,1.000000
+31879,-0.939422,-0.250724,1.000000
+31880,-0.921371,-0.233467,1.000000
+31881,-0.903916,-0.216408,1.000000
+31882,-0.886659,-0.199349,1.000000
+31883,-0.870592,-0.183679,1.000000
+31884,-0.850954,-0.164439,1.000000
+31885,-0.834887,-0.148768,1.000000
+31886,-0.815448,-0.129726,1.000000
+31887,-0.797795,-0.112667,1.000000
+31888,-0.779942,-0.095013,1.000000
+31889,-0.762090,-0.077955,1.000000
+31890,-0.746420,-0.061689,1.000000
+31891,-0.727179,-0.042647,1.000000
+31892,-0.709525,-0.025390,1.000000
+31893,-0.693657,-0.009720,1.000000
+31894,-0.674416,0.009323,1.000000
+31895,-0.658349,0.025390,1.000000
+31896,-0.640695,0.042449,1.000000
+31897,-0.621455,0.061491,1.000000
+31898,-0.603999,0.078748,1.000000
+31899,-0.586147,0.096402,1.000000
+31900,-0.567898,0.113659,1.000000
+31901,-0.550443,0.130718,1.000000
+31902,-0.532590,0.148570,1.000000
+31903,-0.514936,0.165827,1.000000
+31904,-0.497283,0.183282,1.000000
+31905,-0.481216,0.199151,1.000000
+31906,-0.463562,0.216607,1.000000
+31907,-0.445710,0.234062,1.000000
+31908,-0.428056,0.251518,1.000000
+31909,-0.408815,0.270560,1.000000
+31910,-0.390765,0.288412,1.000000
+31911,-0.373706,0.305471,1.000000
+31912,-0.357441,0.320943,1.000000
+31913,-0.337803,0.340580,1.000000
+31914,-0.321934,0.356052,1.000000
+31915,-0.302495,0.375491,1.000000
+31916,-0.284643,0.392550,1.000000
+31917,-0.266791,0.410005,1.000000
+31918,-0.248939,0.427659,1.000000
+31919,-0.231285,0.444916,1.000000
+31920,-0.213234,0.462570,1.000000
+31921,-0.195581,0.479827,1.000000
+31922,-0.178125,0.497084,1.000000
+31923,-0.160273,0.514738,1.000000
+31924,-0.142818,0.532194,1.000000
+31925,-0.126949,0.548261,1.000000
+31926,-0.109097,0.566113,1.000000
+31927,-0.089459,0.585552,1.000000
+31928,-0.071607,0.602809,1.000000
+31929,-0.053557,0.620463,1.000000
+31930,-0.035308,0.638315,1.000000
+31931,-0.017852,0.655572,1.000000
+31932,0.000198,0.673424,1.000000
+31933,0.016265,0.688896,1.000000
+31934,0.036101,0.708732,1.000000
+31935,0.054152,0.726187,1.000000
+31936,0.071607,0.743841,1.000000
+31937,0.089658,0.761693,1.000000
+31938,0.107113,0.779149,1.000000
+31939,0.124965,0.796803,1.000000
+31940,0.141429,0.812671,1.000000
+31941,0.158884,0.830325,1.000000
+31942,0.177927,0.849368,1.000000
+31943,0.195581,0.867418,1.000000
+31944,0.213433,0.884874,1.000000
+31945,0.229500,0.900742,1.000000
+31946,0.249336,0.919784,1.000000
+31947,0.266989,0.937240,1.000000
+31948,0.285040,0.954695,1.000000
+31949,0.301107,0.970564,1.000000
+31950,0.320943,0.989805,1.000000
+31951,0.338200,1.007260,1.000000
+31952,0.355854,1.024517,1.000000
+31953,0.372119,1.040584,1.000000
+31954,0.391360,1.059627,1.000000
+31955,0.409609,1.077479,1.000000
+31956,0.426866,1.094934,1.000000
+31957,0.444718,1.112191,1.000000
+31964,0.566906,1.232793,1.000000
+31965,0.584362,1.250248,-1.000000
+31966,0.602412,1.267902,-1.000000
+31967,0.620066,1.285556,-1.000000
+31968,0.637720,1.302615,-1.000000
+31969,0.656762,1.317690,-1.000000
+31970,0.664895,1.321062,-1.000000
+31971,0.667077,1.320070,-1.000000
+31972,0.669655,1.316698,-1.000000
+31973,0.666482,1.313524,-1.000000
+31974,0.660928,1.307177,-1.000000
+31975,0.652002,1.297656,-1.000000
+31976,0.641687,1.287143,-1.000000
+31977,0.630777,1.276630,-1.000000
+31978,0.619273,1.268101,-1.000000
+31979,0.608760,1.256992,-1.000000
\ No newline at end of file
diff --git a/frc971/control_loops/matlab/drivetrain_strait_low_wgaps.mat b/frc971/control_loops/matlab/drivetrain_strait_low_wgaps.mat
new file mode 100644
index 0000000..e9cd690
--- /dev/null
+++ b/frc971/control_loops/matlab/drivetrain_strait_low_wgaps.mat
Binary files differ
diff --git a/frc971/control_loops/matlab/writeMat.m b/frc971/control_loops/matlab/writeMat.m
new file mode 100644
index 0000000..b1541f5
--- /dev/null
+++ b/frc971/control_loops/matlab/writeMat.m
@@ -0,0 +1,16 @@
+function writeMat(fd, matrix, name)
+ %fprintf(fd, '%s = init_matrix(%d, %d);\n', name, size(matrix, 1), size(matrix, 2));
+ fprintf(fd, '%s << ', name);
+ first_loop = 1;
+ for i=1:size(matrix, 1)
+ for j=1:size(matrix, 2)
+ if first_loop
+ first_loop = 0;
+ else
+ fprintf(fd, ', ');
+ end
+ fprintf(fd, '%.10f', matrix(i, j));
+ end
+ end
+ fprintf(fd, '; \\\n');
+end
\ No newline at end of file
diff --git a/frc971/control_loops/matlab/writeMatFlash.m b/frc971/control_loops/matlab/writeMatFlash.m
new file mode 100644
index 0000000..10104d0
--- /dev/null
+++ b/frc971/control_loops/matlab/writeMatFlash.m
@@ -0,0 +1,16 @@
+function writeMatFlash(fd, matrix, name)
+ %fprintf(fd, '%s = init_matrix(%d, %d);\n', name, size(matrix, 1), size(matrix, 2));
+ fprintf(fd, 'flash_matrix(%s, ', name);
+ first_loop = 1;
+ for i=1:size(matrix, 1)
+ for j=1:size(matrix, 2)
+ if first_loop
+ first_loop = 0;
+ else
+ fprintf(fd, ', ');
+ end
+ fprintf(fd, '%.10f', matrix(i, j));
+ end
+ end
+ fprintf(fd, ');\n');
+end
diff --git a/frc971/control_loops/matlab/writeMatFooter.m b/frc971/control_loops/matlab/writeMatFooter.m
new file mode 100644
index 0000000..b23664e
--- /dev/null
+++ b/frc971/control_loops/matlab/writeMatFooter.m
@@ -0,0 +1,3 @@
+function writeMatFooter(fd)
+ fprintf(fd, '\n');
+end
\ No newline at end of file
diff --git a/frc971/control_loops/matlab/writeMatFooterFlash.m b/frc971/control_loops/matlab/writeMatFooterFlash.m
new file mode 100644
index 0000000..5326a94
--- /dev/null
+++ b/frc971/control_loops/matlab/writeMatFooterFlash.m
@@ -0,0 +1,3 @@
+function writeMatFooterFlash(fd)
+ fprintf(fd, '\n');
+end
diff --git a/frc971/control_loops/matlab/writeMatHeader.m b/frc971/control_loops/matlab/writeMatHeader.m
new file mode 100644
index 0000000..5c100f3
--- /dev/null
+++ b/frc971/control_loops/matlab/writeMatHeader.m
@@ -0,0 +1,4 @@
+function writeMatHeader(fd, number_of_states, number_of_outputs)
+ fprintf(fd, 'typedef StateFeedbackLoop<%d, %d> MatrixClass;\n', number_of_states, number_of_outputs);
+ fprintf(fd, '#define MATRIX_INIT ');
+end
\ No newline at end of file
diff --git a/frc971/control_loops/matlab/writeMatHeaderFlash.m b/frc971/control_loops/matlab/writeMatHeaderFlash.m
new file mode 100644
index 0000000..88a6cc6
--- /dev/null
+++ b/frc971/control_loops/matlab/writeMatHeaderFlash.m
@@ -0,0 +1,4 @@
+function writeMatHeaderFlash(fd, number_of_states, number_of_outputs)
+ fprintf(fd, 'typedef StateFeedbackLoop<%d, %d> MatrixClass;\n', number_of_states, number_of_outputs);
+ fprintf(fd, '#define MATRIX_INIT ');
+end
diff --git a/frc971/control_loops/python/controls.py b/frc971/control_loops/python/controls.py
new file mode 100644
index 0000000..7d34a85
--- /dev/null
+++ b/frc971/control_loops/python/controls.py
@@ -0,0 +1,83 @@
+#!/usr/bin/python
+
+"""
+Control loop pole placement library.
+
+This library will grow to support many different pole placement methods.
+Currently it only supports direct pole placement.
+"""
+
+__author__ = 'Austin Schuh (austin.linux@gmail.com)'
+
+import numpy
+import slycot
+
+class Error (Exception):
+ """Base class for all control loop exceptions."""
+
+
+class PolePlacementError(Error):
+ """Exception raised when pole placement fails."""
+
+
+# TODO(aschuh): dplace should take a control system object.
+# There should also exist a function to manipulate laplace expressions, and
+# something to plot bode plots and all that.
+def dplace(A, B, poles, alpha=1e-6):
+ """Set the poles of (A - BF) to poles.
+
+ Args:
+ A: numpy.matrix(n x n), The A matrix.
+ B: numpy.matrix(n x m), The B matrix.
+ poles: array(imaginary numbers), The poles to use. Complex conjugates poles
+ must be in pairs.
+
+ Raises:
+ ValueError: Arguments were the wrong shape or there were too many poles.
+ PolePlacementError: Pole placement failed.
+
+ Returns:
+ numpy.matrix(m x n), K
+ """
+ # See http://www.icm.tu-bs.de/NICONET/doc/SB01BD.html for a description of the
+ # fortran code that this is cleaning up the interface to.
+ n = A.shape[0]
+ if A.shape[1] != n:
+ raise ValueError("A must be square")
+ if B.shape[0] != n:
+ raise ValueError("B must have the same number of states as A.")
+ m = B.shape[1]
+
+ num_poles = len(poles)
+ if num_poles > n:
+ raise ValueError("Trying to place more poles than states.")
+
+ out = slycot.sb01bd(n=n,
+ m=m,
+ np=num_poles,
+ alpha=alpha,
+ A=A,
+ B=B,
+ w=numpy.array(poles),
+ dico='D')
+
+ A_z = numpy.matrix(out[0])
+ num_too_small_eigenvalues = out[2]
+ num_assigned_eigenvalues = out[3]
+ num_uncontrollable_eigenvalues = out[4]
+ K = numpy.matrix(-out[5])
+ Z = numpy.matrix(out[6])
+
+ if num_too_small_eigenvalues != 0:
+ raise PolePlacementError("Number of eigenvalues that are too small "
+ "and are therefore unmodified is %d." %
+ num_too_small_eigenvalues)
+ if num_assigned_eigenvalues != num_poles:
+ raise PolePlacementError("Did not place all the eigenvalues that were "
+ "requested. Only placed %d eigenvalues." %
+ num_assigned_eigenvalues)
+ if num_uncontrollable_eigenvalues != 0:
+ raise PolePlacementError("Found %d uncontrollable eigenvlaues." %
+ num_uncontrollable_eigenvalues)
+
+ return K
diff --git a/frc971/control_loops/python/libcdd.py b/frc971/control_loops/python/libcdd.py
new file mode 100644
index 0000000..a217728
--- /dev/null
+++ b/frc971/control_loops/python/libcdd.py
@@ -0,0 +1,129 @@
+#!/usr/bin/python
+
+"""Wrapper around libcdd, a polytope manipulation library."""
+
+__author__ = 'Austin Schuh (austin.linux@gmail.com)'
+
+import ctypes
+
+# Wrapper around PyFile_AsFile so that we can print out the error messages.
+# Set the arg type and return types of the function call.
+class FILE(ctypes.Structure):
+ pass
+
+ctypes.pythonapi.PyFile_AsFile.argtypes = [ctypes.py_object]
+ctypes.pythonapi.PyFile_AsFile.restype = ctypes.POINTER(FILE)
+
+# Load and init libcdd. libcdd is a C library that implements algorithm to
+# manipulate half space and vertex representations of polytopes.
+# Unfortunately, the library was compiled with C++ even though it has a lot of C
+# code in it, so all the symbol names are mangled. Ug.
+libcdd = ctypes.cdll.LoadLibrary('libcdd.so')
+libcdd._Z23dd_set_global_constantsv()
+
+# The variable type mytype that libcdd defines (double[1])
+# See http://docs.python.org/2/library/ctypes.html#arrays for the documentation
+# explaining why ctypes.c_double * 1 => double[1]
+# libcdd defines mytype to various things so it can essentially template its
+# functions. What a weird library.
+mytype = ctypes.c_double * 1
+
+
+# Forward declaration for the polyhedra data structure.
+class dd_polyhedradata(ctypes.Structure):
+ pass
+
+
+# Definition of dd_matrixdata
+class dd_matrixdata(ctypes.Structure):
+ _fields_ = [
+ ("rowsize", ctypes.c_long),
+ ("linset", ctypes.POINTER(ctypes.c_ulong)),
+ ("colsize", ctypes.c_long),
+ ("representation", ctypes.c_int),
+ ("numbtype", ctypes.c_int),
+ ("matrix", ctypes.POINTER(ctypes.POINTER(mytype))),
+ ("objective", ctypes.c_int),
+ ("rowvec", ctypes.POINTER(mytype)),
+ ]
+
+# Define the input and output types for a bunch of libcdd functions.
+libcdd._Z15dd_CreateMatrixll.restype = ctypes.POINTER(dd_matrixdata)
+libcdd._Z9ddd_get_dPd.argtypes = [mytype]
+libcdd._Z9ddd_get_dPd.restype = ctypes.c_double
+
+libcdd._Z17dd_CopyGeneratorsP16dd_polyhedradata.argtypes = [
+ ctypes.POINTER(dd_polyhedradata)
+]
+libcdd._Z17dd_CopyGeneratorsP16dd_polyhedradata.restype = ctypes.POINTER(dd_matrixdata)
+
+libcdd._Z16dd_DDMatrix2PolyP13dd_matrixdataP12dd_ErrorType.argtypes = [
+ ctypes.POINTER(dd_matrixdata),
+ ctypes.POINTER(ctypes.c_int)
+]
+libcdd._Z16dd_DDMatrix2PolyP13dd_matrixdataP12dd_ErrorType.restype = (
+ ctypes.POINTER(dd_polyhedradata))
+
+libcdd._Z13dd_FreeMatrixP13dd_matrixdata.argtypes = [
+ ctypes.POINTER(dd_matrixdata)
+]
+
+libcdd._Z16dd_FreePolyhedraP16dd_polyhedradata.argtypes = [
+ ctypes.POINTER(dd_polyhedradata)
+]
+
+libcdd._Z9ddd_set_dPdd.argtypes = [
+ mytype,
+ ctypes.c_double
+]
+
+
+# Various enums.
+DD_INEQUALITY = 1
+DD_REAL = 1
+DD_NO_ERRORS = 17
+
+
+def dd_CreateMatrix(rows, cols):
+ return libcdd._Z15dd_CreateMatrixll(
+ ctypes.c_long(rows),
+ ctypes.c_long(cols))
+
+
+def dd_set_d(mytype_address, double_value):
+ libcdd._Z9ddd_set_dPdd(mytype_address,
+ ctypes.c_double(double_value))
+
+
+def dd_CopyGenerators(polyhedraptr):
+ return libcdd._Z17dd_CopyGeneratorsP16dd_polyhedradata(polyhedraptr)
+
+
+def dd_get_d(mytype_address):
+ return libcdd._Z9ddd_get_dPd(mytype_address)
+
+
+def dd_FreeMatrix(matrixptr):
+ libcdd._Z13dd_FreeMatrixP13dd_matrixdata(matrixptr)
+
+
+def dd_FreePolyhedra(polyhedraptr):
+ libcdd._Z16dd_FreePolyhedraP16dd_polyhedradata(polyhedraptr)
+
+
+def dd_DDMatrix2Poly(matrixptr):
+ error = ctypes.c_int()
+ polyhedraptr = libcdd._Z16dd_DDMatrix2PolyP13dd_matrixdataP12dd_ErrorType(
+ matrixptr,
+ ctypes.byref(error))
+
+ # Return None on error.
+ # The error values are enums, so they aren't exposed.
+ if error.value != NO_ERRORS:
+ # Dump out the errors to stderr
+ libcdd._Z21dd_WriteErrorMessagesP8_IO_FILE12dd_ErrorType(
+ ctypes.pythonapi.PyFile_AsFile(ctypes.py_object(sys.stdout)),
+ error)
+ dd_FreePolyhedra(polyhedraptr)
+ return None
+ return polyhedraptr
diff --git a/frc971/control_loops/python/polytope.py b/frc971/control_loops/python/polytope.py
new file mode 100644
index 0000000..e08a160
--- /dev/null
+++ b/frc971/control_loops/python/polytope.py
@@ -0,0 +1,224 @@
+#!/usr/bin/python
+
+"""
+Polyhedral set library.
+
+This library implements convex regions of the form H x <= k, where H, x, and k
+are matricies. It also provides convenient methods to find all the verticies.
+"""
+
+__author__ = 'Austin Schuh (austin.linux@gmail.com)'
+
+
+import libcdd
+import numpy
+import string
+import sys
+
+
+def _PiecewiseConcat(*args):
+ """Concatenates strings inside lists, elementwise.
+
+ Given ['a', 's'] and ['d', 'f'], returns ['ad', 'sf']
+ """
+ return map(''.join, zip(*args))
+
+
+def _SplitAndPad(s):
+ """Splits a string on newlines, and pads the lines to be the same width."""
+ split_string = s.split('\n')
+ width = max(len(stringpiece) for stringpiece in split_string) + 1
+
+ padded_strings = [string.ljust(stringpiece, width, ' ')
+ for stringpiece in split_string]
+ return padded_strings
+
+
+def _PadHeight(padded_array, min_height):
+ """Adds lines of spaces to the top and bottom of an array symmetrically."""
+ height = len(padded_array)
+ if height < min_height:
+ pad_array = [' ' * len(padded_array[0])]
+ height_error = min_height - height
+ return (pad_array * ((height_error) / 2) +
+ padded_array +
+ pad_array * ((height_error + 1) / 2))
+ return padded_array
+
+
+class HPolytope(object):
+ """This object represents a H-polytope.
+
+ Polytopes are convex regions in n-dimensional space.
+ For H-polytopes, this is represented as the intersection of a set of half
+ planes. The mathematic equation that represents this is H x <= k.
+ """
+
+ def __init__(self, H, k):
+ """Constructs a H-polytope from the H and k matricies.
+
+ Args:
+ H: numpy.Matrix (n by k), where n is the number of constraints, and k is
+ the number of dimensions in the space. Does not copy the matrix.
+ k: numpy.Matrix (n by 1). Does not copy the matrix.
+ """
+ self._H = H
+ self._k = k
+
+ @property
+ def k(self):
+ """Returns the k in H x <= k."""
+ return self._k
+
+ @property
+ def H(self):
+ """Returns the H in H x <= k."""
+ return self._H
+
+ @property
+ def ndim(self):
+ """Returns the dimension of the set."""
+ return self._H.shape[1]
+
+ @property
+ def num_constraints(self):
+ """Returns the number of constraints defining the set."""
+ return self._k.shape[0]
+
+ def IsInside(self, point):
+ """Returns true if the point is inside the polytope, edges included."""
+ return (self._H * point <= self._k).all()
+
+ def Vertices(self):
+ """Returns a matrix with the vertices of the set in its rows."""
+ # TODO(aschuh): It would be better to write some small C/C++ function that
+ # does all of this and takes in a numpy array.
+ # The copy is expensive in Python and cheaper in C.
+
+ # Create an empty matrix with the correct size.
+ matrixptr = libcdd.dd_CreateMatrix(self.num_constraints, self.ndim + 1)
+ matrix = matrixptr.contents
+
+ try:
+ # Copy the data into the matrix.
+ for i in xrange(self.num_constraints):
+ libcdd.dd_set_d(matrix.matrix[i][0], self._k[i, 0])
+ for j in xrange(self.ndim):
+ libcdd.dd_set_d(matrix.matrix[i][j + 1], -self._H[i, j])
+
+ # Set enums to the correct values.
+ matrix.representation = libcdd.DD_INEQUALITY
+ matrix.numbtype = libcdd.DD_REAL
+
+ # TODO(aschuh): Set linearity if it is useful.
+ # This would be useful if we had any constraints saying B - A x = 0
+
+ # Build a Polyhedra
+ polyhedraptr = libcdd.dd_DDMatrix2Poly(matrixptr)
+
+ if not polyhedraptr:
+ return None
+
+ try:
+ # Return None on error.
+ # The error values are enums, so they aren't exposed.
+
+ # Magic happens here. Computes the vertices
+ vertex_matrixptr = libcdd.dd_CopyGenerators(
+ polyhedraptr)
+ vertex_matrix = vertex_matrixptr.contents
+
+ try:
+ # Count the number of vertices and rays in the result.
+ num_vertices = 0
+ num_rays = 0
+ for i in xrange(vertex_matrix.rowsize):
+ if libcdd.dd_get_d(vertex_matrix.matrix[i][0]) == 0:
+ num_rays += 1
+ else:
+ num_vertices += 1
+
+ # Build zeroed matricies for the new vertices and rays.
+ vertices = numpy.matrix(numpy.zeros((num_vertices,
+ vertex_matrix.colsize - 1)))
+ rays = numpy.matrix(numpy.zeros((num_rays,
+ vertex_matrix.colsize - 1)))
+
+ ray_index = 0
+ vertex_index = 0
+
+ # Copy the data out of the matrix.
+ for index in xrange(vertex_matrix.rowsize):
+ if libcdd.dd_get_d(vertex_matrix.matrix[index][0]) == 0.0:
+ for j in xrange(vertex_matrix.colsize - 1):
+ rays[ray_index, j] = libcdd.dd_get_d(
+ vertex_matrix.matrix[index][j + 1])
+ ray_index += 1
+ else:
+ for j in xrange(vertex_matrix.colsize - 1):
+ vertices[vertex_index, j] = libcdd.dd_get_d(
+ vertex_matrix.matrix[index][j + 1])
+ vertex_index += 1
+ finally:
+ # Free everything.
+ libcdd.dd_FreeMatrix(vertex_matrixptr)
+
+ finally:
+ libcdd.dd_FreePolyhedra(polyhedraptr)
+
+ finally:
+ libcdd.dd_FreeMatrix(matrixptr)
+
+ # Rays are unsupported right now. This may change in the future.
+ assert(rays.shape[0] == 0)
+
+ return vertices
+
+
+ def __str__(self):
+ """Returns a formatted version of the polytope.
+
+ The dump will look something like the following, which prints out the matrix
+ comparison.
+
+ [[ 1 0] [[12]
+ [-1 0] [[x0] <= [12]
+ [ 0 1] [x1]] [12]
+ [ 0 -1]] [12]]
+ """
+ height = max(self.ndim, self.num_constraints)
+
+ # Split the print up into 4 parts and concatenate them all.
+ H_strings = _PadHeight(_SplitAndPad(str(self.H)), height)
+ v_strings = _PadHeight(_SplitAndPad(str(self.k)), height)
+ x_strings = _PadHeight(self._MakeXStrings(), height)
+ cmp_strings = self._MakeCmpStrings(height)
+
+ return '\n'.join(_PiecewiseConcat(H_strings, x_strings,
+ cmp_strings, v_strings))
+
+ def _MakeXStrings(self):
+ """Builds an array of strings with constraint names in it for printing."""
+ x_strings = []
+ if self.ndim == 1:
+ x_strings = ["[[x0]] "]
+ else:
+ for index in xrange(self.ndim):
+ if index == 0:
+ x = "[[x%d] " % index
+ elif index == self.ndim - 1:
+ x = " [x%d]] " % index
+ else:
+ x = " [x%d] " % index
+ x_strings.append(x)
+ return x_strings
+
+ def _MakeCmpStrings(self, height):
+ """Builds an array of strings with the comparison in it for printing."""
+ cmp_strings = []
+ for index in xrange(height):
+ if index == (height - 1) / 2:
+ cmp_strings.append("<= ")
+ else:
+ cmp_strings.append(" ")
+ return cmp_strings
diff --git a/frc971/control_loops/python/polytope_test.py b/frc971/control_loops/python/polytope_test.py
new file mode 100755
index 0000000..9a35ebe
--- /dev/null
+++ b/frc971/control_loops/python/polytope_test.py
@@ -0,0 +1,192 @@
+#!/usr/bin/python
+
+import numpy
+from numpy.testing import *
+import polytope
+import unittest
+
+__author__ = 'Austin Schuh (austin.linux@gmail.com)'
+
+def MakePoint(*args):
+ """Makes a point from a set of arguments."""
+ return numpy.matrix([[arg] for arg in args])
+
+class TestHPolytope(unittest.TestCase):
+ def setUp(self):
+ """Builds a simple box polytope."""
+ self.H = numpy.matrix([[1, 0],
+ [-1, 0],
+ [0, 1],
+ [0, -1]])
+ self.k = numpy.matrix([[12],
+ [12],
+ [12],
+ [12]])
+ self.p = polytope.HPolytope(self.H, self.k)
+
+ def test_Hk(self):
+ """Tests that H and k are saved correctly."""
+ assert_array_equal(self.p.H, self.H)
+ assert_array_equal(self.p.k, self.k)
+
+ def test_IsInside(self):
+ """Tests IsInside for various points."""
+ inside_points = [
+ MakePoint(0, 0),
+ MakePoint(6, 6),
+ MakePoint(12, 6),
+ MakePoint(-6, 10)]
+ outside_points = [
+ MakePoint(14, 0),
+ MakePoint(-14, 0),
+ MakePoint(0, 14),
+ MakePoint(0, -14),
+ MakePoint(14, -14)]
+
+ for inside_point in inside_points:
+ self.assertTrue(self.p.IsInside(inside_point),
+ msg='Point is' + str(inside_point))
+
+ for outside_point in outside_points:
+ self.assertFalse(self.p.IsInside(outside_point),
+ msg='Point is' + str(outside_point))
+
+ def AreVertices(self, p, vertices):
+ """Checks that all the vertices are on corners of the set."""
+ for i in xrange(vertices.shape[0]):
+ # Check that all the vertices have the correct number of active
+ # constraints.
+ lmda = p.H * vertices[i,:].T - p.k
+ num_active_constraints = 0
+ for j in xrange(lmda.shape[0]):
+ # Verify that the constraints are either active, or not violated.
+ if numpy.abs(lmda[j, 0]) <= 1e-9:
+ num_active_constraints += 1
+ else:
+ self.assertLessEqual(lmda[j, 0], 0.0)
+
+ self.assertEqual(p.ndim, num_active_constraints)
+
+ def HasSamePoints(self, expected, actual):
+ """Verifies that the points in expected are in actual."""
+ found_points = set()
+ self.assertEqual(expected.shape, actual.shape)
+ for index in xrange(expected.shape[0]):
+ expected_point = expected[index, :]
+ for actual_index in xrange(actual.shape[0]):
+ actual_point = actual[actual_index, :]
+ if numpy.abs(expected_point - actual_point).max() <= 1e-4:
+ found_points.add(actual_index)
+ break
+
+ self.assertEqual(len(found_points), actual.shape[0],
+ msg="Expected:\n" + str(expected) + "\nActual:\n" + str(actual))
+
+ def test_Skewed_Nonsym_Vertices(self):
+ """Tests the vertices of a severely skewed space."""
+ self.H = numpy.matrix([[10, -1],
+ [-1, -1],
+ [-1, 10],
+ [10, 10]])
+ self.k = numpy.matrix([[2],
+ [2],
+ [2],
+ [2]])
+ self.p = polytope.HPolytope(self.H, self.k)
+ vertices = self.p.Vertices()
+ self.AreVertices(self.p, vertices)
+
+ self.HasSamePoints(
+ numpy.matrix([[0., 0.2],
+ [0.2, 0.],
+ [-2., 0.],
+ [0., -2.]]),
+ vertices)
+
+ def test_Vertices_Nonsym(self):
+ """Tests the vertices of a nonsymetric space."""
+ self.k = numpy.matrix([[6],
+ [12],
+ [2],
+ [10]])
+ self.p = polytope.HPolytope(self.H, self.k)
+ vertices = self.p.Vertices()
+ self.AreVertices(self.p, vertices)
+
+ self.HasSamePoints(
+ numpy.matrix([[6., 2.],
+ [6., -10.],
+ [-12., -10.],
+ [-12., 2.]]),
+ vertices)
+
+ def test_Vertices(self):
+ """Tests the vertices of a nonsymetric space."""
+ self.HasSamePoints(self.p.Vertices(),
+ numpy.matrix([[12., 12.],
+ [12., -12.],
+ [-12., -12.],
+ [-12., 12.]]))
+
+ def test_concat(self):
+ """Tests that the concat function works for simple inputs."""
+ self.assertEqual(["asd", "qwe"],
+ polytope._PiecewiseConcat(["a", "q"],
+ ["s", "w"],
+ ["d", "e"]))
+
+ def test_str(self):
+ """Verifies that the str method works for the provided p."""
+ self.assertEqual('[[ 1 0] [[12] \n'
+ ' [-1 0] [[x0] <= [12] \n'
+ ' [ 0 1] [x1]] [12] \n'
+ ' [ 0 -1]] [12]] ',
+ str(self.p))
+
+ def MakePWithDims(self, num_constraints, num_dims):
+ """Makes a zeroed out polytope with the correct size."""
+ self.p = polytope.HPolytope(
+ numpy.matrix(numpy.zeros((num_constraints, num_dims))),
+ numpy.matrix(numpy.zeros((num_constraints, 1))))
+
+ def test_few_constraints_odd_constraint_even_dims_str(self):
+ """Tests printing out the set with odd constraints and even dimensions."""
+ self.MakePWithDims(num_constraints=5, num_dims=2)
+ self.assertEqual('[[ 0. 0.] [[ 0.] \n'
+ ' [ 0. 0.] [[x0] [ 0.] \n'
+ ' [ 0. 0.] [x1]] <= [ 0.] \n'
+ ' [ 0. 0.] [ 0.] \n'
+ ' [ 0. 0.]] [ 0.]] ',
+ str(self.p))
+
+ def test_few_constraints_odd_constraint_small_dims_str(self):
+ """Tests printing out the set with odd constraints and odd dimensions."""
+ self.MakePWithDims(num_constraints=5, num_dims=1)
+ self.assertEqual('[[ 0.] [[ 0.] \n'
+ ' [ 0.] [ 0.] \n'
+ ' [ 0.] [[x0]] <= [ 0.] \n'
+ ' [ 0.] [ 0.] \n'
+ ' [ 0.]] [ 0.]] ',
+ str(self.p))
+
+ def test_few_constraints_odd_constraint_odd_dims_str(self):
+ """Tests printing out the set with odd constraints and odd dimensions."""
+ self.MakePWithDims(num_constraints=5, num_dims=3)
+ self.assertEqual('[[ 0. 0. 0.] [[ 0.] \n'
+ ' [ 0. 0. 0.] [[x0] [ 0.] \n'
+ ' [ 0. 0. 0.] [x1] <= [ 0.] \n'
+ ' [ 0. 0. 0.] [x2]] [ 0.] \n'
+ ' [ 0. 0. 0.]] [ 0.]] ',
+ str(self.p))
+
+ def test_many_constraints_even_constraint_odd_dims_str(self):
+ """Tests printing out the set with even constraints and odd dimensions."""
+ self.MakePWithDims(num_constraints=2, num_dims=3)
+ self.assertEqual('[[ 0. 0. 0.] [[x0] [[ 0.] \n'
+ ' [ 0. 0. 0.]] [x1] <= [ 0.]] \n'
+ ' [x2]] ',
+ str(self.p))
+
+
+if __name__ == '__main__':
+ unittest.main()
diff --git a/frc971/crio/build.sh b/frc971/crio/build.sh
new file mode 100755
index 0000000..ec04978
--- /dev/null
+++ b/frc971/crio/build.sh
@@ -0,0 +1,3 @@
+#!/bin/bash
+
+../../aos/build/build.sh crio crio.gyp no $1
diff --git a/frc971/crio/crio.gyp b/frc971/crio/crio.gyp
new file mode 100644
index 0000000..236ec14
--- /dev/null
+++ b/frc971/crio/crio.gyp
@@ -0,0 +1,54 @@
+{
+ 'targets': [
+ {
+# The WPILib code that we've modified.
+ 'target_name': 'WPILib_changes',
+ 'type': 'static_library',
+ 'sources': [
+ '<(AOS)/externals/WPILib/WPILib/DriverStationLCD.cpp',
+ '<(AOS)/externals/WPILib/WPILib/Synchronized.cpp',
+ '<(AOS)/externals/WPILib/WPILib/DriverStation.cpp',
+ '<(AOS)/externals/WPILib/WPILib/Notifier.cpp',
+ '<(AOS)/externals/WPILib/WPILib/MotorSafetyHelper.cpp',
+ '<(AOS)/externals/WPILib/WPILib/Resource.cpp',
+ '<(AOS)/externals/WPILib/WPILib/SolenoidBase.cpp',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):WPILib',
+ ],
+ 'cflags!': ['-Werror'],
+ },
+ {
+ 'target_name': 'user_program',
+ 'type': 'static_library',
+ 'sources': [
+ 'main.cc',
+ ],
+ 'dependencies': [
+ '../input/input.gyp:SensorReader',
+ '../input/input.gyp:SensorWriter',
+ '../output/output.gyp:MotorWriter',
+ '../output/output.gyp:SensorSender',
+ 'WPILib_changes',
+ '<(EXTERNALS):WPILib',
+ '<(AOS)/common/messages/messages.gyp:aos_queues',
+ ],
+ },
+ {
+ 'target_name': 'FRC_UserProgram',
+ 'type': 'shared_library',
+ 'dependencies': [
+ 'user_program'
+ ],
+ },
+ {
+ 'target_name': 'FRC_UserProgram_WithTests',
+ 'type': 'shared_library',
+ 'dependencies': [
+ 'user_program',
+ # For testing.
+ '<(AOS)/build/aos_all.gyp:Crio',
+ ],
+ },
+ ],
+}
diff --git a/frc971/crio/main.cc b/frc971/crio/main.cc
new file mode 100644
index 0000000..e814b97
--- /dev/null
+++ b/frc971/crio/main.cc
@@ -0,0 +1,19 @@
+#include "aos/crio/controls/ControlsManager.h"
+
+#include "aos/crio/motor_server/CRIOControlLoopRunner.h"
+
+namespace frc971 {
+
+class MyRobot : public ::aos::crio::ControlsManager {
+ public:
+ virtual void RegisterControlLoops() {
+ //::aos::crio::CRIOControlLoopRunner::AddControlLoop(&shooter_);
+ }
+
+ private:
+ //::frc971::control_loops::ShooterMotor shooter_;
+};
+
+} // namespace frc971
+
+START_ROBOT_CLASS(::frc971::MyRobot);
diff --git a/frc971/frc971.gyp b/frc971/frc971.gyp
new file mode 100644
index 0000000..ff43949
--- /dev/null
+++ b/frc971/frc971.gyp
@@ -0,0 +1,14 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'common',
+ 'type': 'static_library',
+ 'sources': [
+ 'constants.cpp',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ ],
+ }
+ ],
+}
diff --git a/frc971/input/AutoMode.act b/frc971/input/AutoMode.act
new file mode 100644
index 0000000..b7b98c3
--- /dev/null
+++ b/frc971/input/AutoMode.act
@@ -0,0 +1,7 @@
+args automode_args {
+};
+status automode_status {
+};
+//has OnEnd;
+//has OnStart;
+priority 99;
diff --git a/frc971/input/AutoMode.cc b/frc971/input/AutoMode.cc
new file mode 100644
index 0000000..254d6b6
--- /dev/null
+++ b/frc971/input/AutoMode.cc
@@ -0,0 +1,15 @@
+#include "frc971/input/AutoMode.q.h"
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <unistd.h>
+
+#include "aos/common/control_loop/Timing.h"
+
+namespace frc971 {
+
+void AutoMode_t::DoAction() {
+ Sleep(10); // wait until it ends
+}
+
+} // namespace frc971
diff --git a/frc971/input/GyroReader.cc b/frc971/input/GyroReader.cc
new file mode 100644
index 0000000..a6e1d00
--- /dev/null
+++ b/frc971/input/GyroReader.cc
@@ -0,0 +1,50 @@
+#include <unistd.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+
+#include "aos/aos_core.h"
+
+#include "frc971/queues/GyroAngle.q.h"
+
+#define M_PI 3.14159265358979323846264338327
+
+using frc971::sensors::gyro;
+
+int main(){
+ aos::Init();
+ int fd = open("/dev/aschuh0", O_RDONLY);
+ int rate_limit = 0;
+ if (fd < 0) {
+ LOG(ERROR, "No Gyro found.\n");
+ } else {
+ LOG(INFO, "Gyro now connected\n");
+ }
+
+ while (true) {
+ int64_t gyro_value;
+ if (read(fd, (void *)&gyro_value, sizeof(gyro_value)) != sizeof(gyro_value)) {
+ LOG(ERROR, "Could not read gyro errno: %d\n", errno);
+ if (errno == ENODEV || errno == EBADF) {
+ close(fd);
+ while (1) {
+ usleep(1000);
+ fd = open("/dev/aschuh0", O_RDONLY);
+ if (fd > 0) {
+ LOG(INFO, "Found gyro again\n");
+ break;
+ }
+ }
+ }
+ continue;
+ }
+ rate_limit ++;
+ if (rate_limit > 10) {
+ LOG(DEBUG, "Gyro is %d\n", (int)(gyro_value / 16));
+ rate_limit = 0;
+ }
+ gyro.MakeWithBuilder().angle(gyro_value / 16.0 / 1000.0 / 180.0 * M_PI).Send();
+ }
+
+ aos::Cleanup();
+}
diff --git a/frc971/input/JoystickReader.cc b/frc971/input/JoystickReader.cc
new file mode 100644
index 0000000..e0cfbac
--- /dev/null
+++ b/frc971/input/JoystickReader.cc
@@ -0,0 +1,100 @@
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+#include <math.h>
+
+#include "aos/aos_core.h"
+#include "aos/atom_code/input/FRCComm.h"
+#include "aos/atom_code/input/JoystickInput.h"
+
+#include "frc971/input/AutoMode.q.h"
+#include "frc971/control_loops/DriveTrain.q.h"
+#include "frc971/queues/GyroAngle.q.h"
+#include "frc971/queues/Piston.q.h"
+
+using ::frc971::control_loops::drivetrain;
+using ::frc971::control_loops::shifters;
+using ::frc971::sensors::gyro;
+
+namespace frc971 {
+
+class JoystickReader : public aos::JoystickInput {
+ public:
+ JoystickReader() : aos::JoystickInput() {
+ shifters.MakeWithBuilder().set(true).Send();
+ }
+
+ virtual void RunIteration() {
+ static bool is_high_gear = false;
+
+ if (Pressed(0, AUTONOMOUS)) {
+ if (PosEdge(0, ENABLED)){
+ LOG(INFO, "Starting auto mode\n");
+ AutoMode.Start();
+ }
+ if (NegEdge(0, ENABLED)) {
+ LOG(INFO, "Stopping auto mode\n");
+ AutoMode.Stop();
+ }
+ } else { // teleop
+ bool is_control_loop_driving = false;
+ double left_goal = 0.0;
+ double right_goal = 0.0;
+ const double wheel = control_data_.stick0Axis1 / 127.0;
+ const double throttle = -control_data_.stick1Axis2 / 127.0;
+ const double kThrottleGain = 1.0 / 2.5;
+ if (Pressed(0, 7) || Pressed(0, 11)) {
+ static double distance = 0.0;
+ static double angle = 0.0;
+ static double filtered_goal_distance = 0.0;
+ if (PosEdge(0, 7) || PosEdge(0, 11)) {
+ if (drivetrain.position.FetchLatest() && gyro.FetchLatest()) {
+ distance = (drivetrain.position->left_encoder +
+ drivetrain.position->right_encoder) / 2.0
+ - throttle * kThrottleGain / 2.0;
+ angle = gyro->angle;
+ filtered_goal_distance = distance;
+ }
+ }
+ is_control_loop_driving = true;
+
+ //const double gyro_angle = Gyro.View().angle;
+ const double goal_theta = angle - wheel * 0.27;
+ const double goal_distance = distance + throttle * kThrottleGain;
+ const double robot_width = 22.0 / 100.0 * 2.54;
+ const double kMaxVelocity = 0.6;
+ if (goal_distance > kMaxVelocity * 0.02 + filtered_goal_distance) {
+ filtered_goal_distance += kMaxVelocity * 0.02;
+ } else if (goal_distance < -kMaxVelocity * 0.02 + filtered_goal_distance) {
+ filtered_goal_distance -= kMaxVelocity * 0.02;
+ } else {
+ filtered_goal_distance = goal_distance;
+ }
+ left_goal = filtered_goal_distance - robot_width * goal_theta / 2.0;
+ right_goal = filtered_goal_distance + robot_width * goal_theta / 2.0;
+ is_high_gear = false;
+
+ LOG(DEBUG, "Left goal %f Right goal %f\n", left_goal, right_goal);
+ }
+ if (!(drivetrain.goal.MakeWithBuilder()
+ .steering(wheel)
+ .throttle(throttle)
+ .highgear(is_high_gear).quickturn(Pressed(0, 5))
+ .control_loop_driving(is_control_loop_driving)
+ .left_goal(left_goal).right_goal(right_goal).Send())) {
+ LOG(WARNING, "sending stick values failed\n");
+ }
+
+ if (PosEdge(1, 1)) {
+ is_high_gear = false;
+ }
+ if (PosEdge(1, 3)) {
+ is_high_gear = true;
+ }
+ }
+ }
+};
+
+} // namespace frc971
+
+AOS_RUN(frc971::JoystickReader)
diff --git a/frc971/input/SensorReader.cc b/frc971/input/SensorReader.cc
new file mode 100644
index 0000000..fd14df4
--- /dev/null
+++ b/frc971/input/SensorReader.cc
@@ -0,0 +1,44 @@
+#define __STDC_LIMIT_MACROS
+
+#include <arpa/inet.h>
+
+#include "aos/aos_core.h"
+#include "aos/common/inttypes.h"
+#include "aos/common/input/SensorInput.h"
+
+#include "frc971/control_loops/DriveTrain.q.h"
+#include "frc971/queues/sensor_values.h"
+
+#define M_PI 3.14159265358979323846
+
+using ::frc971::control_loops::drivetrain;
+
+namespace frc971 {
+
+namespace {
+inline double drivetrain_translate(int32_t in) {
+ // TODO(2013) fix the math
+ return static_cast<double>(in) / (256.0 * 4.0 * 44.0 / 32.0) *
+ (3.5 * 2.54 / 100.0 * M_PI);
+}
+} // namespace
+
+class SensorReader : public aos::SensorInput<sensor_values> {
+ virtual void RunIteration(sensor_values &sensors) {
+ for (size_t i = 0; i < sizeof(sensors.encoders) / sizeof(sensors.encoders[0]); ++i) {
+ sensors.encoders[i] = ntohl(sensors.encoders[i]);
+ }
+
+ // TODO(aschuh): Convert to meters.
+ const double left_encoder = drivetrain_translate(sensors.lencoder);
+ const double right_encoder = drivetrain_translate(sensors.rencoder);
+ drivetrain.position.MakeWithBuilder()
+ .left_encoder(left_encoder)
+ .right_encoder(right_encoder)
+ .Send();
+ }
+};
+
+} // namespace frc971
+
+AOS_RUN(frc971::SensorReader)
diff --git a/frc971/input/SensorWriter.cc b/frc971/input/SensorWriter.cc
new file mode 100644
index 0000000..1fb34db
--- /dev/null
+++ b/frc971/input/SensorWriter.cc
@@ -0,0 +1,40 @@
+#include <arpa/inet.h>
+
+#include "WPILib/Task.h"
+#include "WPILib/Encoder.h"
+#include "WPILib/DigitalInput.h"
+#include "WPILib/Counter.h"
+
+#include "aos/aos_core.h"
+#include "aos/crio/motor_server/SensorOutput.h"
+#include "aos/common/inttypes.h"
+#include "aos/common/mutex.h"
+#include "aos/crio/shared_libs/interrupt_notifier.h"
+
+#include "frc971/queues/sensor_values.h"
+
+using ::aos::MutexLocker;
+
+namespace frc971 {
+
+class SensorWriter : public aos::SensorOutput<sensor_values> {
+ Encoder lencoder;
+ Encoder rencoder;
+
+ public:
+ SensorWriter() : lencoder(1, 2), rencoder(3, 4) {
+ lencoder.Start();
+ rencoder.Start();
+
+ printf("frc971::SensorWriter started\n");
+ }
+
+ virtual void RunIteration(sensor_values &vals) {
+ vals.lencoder = htonl(-lencoder.GetRaw());
+ vals.rencoder = -htonl(-rencoder.GetRaw());
+ }
+};
+
+} // namespace frc971
+
+AOS_RUN(frc971::SensorWriter)
diff --git a/frc971/input/input.gyp b/frc971/input/input.gyp
new file mode 100644
index 0000000..43f591a
--- /dev/null
+++ b/frc971/input/input.gyp
@@ -0,0 +1,80 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'actions',
+ 'type': 'static_library',
+ 'sources': ['AutoMode.act'],
+ 'variables': {
+ 'header_path': 'frc971/input',
+ },
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ '<(AOS)/common/common.gyp:controls',
+ ],
+ 'includes': ['../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'JoystickReader',
+ 'type': 'executable',
+ 'sources': [
+ 'JoystickReader.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ '<(AOS)/atom_code/input/input.gyp:joystick',
+ '<(AOS)/common/network/network.gyp:socket',
+ 'actions',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:control_loops',
+ '<(DEPTH)/frc971/queues/queues.gyp:queues',
+ ],
+ },
+ {
+ 'target_name': 'SensorReader',
+ 'type': '<(aos_target)',
+ 'sources': [
+ 'SensorReader.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:control_loops',
+ '<(DEPTH)/frc971/queues/queues.gyp:queues',
+ '<(AOS)/common/network/network.gyp:socket',
+ ],
+ },
+ {
+ 'target_name': 'SensorWriter',
+ 'type': '<(aos_target)',
+ 'sources': [
+ 'SensorWriter.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:control_loops',
+ ],
+ },
+ {
+ 'target_name': 'GyroReader',
+ 'type': 'executable',
+ 'sources': [
+ 'GyroReader.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ '<(DEPTH)/frc971/queues/queues.gyp:queues',
+ ],
+ },
+ {
+ 'target_name': 'AutoMode',
+ 'type': 'executable',
+ 'sources': [
+ 'AutoMode.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:control_loops',
+ '<(DEPTH)/frc971/queues/queues.gyp:queues',
+ 'actions',
+ ],
+ },
+ ],
+}
diff --git a/frc971/output/AtomMotorWriter.cc b/frc971/output/AtomMotorWriter.cc
new file mode 100644
index 0000000..d0bf6f7
--- /dev/null
+++ b/frc971/output/AtomMotorWriter.cc
@@ -0,0 +1,40 @@
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+
+#include "aos/aos_core.h"
+#include "aos/common/network/SendSocket.h"
+#include "aos/common/control_loop/Timing.h"
+#include "aos/common/messages/RobotState.q.h"
+#include "aos/atom_code/output/MotorOutput.h"
+
+#include "frc971/queues/Piston.q.h"
+#include "frc971/control_loops/DriveTrain.q.h"
+#include "frc971/constants.h"
+
+using ::frc971::control_loops::drivetrain;
+using ::frc971::control_loops::shifters;
+
+namespace frc971 {
+namespace output {
+
+class MotorWriter : public aos::MotorOutput {
+ void RunIteration() {
+ if (drivetrain.output.FetchLatest()) {
+ AddMotor(TALON, 7, -drivetrain.output->left_voltage / 12.0);
+ AddMotor(TALON, 4, drivetrain.output->right_voltage / 12.0);
+ } else {
+ AddMotor(TALON, 7, 0.0f);
+ AddMotor(TALON, 4, 0.0f);
+ }
+
+ if (shifters.FetchLatest()) {
+ AddSolenoid(1, shifters->set);
+ }
+ }
+};
+
+} // namespace output
+} // namespace frc971
+
+AOS_RUN(frc971::output::MotorWriter)
diff --git a/frc971/output/CRIOMotorWriter.cc b/frc971/output/CRIOMotorWriter.cc
new file mode 100644
index 0000000..b85ac4b
--- /dev/null
+++ b/frc971/output/CRIOMotorWriter.cc
@@ -0,0 +1,15 @@
+#include "WPILib/Victor.h"
+
+#include "aos/crio/motor_server/MotorOutput.h"
+#include "aos/aos_core.h"
+
+namespace frc971 {
+
+class MotorWriter : public aos::MotorOutput {
+ virtual void RunIteration() {
+ }
+};
+
+} // namespace frc971
+
+AOS_RUN(frc971::MotorWriter)
diff --git a/frc971/output/CameraServer.cc b/frc971/output/CameraServer.cc
new file mode 100644
index 0000000..96f698d
--- /dev/null
+++ b/frc971/output/CameraServer.cc
@@ -0,0 +1,81 @@
+#include <string.h>
+
+#include "aos/aos_core.h"
+#include "aos/atom_code/output/HTTPServer.h"
+#include "aos/atom_code/output/evhttp_ctemplate_emitter.h"
+#include "ctemplate/template.h"
+
+#include "frc971/constants.h"
+
+RegisterTemplateFilename(ROBOT_HTML, "robot.html.tpl");
+
+namespace frc971 {
+
+const char *const kPath = "/home/driver/robot_code/bin/";
+//const char *const kPath = "/home/brians/Desktop/git_frc971/2012/trunk/src/frc971/output";
+
+class CameraServer : public aos::http::HTTPServer {
+ public:
+ CameraServer() : HTTPServer(kPath, 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));
+
+ 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 (!ctemplate::ExpandTemplate(ROBOT_HTML, ctemplate::STRIP_WHITESPACE,
+ &dict, &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
+
+AOS_RUN_NRT(frc971::CameraServer)
+
diff --git a/frc971/output/SensorSender.cc b/frc971/output/SensorSender.cc
new file mode 100644
index 0000000..e2d2e51
--- /dev/null
+++ b/frc971/output/SensorSender.cc
@@ -0,0 +1,5 @@
+#include "aos/crio/motor_server/SensorSender.h"
+#include "frc971/queues/sensor_values.h"
+
+AOS_RUN_FORK(aos::SensorSender<frc971::sensor_values>, "971SS", 100)
+
diff --git a/frc971/output/output.gyp b/frc971/output/output.gyp
new file mode 100644
index 0000000..6cb28d1
--- /dev/null
+++ b/frc971/output/output.gyp
@@ -0,0 +1,59 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'CameraServer',
+ 'type': 'executable',
+ 'sources': [
+ 'CameraServer.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ '<(AOS)/atom_code/output/output.gyp:http_server',
+ '../frc971.gyp:common',
+ ],
+ 'copies': [
+ {
+ 'destination': '<(rsync_dir)',
+ 'files': [
+ 'robot.html.tpl',
+ ],
+ },
+ ],
+ },
+ {
+ 'target_name': 'MotorWriter',
+ 'type': '<(aos_target)',
+ 'conditions': [
+ ['OS=="atom"', {
+ 'sources': ['AtomMotorWriter.cc'],
+ 'dependencies': [
+ '../frc971.gyp:common',
+ '<(AOS)/atom_code/output/output.gyp:motor_output',
+ '<(AOS)/atom_code/messages/messages.gyp:messages',
+ ],
+ }, {
+ 'sources': ['CRIOMotorWriter.cc'],
+ }
+ ],
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ '<(AOS)/common/common.gyp:controls',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:control_loops',
+ '<(DEPTH)/frc971/queues/queues.gyp:queues',
+ '<(AOS)/common/network/network.gyp:socket',
+ ],
+ },
+ {
+ 'target_name': 'SensorSender',
+ 'type': '<(aos_target)',
+ 'sources': [
+ 'SensorSender.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ '<(AOS)/common/network/network.gyp:socket',
+ ],
+ },
+ ],
+}
diff --git a/frc971/output/robot.html.tpl b/frc971/output/robot.html.tpl
new file mode 100644
index 0000000..7ae51a6
--- /dev/null
+++ b/frc971/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/queues/GyroAngle.q b/frc971/queues/GyroAngle.q
new file mode 100644
index 0000000..bcf3ac4
--- /dev/null
+++ b/frc971/queues/GyroAngle.q
@@ -0,0 +1,7 @@
+package frc971.sensors;
+
+message Gyro {
+ double angle;
+};
+
+queue Gyro gyro;
diff --git a/frc971/queues/Piston.q b/frc971/queues/Piston.q
new file mode 100644
index 0000000..0819567
--- /dev/null
+++ b/frc971/queues/Piston.q
@@ -0,0 +1,7 @@
+package frc971.control_loops;
+
+message Piston {
+ bool set;
+};
+
+queue Piston shifters;
diff --git a/frc971/queues/queues.gyp b/frc971/queues/queues.gyp
new file mode 100644
index 0000000..70ed873
--- /dev/null
+++ b/frc971/queues/queues.gyp
@@ -0,0 +1,40 @@
+{
+ 'variables': {
+ 'queue_files': [
+ 'GyroAngle.q',
+ 'Piston.q',
+ ]
+ },
+ 'targets': [
+ {
+ 'target_name': 'queues',
+ 'type': 'static_library',
+ 'sources': ['<@(queue_files)'],
+ 'variables': {
+ 'header_path': 'frc971/queues',
+ },
+ 'dependencies': [
+ '<(AOS)/common/common.gyp:queues',
+ '<(AOS)/build/aos.gyp:libaos',
+ ],
+ 'includes': ['../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'frc971_queues_so',
+ 'type': 'loadable_module',
+ 'sources': ['<@(queue_files)'],
+ 'variables': {
+ 'header_path': 'frc971/queues',
+ },
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:aos_shared_lib',
+ ],
+ 'direct_dependent_settings': {
+ 'variables': {
+ 'jni_libs': ['frc971_queues_so'],
+ },
+ },
+ 'includes': ['../../aos/build/queues.gypi'],
+ },
+ ],
+}
diff --git a/frc971/queues/sensor_values.h b/frc971/queues/sensor_values.h
new file mode 100644
index 0000000..16d6890
--- /dev/null
+++ b/frc971/queues/sensor_values.h
@@ -0,0 +1,22 @@
+#ifndef __COMMON_SENSOR_VALUES_H_
+#define __COMMON_SENSOR_VALUES_H_
+
+#include <stdint.h>
+
+namespace frc971 {
+
+struct sensor_values {
+ union {
+ struct {
+ int32_t lencoder, rencoder;
+ };
+ uint32_t encoders[2];
+ };
+
+ // TODO(2013) all the rest
+};
+
+} // namespace frc971
+
+#endif
+