Added arduino libs

libraries/SimplePID-master/LICENSE

@@ -0,0 +1,28 @@
+Copyright (c) 2015, Mark Rose
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of bsd-license nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+

libraries/SimplePID-master/README.asciidoc

@@ -0,0 +1,93 @@
+= SimplePID
+
+A PID controller library independent of run environment, but one which works well with Arduino.
+
+== Installation
+
+To install the library into your Arduino installation, go to the _Releases_ tab and download
+the latest release. This will be a ZIP file called `SimplePID-__version__.zip`, for some
+version number. Unzip this into your `__sketchbook__/libraries` folder, where _sketchbook_
+is the root of your Arduino sketches. On OS X and Windows it is usually `~/Documents/Arduino`.
+This will create a directory called `SimplePID-__version__`. Rename that directory to
+`SimplePID`.
+
+You should then have a directory under your _sketchbook_ directory containing the library files at:
+
+  libraries/SimplePID/
+
+== Usage
+
+To use the SimplePID library, add `#include <SimplePID.h>` to the includes in your sketch.
+Then create an instance of the PID controller by defining a global variable:
+
+----
+SimplePID myPID(pConstant, iConstant, dConstant);
+----
+
+(Replace `pConstant`, `iConstant`, and `dConstant` with the proportional, integral, and differential
+constants you desire.) For a P-controller, set the integral and differential constants to zero,
+for example.
+
+=== Setting the Target
+
+The target value you want to track is usually called the _setpoint_. At any time you can change
+the setpoint by calling `SimplePID.setSetPoint(someValue)`. For example, using the PID controller
+defined in the example above, we could set the target at 3.4 as follows:
+
+----
+myPID.setSetPoint(3.4);
+----
+
+=== Getting the Control Value
+
+Your code needs to have some way of keeping track of time in order to determine the delta
+time from the last control value. A recommended way of doing that is farther down. You also
+need to have a way of sensing the actual error value,
+the difference in speed of the motor versus the desired value, or the process error.
+Once you have the actual error value and the delta time, in seconds, you get the control value
+to use like this:
+
+----
+float controlValue = myPID.getControlValue(actualError, dt);
+----
+
+For a motor, you will usually add the control value to the current control value and change
+the motor speed.
+
+=== Keeping Track of Delta Time
+
+One way of doing that is as follows:
+
+----
+#include <SimplePID.h>
+
+SimplePID myPID(...);
+unsigned long lastLoopTime;
+
+void setup() {
+    ...
+    lastLoopTime = micros();
+}
+
+void loop() {
+    delay(howeverLongYouLike);
+    
+    float error = ... some way of getting the actual error value ...
+    
+    unsigned long curLoopTime = micros();
+    float dt = (curLoopTime - lastLoopTime) / 1E6;
+    
+    float controlValue = myPID.getControlValue(error, dt);
+    ... code to control the motor or other process ...
+    
+    lastLoopTime = curLoopTime;
+}
+----
+
+In this case `dt` is set to the delta time since the last control, in seconds.
+
+== An Example
+
+There is an example of using the library to investigate PID tunings for a motor. It is specific
+to the DFRobot Romeo v2 Arduino board and motor driver, but should give a reasonable impression
+of how to use `SimplePID`. Open that sketch by going to Files > Examples > SimplePID > RomeoPIDTest.

libraries/SimplePID-master/SimplePID.cpp

@@ -0,0 +1,61 @@
+#include <float.h>
+#include "SimplePID.h"
+
+SimplePID::SimplePID(float Kp, float Ki, float Kd) {
+  this->Kp = Kp;
+  this->Ki = Ki;
+  this->Kd = Kd;
+
+  minOutput = -FLT_MAX;
+  maxOutput = FLT_MAX;
+  
+  sumError = 0.0;
+  lastActual = 0.0;
+}
+
+void SimplePID::setConstants(float Kp, float Ki, float Kd) {
+  this->Kp = Kp;
+  this->Ki = Ki;
+  this->Kd = Kd;
+}
+  
+void SimplePID::setOutputRange(float minOutput, float maxOutput) {
+  this->minOutput = minOutput;
+  this->maxOutput = maxOutput;
+}
+
+void SimplePID::setSetPoint(float setPoint) {
+  this->setPoint = setPoint;
+  sumError = 0.0;
+}
+
+float SimplePID::getCumulativeError() {
+  return sumError;
+}
+
+void SimplePID::clearCumulativeError() {
+  sumError = 0.0;
+}
+
+float SimplePID::getControlValue(float actual, float dt) {
+  float error = setPoint - actual;
+
+  float newSum = sumError + error*dt;
+
+  // The derivative is calculated by assuming the two setpoints are
+  // equal. This works better when changing the setpoint because the
+  // derivative error does not suddenly increase.
+  float dErrorDt = (lastActual - actual) / dt;
+  lastActual = actual;
+
+  float output = Kp*error + Ki*sumError + Kd*dErrorDt;
+
+  if (output >= maxOutput) {
+    return maxOutput;
+  } else if (output <= minOutput) {
+    return minOutput;
+  } else {
+    sumError = newSum;
+    return output;
+  }
+}

libraries/SimplePID-master/SimplePID.h

@@ -0,0 +1,36 @@
+// SimplePID.h - Definitions for the simple PID library.
+
+#ifndef SIMPLE_PID_H
+#define SIMPLE_PID_H
+
+class SimplePID {
+  public:
+
+    SimplePID(float Kp, float Ki, float Kd);
+
+    void setConstants(float Kp, float Ki, float Kd);
+    
+    void setOutputRange(float minOutput, float maxOutput);
+
+    void setSetPoint(float setPoint);
+
+    float getCumulativeError();
+
+    void clearCumulativeError();
+
+    float getControlValue(float actual, float dt);
+
+  private:
+  
+    float Kp;
+    float Ki;
+    float Kd;
+    float setPoint;
+    float minOutput;
+    float maxOutput;
+    float lastActual;
+    float sumError;
+
+};
+
+#endif SIMPLE_PID_H

libraries/SimplePID-master/examples/AStarPIDTest/AStarPIDTest.ino

@@ -0,0 +1,296 @@
+// AStarPIDTest - Test the PID control for an AStar-based robot.
+//
+// To gather PID tuning data for graphing, put the robot on blocks, then
+// start this program and open a Serial Monitor. Press S4 to start both
+// motors, let them come to a steady speed, then press S4 to stop the
+// motors. Copy the Serial output to a spreadsheet and plot target speed and
+// current speed versus time. You should see a classic PID controller graph
+// with some overshoot.
+
+#include <AStar32U4.h>
+#include <EnableInterrupt.h>
+#include <SimplePID.h>
+
+// Motor parameters. These are based on a Pololu #2285 motor with 48cpr
+// encoder.
+const float GEAR_RATIO = 210.59;
+const float ENCODER_CPR = 12;
+
+// Number of encoder ticks per revolution of the wheel.
+const float TICKS_PER_REV = GEAR_RATIO * ENCODER_CPR;
+
+// Maximum desired motor speed. Pololu #2285 are rated for 130rpm, but we
+// don't drive them with enough voltage to achieve that.
+const float MAX_REVS_PER_SEC = 100.0 / 60.0;
+
+// Use 60% of maximum speed for PID tuning.
+const int PID_TUNING_TICKS_PER_SEC = 0.6 * MAX_REVS_PER_SEC * TICKS_PER_REV;
+
+// DFRobot Romeo v2 and BLE PIN assignments.
+
+const int ONBOARD_SWITCH_PIN = A7;
+
+// Pins for the Pololu motor encoder outputs.
+const int M1_A = 7;
+const int M1_B = 11;
+const int M2_A = 15;
+const int M2_B = 16;
+
+// Minimum motor control value. Motor output below this will stall.
+const int MIN_MOTOR_CMD = 10;
+
+// Maximum motor control value.
+const int MAX_MOTOR_CMD = 400;
+
+AStar32U4Motors motors;
+
+// These objects provide access to the A-Star's on-board
+// buttons.  We will only use buttonA.
+AStar32U4ButtonA buttonA;
+AStar32U4ButtonB buttonB;
+AStar32U4ButtonC buttonC;
+
+enum { NO_BUTTON, BUTTON_A, BUTTON_B, BUTTON_C };
+
+int leftSpeedTarget = 0;
+int rightSpeedTarget = 0;
+
+volatile long leftTicks;
+volatile long rightTicks;
+
+long lastLeftTicks = 0;
+long lastRightTicks = 0;
+
+int leftMotorCmd = 0;
+int rightMotorCmd = 0;
+
+unsigned long lastLoopTime;
+
+unsigned long lastSwitchTime;
+
+float loopTime = 0.0;
+
+// Ziegler-Nichols tuning. See this Wikipedia article for details:
+//     https://en.wikipedia.org/wiki/PID_controller#Loop_tuning
+// To tune the controller, set USE_KU_ONLY to 1 and increase Ku
+// until oscillation occurs, and set Tu to the oscillation period
+// in seconds. Then set USE_KU_ONLY to zero to use the
+// Ziegler-Nichols tune. To get a less agressive tune, set LESS_AGGRESSIVE
+// to 1.
+
+// Set to 1 to use Ku only, to determine oscillation point.
+#define USE_KU_ONLY 0
+
+const float Ku = .15;
+const float Tu = .1142857143;
+
+#define MANUAL_TUNE 1
+#define LESS_AGGRESSIVE 0
+
+#if MANUAL_TUNE
+// Found empirically to rapidly converge with little overshoot.
+// There is no integral constant, but not needed when driving
+// on flat ground, and when the heading is otherwise controlled
+// by higher-level software. (I.e., there's no need to catch up
+// for past errors.)
+const float Kp = 0.07;
+const float Ki = 0.0;
+const float Kd = 0.001;
+#elif !LESS_AGGRESSIVE
+const float Kp = 0.6*Ku;
+const float Ki = 2*Kp/Tu;
+const float Kd = Kp*Tu/8;
+#else
+const float Kp = 0.2*Ku;
+const float Ki = 2*Kp/Tu;
+const float Kd = Kp*Tu/3;
+#endif
+
+#if USE_KU_ONLY
+SimplePID leftController = SimplePID(Ku, 0, 0);
+SimplePID rightController = SimplePID(Ku, 0, 0);
+#else
+SimplePID leftController = SimplePID(Kp, Ki, Kd);
+SimplePID rightController = SimplePID(Kp, Ki, Kd);
+#endif
+
+// Sets up the serial output and the motor control pins, and attaches
+// interrupt handlers to the pins on which we will read the encoder
+// phototransistor values.
+void setup() {
+  Serial.begin(115200);
+  delay(3000);
+
+  enableInterrupt(M1_A, leftAChange, CHANGE);
+  enableInterrupt(M1_B, leftBChange, CHANGE);
+  enableInterrupt(M2_A, rightAChange, CHANGE);
+  enableInterrupt(M2_B, rightBChange, CHANGE);
+
+//  Serial.print("Time (s)\t");
+  Serial.print("Left Target\tLeft Speed");
+//  Serial.print("\tLeft Cum Error\tLeft Motor\t");
+  Serial.print("\tRight Target\tRight Speed");
+//  Serial.print("\tRight Cum Error\tRight Motor");
+  Serial.println();
+
+  lastLoopTime = micros();
+  lastSwitchTime = millis();
+
+  leftTicks = rightTicks = 0;
+}
+
+// Loops forever showing the motor speeds and errors since the last loop,
+// and adjusts the target speeds depending on whether the user is pressing
+// switches S2 through S5, as indicated in the code below.
+void loop() {
+  delay(15);
+
+  unsigned long curLoopTime = micros();
+  noInterrupts();
+  long curLeftTicks = leftTicks;
+  long curRightTicks = rightTicks;
+  interrupts();
+
+  float dt = (curLoopTime - lastLoopTime) / 1E6;
+  
+  float leftSpeed = (curLeftTicks - lastLeftTicks) / dt;
+  float rightSpeed = (curRightTicks - lastRightTicks) / dt;
+
+  int leftControl = leftController.getControlValue(leftSpeed, dt);
+  leftMotorCmd += min(MAX_MOTOR_CMD, leftControl);
+  leftMotorCmd = constrain(leftMotorCmd, -MAX_MOTOR_CMD, MAX_MOTOR_CMD);
+  if (leftMotorCmd > 0) {
+    leftMotorCmd = max(leftMotorCmd, MIN_MOTOR_CMD);
+  }
+  
+  int rightControl = rightController.getControlValue(rightSpeed, dt);
+  rightMotorCmd += min(MAX_MOTOR_CMD, rightControl);
+  rightMotorCmd = constrain(rightMotorCmd, -MAX_MOTOR_CMD, MAX_MOTOR_CMD);
+  if (rightMotorCmd > 0) {
+    rightMotorCmd = max(rightMotorCmd, MIN_MOTOR_CMD);
+  }
+
+  // Coast to a stop if target is zero.
+  if (leftSpeedTarget == 0) {
+    leftMotorCmd = 0;
+  }
+  if (rightSpeedTarget == 0) {
+    rightMotorCmd = 0;
+  }
+  
+  setSpeed(leftMotorCmd, rightMotorCmd);
+
+  if (leftSpeedTarget > 0 || leftSpeed > 0 || rightSpeedTarget > 0 || rightSpeed > 0) {
+//    Serial.print(loopTime, 3);
+//    Serial.print("\t");
+    Serial.print(leftSpeedTarget);
+    Serial.print("\t");
+    Serial.print(leftSpeed);
+    Serial.print("\t");
+//    Serial.print(leftController.getCumulativeError());
+//    Serial.print("\t");
+//    Serial.print(leftMotorCmd);
+//    Serial.print("\t");
+  
+    Serial.print(rightSpeedTarget);
+    Serial.print("\t");
+    Serial.print(rightSpeed);
+    Serial.print("\t");
+//    Serial.print(rightController.getCumulativeError());
+//    Serial.print("\t");
+//    Serial.print(rightMotorCmd);
+//    Serial.print("\t");
+  
+    Serial.println();
+
+    loopTime += dt;
+}
+
+  // Ignore switches for a short time after pressing, to avoid bounce.
+  if (curLoopTime - lastSwitchTime > 0.5*1E6) {
+    int switchValue = readSwitch();
+    if (switchValue > 0) {
+      lastSwitchTime = curLoopTime;
+
+      switch (switchValue) {
+        case BUTTON_A:
+          // Left motor on/off.
+          leftSpeedTarget = (leftSpeedTarget==0 ? PID_TUNING_TICKS_PER_SEC : 0);
+          break;
+        case BUTTON_B:
+          // Right motor on/off.
+          rightSpeedTarget = (rightSpeedTarget==0 ? PID_TUNING_TICKS_PER_SEC : 0);
+          break;
+        case BUTTON_C:
+          // Both motors on/off.
+          leftSpeedTarget = (leftSpeedTarget==0 ? PID_TUNING_TICKS_PER_SEC : 0);
+          rightSpeedTarget = (rightSpeedTarget==0 ? PID_TUNING_TICKS_PER_SEC : 0);
+          break;
+        default:
+          // no change in speed
+          break;
+      }
+
+      leftController.setSetPoint(leftSpeedTarget);
+      rightController.setSetPoint(rightSpeedTarget);
+    }    
+  }
+
+  lastLeftTicks = curLeftTicks;
+  lastRightTicks = curRightTicks;
+  lastLoopTime = curLoopTime;
+}
+
+void waitForSwitch1() {
+  while (readSwitch() != 1) {
+    // do nothing
+  }
+}
+
+void setSpeed(int leftSpeed, int rightSpeed) {
+  motors.setSpeeds(leftSpeed, rightSpeed);
+}
+
+void leftAChange() {
+  if (digitalRead(M1_A) == digitalRead(M1_B)) {
+    ++leftTicks;
+  } else {
+    --leftTicks;
+  }
+}
+
+void leftBChange() {
+  if (digitalRead(M1_A) != digitalRead(M1_B)) {
+    ++leftTicks;
+  } else {
+    --leftTicks;
+  }
+}
+
+void rightAChange() {
+  if (digitalRead(M2_A) != digitalRead(M2_B)) {
+    ++rightTicks;
+  } else {
+    --rightTicks;
+  }
+}
+
+void rightBChange() {
+  if (digitalRead(M2_A) == digitalRead(M2_B)) {
+    ++rightTicks;
+  } else {
+    --rightTicks;
+  }
+}
+
+int readSwitch() {
+  if (buttonA.getSingleDebouncedRelease()) {
+    return BUTTON_A;
+  } else if (buttonB.getSingleDebouncedRelease()) {
+    return BUTTON_B;
+  } else if (buttonC.getSingleDebouncedRelease()) {
+    return BUTTON_C;
+  } else {
+    return 0;
+  }
+}

libraries/SimplePID-master/examples/RomeoPIDTest/RomeoPIDTest.ino

@@ -0,0 +1,294 @@
+// RomeoPIDTest - Test the PID control for a DFRobot Romeo-based robot.
+//
+// To gather PID tuning data for graphing, put the robot on blocks, then
+// start this program and open a Serial Monitor. Press S4 to start both
+// motors, let them come to a steady speed, then press S4 to stop the
+// motors. Copy the Serial output to a spreadsheet and plot target speed and
+// current speed versus time. You should see a classic PID controller graph
+// with some overshoot.
+
+#include <EnableInterrupt.h>
+#include <SimplePID.h>
+
+// Motor parameters. These are based on a Pololu #2285 motor with 48cpr
+// encoder.
+const float GEAR_RATIO = 46.85;
+const float ENCODER_CPR = 48;
+
+// Number of encoder ticks per revolution of the wheel.
+const float TICKS_PER_REV = GEAR_RATIO * ENCODER_CPR;
+
+// Maximum desired motor speed. Pololu #2285 are rated for 130rpm, but we
+// don't drive them with enough voltage to achieve that.
+const float MAX_REVS_PER_SEC = 100.0 / 60.0;
+
+// Use 60% of maximum speed for PID tuning.
+const int PID_TUNING_TICKS_PER_SEC = 0.6 * MAX_REVS_PER_SEC * TICKS_PER_REV;
+
+// DFRobot Romeo v2 and BLE PIN assignments.
+
+const int ONBOARD_SWITCH_PIN = A0;
+
+const int LEFT_DIRECTION = 4;
+const int LEFT_SPEED = 5;
+const int RIGHT_SPEED = 6;
+const int RIGHT_DIRECTION = 7;
+
+// Pins for the Pololu motor encoder outputs.
+const int M1_A = 10;
+const int M1_B = 11;
+const int M2_A = 14;
+const int M2_B = 15;
+
+// Minimum motor control value. Motor output below this will stall.
+const int MIN_MOTOR_CMD = 60;
+
+int leftSpeedTarget = 0;
+int rightSpeedTarget = 0;
+
+volatile unsigned long leftTicks = 0;
+volatile unsigned long lastLeftTickTime = 0;
+volatile unsigned long rightTicks = 0;
+volatile unsigned long lastRightTickTime = 0;
+
+int lastLeftTicks = 0;
+int lastRightTicks = 0;
+
+int leftMotorCmd = 0;
+int rightMotorCmd = 0;
+
+unsigned long lastLoopTime;
+
+unsigned long lastSwitchTime;
+
+float loopTime = 0.0;
+
+// Ziegler-Nichols tuning. See this Wikipedia article for details:
+//     https://en.wikipedia.org/wiki/PID_controller#Loop_tuning
+// To tune the controller, set USE_KU_ONLY to 1 and increase Ku
+// until oscillation occurs, and set Tu to the oscillation period
+// in seconds. Then set USE_KU_ONLY to zero to use the
+// Ziegler-Nichols tune. To get a less agressive tune, set LESS_AGGRESSIVE
+// to 1.
+
+// Set to 1 to use Ku only, to determine oscillation point.
+#define USE_KU_ONLY 0
+
+const float Ku = .11;
+const float Tu = .20;
+
+#define LESS_AGGRESSIVE 0
+
+#if !LESS_AGGRESSIVE
+const float Kp = 0.6*Ku;
+const float Ki = 2*Kp/Tu;
+const float Kd = Kp*Tu/8;
+#else
+const float Kp = 0.2*Ku;
+const float Ki = 2*Kp/Tu;
+const float Kd = Kp*Tu/3;
+#endif
+
+#if USE_KU_ONLY
+SimplePID leftController = SimplePID(Ku, 0, 0);
+SimplePID rightController = SimplePID(Ku, 0, 0);
+#else
+SimplePID leftController = SimplePID(Kp, Ki, Kd);
+SimplePID rightController = SimplePID(Kp, Ki, Kd);
+#endif
+
+// Sets up the serial output and the motor control pins, and attaches
+// interrupt handlers to the pins on which we will read the encoder
+// phototransistor values.
+void setup() {
+  Serial.begin(115200);
+  delay(3000);
+
+  pinMode(LEFT_DIRECTION, OUTPUT);
+  pinMode(LEFT_SPEED, OUTPUT);
+  pinMode(RIGHT_DIRECTION, OUTPUT);
+  pinMode(RIGHT_SPEED, OUTPUT);
+  
+  enableInterrupt(M1_A, leftAChange, CHANGE);
+  enableInterrupt(M1_B, leftBChange, CHANGE);
+  enableInterrupt(M2_A, rightAChange, CHANGE);
+  enableInterrupt(M2_B, rightBChange, CHANGE);
+
+  Serial.print("Time (s)\tLeft Target\tLeft Speed\tLeft Cum Error\tLeft Motor");
+  Serial.print("\tRight Target\tRight Speed\tRight Cum Error\tRight Motor");
+  Serial.println();
+
+  lastLoopTime = micros();
+  lastSwitchTime = millis();
+}
+
+// Loops forever showing the motor speeds and errors since the last loop,
+// and adjusts the target speeds depending on whether the user is pressing
+// switches S2 through S5, as indicated in the code below.
+void loop() {
+  delay(15);
+
+  unsigned long curLoopTime = micros();
+  noInterrupts();
+  int curLeftTicks = leftTicks;
+  int curRightTicks = rightTicks;
+  interrupts();
+
+  float dt = (curLoopTime - lastLoopTime) / 1E6;
+  
+  float leftSpeed = (curLeftTicks - lastLeftTicks) / dt;
+  float rightSpeed = (curRightTicks - lastRightTicks) / dt;
+
+  int leftControl = leftController.getControlValue(leftSpeed, dt);
+  leftMotorCmd += min(255, leftControl);
+  leftMotorCmd = constrain(leftMotorCmd, -255, 255);
+  if (leftMotorCmd > 0) {
+    leftMotorCmd = max(leftMotorCmd, MIN_MOTOR_CMD);
+  }
+  
+  int rightControl = rightController.getControlValue(rightSpeed, dt);
+  rightMotorCmd += min(255, rightControl);
+  rightMotorCmd = constrain(rightMotorCmd, -255, 255);
+  if (rightMotorCmd > 0) {
+    rightMotorCmd = max(rightMotorCmd, MIN_MOTOR_CMD);
+  }
+
+  // Coast to a stop if target is zero.
+  if (leftSpeedTarget == 0) {
+    leftMotorCmd = 0;
+  }
+  if (rightSpeedTarget == 0) {
+    rightMotorCmd = 0;
+  }
+  
+  setSpeed(leftMotorCmd, rightMotorCmd);
+
+  if (leftSpeedTarget > 0 || leftSpeed > 0 || rightSpeedTarget > 0 || rightSpeed > 0) {
+    Serial.print(loopTime, 3);
+    Serial.print("\t");
+    Serial.print(leftSpeedTarget);
+    Serial.print("\t");
+    Serial.print(leftSpeed);
+    Serial.print("\t");
+    Serial.print(leftController.getCumulativeError());
+    Serial.print("\t");
+    Serial.print(leftMotorCmd);
+    Serial.print("\t");
+  
+    Serial.print(rightSpeedTarget);
+    Serial.print("\t");
+    Serial.print(rightSpeed);
+    Serial.print("\t");
+    Serial.print(rightController.getCumulativeError());
+    Serial.print("\t");
+    Serial.print(rightMotorCmd);
+    Serial.print("\t");
+  
+    Serial.println();
+
+    loopTime += dt;
+}
+
+  // Ignore switches for a short time after pressing, to avoid bounce.
+  if (curLoopTime - lastSwitchTime > 0.5*1E6) {
+    int switchValue = readSwitch();
+    if (switchValue > 0) {
+      lastSwitchTime = curLoopTime;
+
+      switch (switchValue) {
+        case 2:
+          // Left motor on/off.
+          leftSpeedTarget = (leftSpeedTarget==0 ? PID_TUNING_TICKS_PER_SEC : 0);
+          break;
+        case 3:
+          // Right motor on/off.
+          rightSpeedTarget = (rightSpeedTarget==0 ? PID_TUNING_TICKS_PER_SEC : 0);
+          break;
+        case 4:
+          // Both motors on/off.
+          leftSpeedTarget = (leftSpeedTarget==0 ? PID_TUNING_TICKS_PER_SEC : 0);
+          rightSpeedTarget = (rightSpeedTarget==0 ? PID_TUNING_TICKS_PER_SEC : 0);
+          break;
+        case 5:
+          break;
+        default:
+          // no change in speed
+          break;
+      }
+
+      leftController.setSetPoint(leftSpeedTarget);
+      rightController.setSetPoint(rightSpeedTarget);
+    }    
+  }
+
+  lastLeftTicks = curLeftTicks;
+  lastRightTicks = curRightTicks;
+  lastLoopTime = curLoopTime;
+}
+
+void waitForSwitch1() {
+  while (readSwitch() != 1) {
+    // do nothing
+  }
+}
+
+void setSpeed(int leftSpeed, int rightSpeed) {
+  digitalWrite(LEFT_DIRECTION, (leftSpeed >= 0 ? HIGH : LOW));
+  analogWrite(LEFT_SPEED, abs(leftSpeed));
+  digitalWrite(RIGHT_DIRECTION, (rightSpeed >= 0 ? HIGH : LOW));
+  analogWrite(RIGHT_SPEED, abs(rightSpeed));
+}
+
+void leftAChange() {
+  if (digitalRead(M1_A) == digitalRead(M1_B)) {
+    ++leftTicks;
+  } else {
+    --leftTicks;
+  }
+}
+
+void leftBChange() {
+  if (digitalRead(M1_A) != digitalRead(M1_B)) {
+    ++leftTicks;
+  } else {
+    --leftTicks;
+  }
+}
+
+void rightAChange() {
+  if (digitalRead(M2_A) != digitalRead(M2_B)) {
+    ++rightTicks;
+  } else {
+    --rightTicks;
+  }
+}
+
+void rightBChange() {
+  if (digitalRead(M2_A) == digitalRead(M2_B)) {
+    ++rightTicks;
+  } else {
+    --rightTicks;
+  }
+}
+
+// Reads the value of the built-in switches S1 through S5. The switches
+// are connected in parallel with resistors of five different sizes to
+// analog pin A0. The difference in voltage at A0 allows us to determine
+// which switch is pressed.
+int readSwitch() {
+  int value = analogRead(ONBOARD_SWITCH_PIN);
+
+  if (value > 800) {
+    return 0;
+  } else if (value > 600) {
+    return 5;
+  } else if (value > 400) {
+    return 4;
+  } else if (value > 250) {
+    return 3;
+  } else if (value > 75) {
+    return 2;
+  } else {
+    return 1;
+  }
+}