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/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)