Diff: robot.h

Revision:

0:caa8ee3bd882

Child:

1:ff11ee1c6baa

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/robot.h	Sun Nov 12 00:14:05 2017 +0000
@@ -0,0 +1,84 @@
+// GEAR RATIO AND ENCODER COUNTS
+#define GEAR_RATIO       131
+#define COUNTS_PER_REV   64
+
+// Motor 1
+#define M1_DIR              D7
+#define M1_PWM              D6
+#define M1_ENC_A            D10
+#define M1_ENC_B            D11
+
+// Motor 2
+#define M2_DIR              D4
+#define M2_PWM              D5
+#define M2_ENC_A            D12
+#define M2_ENC_B            D13
+
+#include "motor.h"
+#include "controller.h"
+
+
+
+/**
+ * Robot
+ */
+namespace robot
+{
+
+enum State {
+    OFF,
+    CALIBRATION,
+    HOMING,
+    READY,
+    DEMO,
+    MANUAL
+};
+
+const char *StateNames[] = {
+    "Off",
+    "Calibration",
+    "Homing",
+    "Ready",
+    "Demo",
+    "Manual"
+};
+
+void SwitchState(State new_state);
+void GoToNextState();
+
+State state = OFF;
+
+const double kSampleTime = 0.001;
+
+const double kL1 = 0.30;
+const double kL2 = 0.38;
+
+const double kCalibAngleMotor1 = 140;
+const double kCalibAngleMotor2 = 0;
+
+
+Motor m1(M1_PWM, M1_DIR, M1_ENC_A, M1_ENC_B, GEAR_RATIO*COUNTS_PER_REV);
+Motor m2(M2_PWM, M2_DIR, M2_ENC_A, M2_ENC_B, GEAR_RATIO*COUNTS_PER_REV, true);
+
+Controller c1(.8,.1,0,kSampleTime);
+Controller c2(.8,.1,0,kSampleTime);
+
+bool is_calibrated = false;
+
+bool has_power();
+
+double get_angle(int i);
+double get_x();
+double get_y();
+
+double ForwardKinematicsX(double theta_1, double theta_2);
+double ForwardKinematicsY(double theta_1, double theta_2);
+
+double InverseKinematicsTheta(double x, double y, int i);
+
+void Start();
+void Stop();
+void ControlLoop();
+
+void SetCalibrationAngles();
+}
\ No newline at end of file