[zumo-cc3200/zumo-cc3200.git] / src / Energia / libraries / ZumoCC3200 / examples / SplineDrive / motorLoop.ino
diff --git a/src/Energia/libraries/ZumoCC3200/examples/SplineDrive/motorLoop.ino b/src/Energia/libraries/ZumoCC3200/examples/SplineDrive/motorLoop.ino
--- /dev/null
@@ -0,0 +1,70 @@
+/*
+ * ======== motorLoop ========
+ * This sketch waits until trajectory is generated, then proceeeds
+ * to use PID heading control to follow the calculated trajectory
+ */
+
+#include <Energia.h>
+#include <math.h>
+
+#include <ZumoMotors.h>
+#include <PIDController.h>
+#include "MotionPlanner.h"
+
+#define PERIOD 0 /* period of motor control updates */
+#define TEST 0
+
+float targetAngle = 0.0;
+bool targetAngleSet = false;
+
+static ZumoMotors motors; /* Zumo motor driver provided by Pololu */
+
+PIDController steerPID;
+
+Utilities util;
+
+/*
+ * ======== motorSetup ========
+ */
+
+void motorSetup(void)
+{
+ Serial.begin(9600);
+ Serial.println("motorSetup ...");
+
+ /* initialize the Pololu driver motor library */
+ motors.setRightSpeed(0);
+ motors.setLeftSpeed(0);
+
+ Serial.println("motorSetup done.");
+
+ util = Utilities();
+
+ steerPID = PIDController(0.01717, 0.0, 0.003);
+
+}
+
+/*
+
+ * ======== motorLoop ========
+
+ */
+
+void motorLoop(void)
+{
+
+ if(mp.ready()){
+
+ mp.followSpline(motors, steerPID, 0.35);
+
+ }
+
+ delay(20);
+}
+
+
+
+
+
+
+