BalancingRobotFullSize version for Sabertooth in simplified serial mode

BalancingRobotFullSize.ino

/* Copyright (C) 2014 Kristian Lauszus, TKJ Electronics. All rights reserved.

 This software may be distributed and modified under the terms of the GNU
 General Public License version 2 (GPL2) as published by the Free Software
 Foundation and appearing in the file GPL2.TXT included in the packaging of
 this file. Please note that GPL2 Section 2[b] requires that all works based
 on this software must also be made publicly available under the terms of
 the GPL2 ("Copyleft").

 Contact information
 -------------------

 Kristian Lauszus, TKJ Electronics
 Web      :  http://www.tkjelectronics.com
 e-mail   :  kristianl@tkjelectronics.com
*/

#include <Arduino.h>
#include <Wire.h> // Standard Arduino I2C library
#include <Kalman.h>
#include <SoftwareSerial.h>
#include <SabertoothSimplified.h>

#include "BalancingRobotFullSize.h"
#include "Pins.h"
#include "Motor.h"
#include "IMU.h"
#include "EEPROM.h"
#include "Protocol.h"
#include "PID.h"

#if defined(__AVR_ATmega328P__) && NUM_ANALOG_INPUTS != 8
#error "Please update the Arduino IDE to version 1.5.7 or above - see: https://github.com/arduino/Arduino/pull/2148"
#endif

double turningValue; // The turning value of the steering rod
uint16_t batteryLevel; // Battery level multiplied by 100 i.e. 24.50V becomes 2450

uint32_t kalmanTimer; // Timer used for the Kalman filter
static uint32_t pidTimer; // Timer used for the PID loop

static bool layingDown; // Used to indicate if the robot is laying down or the button is pressed
static double zeroTurning; // Used for calibration of the steer at startup
static uint8_t batteryCounter;

double getTurning() {
  double turning = (double)analogRead(STEER_PIN) / 204.6 - 2.5; // First convert reading to voltage and then subtract 2.5V, as this is the center of the steering wheel
  turning *= cfg.turningScale; // Scale the turning value, so it will actually turn
  //Serial.println(turning);
  return turning;
}

void setup() {
  /* Setup deadman button */
  deadmanButton::SetDirRead();

  /* Setup buzzer pin */
  buzzer::SetDirWrite();

  /* Read the PID values, target angle and other saved values in the EEPROM */
  if (!checkEEPROMVersion())
    readEEPROMValues(); // Only read the EEPROM values if they have not been restored
  else { // Indicate that the EEPROM values have been reset by turning on the buzzer
    buzzer::Set();
    delay(1000);
    buzzer::Clear();
    delay(100); // Wait a little after the pin is cleared
  }

  initSerial();
  initMotors();
  initIMU();

  zeroTurning = getTurning(); // Calibrate turning

  /* Beep to indicate that it is now ready */
  buzzer::Set();
  delay(100);
  buzzer::Clear();

  /* Setup timing */
  kalmanTimer = micros();
  pidTimer = kalmanTimer;
}

void loop () {
#if 0
  if (leftF1::IsSet() || leftF2::IsSet() || rightF1::IsSet() || rightF2::IsSet()) { // Check the diagnostic pins
    buzzer::Set();
    Serial.print(F("Diagnostic error: "));
    Serial.print(leftF1::IsSet());
    Serial.write('\t');
    Serial.print(leftF2::IsSet());
    Serial.write('\t');
    Serial.print(rightF1::IsSet());
    Serial.write('\t');
    Serial.println(rightF2::IsSet());
  }
#endif

  if (dataReady::IsSet()) { // Check is new data is ready
    updateAngle();

    turningValue = getTurning() - zeroTurning; // Update turning value

    /* Drive motors */
    uint32_t timer = micros();
    // If the robot is laying down, it has to be put in a vertical position before it starts balancing
    // If it's already balancing it has to be ±45 degrees before it stops trying to balance
    // Also make sure that the deadman buttons is pressed
    if (!deadmanButton::IsSet() || (layingDown && (pitch < cfg.targetAngle - 5 || pitch > cfg.targetAngle + 5)) || (!layingDown && (pitch < cfg.targetAngle - 45 || pitch > cfg.targetAngle + 45))) {
      layingDown = true; // The robot is in a unsolvable position, so turn off both motors and wait until it's vertical again
      stopAndReset();
    } else {
      layingDown = false; // It's no longer laying down
      updatePID(cfg.targetAngle, 0 /*targetOffset*/, turningValue, (double)(timer - pidTimer) / 1000000.0);
    }
    pidTimer = timer;

    if (++batteryCounter > 100) {
      batteryCounter = 0;
      batteryLevel = (double)analogRead(VBAT_PIN) / 204.6 * 780.0; // It is connected to a 68k-10k voltage divider and then we multiply this by 100, so 12.50V will be equal to 1250 - the voltage divider is connected to an op amp configured as a buffer
      if (batteryLevel < 2160 && batteryLevel > 500) // Equal to 3.6V per cell (21.60V in total) - don't turn on if it's below 5V, this means that no battery is connected
        buzzer::Set();
      else
        buzzer::Clear();
      // TODO: Calibrate this value
    }
  } else
    parseSerialData(); // Parse incoming serial data
}

Motor.cpp

/* Copyright (C) 2014 Kristian Lauszus, TKJ Electronics. All rights reserved.

 This software may be distributed and modified under the terms of the GNU
 General Public License version 2 (GPL2) as published by the Free Software
 Foundation and appearing in the file GPL2.TXT included in the packaging of
 this file. Please note that GPL2 Section 2[b] requires that all works based
 on this software must also be made publicly available under the terms of
 the GPL2 ("Copyleft").

 Contact information
 -------------------

 Kristian Lauszus, TKJ Electronics
 Web      :  http://www.tkjelectronics.com
 e-mail   :  kristianl@tkjelectronics.com
*/

#include <SoftwareSerial.h>
#include <SabertoothSimplified.h>

#include "Motor.h"
#include "Pins.h"

SoftwareSerial SoftSerial(NOT_A_PIN, 11); // RX on no pin (unused), TX on pin 11 (to S1)
SabertoothSimplified ST(SoftSerial); // Use SoftwareSerial as the serial port

void setPWM(Command motor, int power);

void initMotors() {
  SoftSerial.begin(38400);
  stopMotors();
}

void moveMotor(Command motor, Command direction, double speedRaw) { // Speed is a value in percentage 0-100%
  if (speedRaw > 100)
    speedRaw = 100.0;
  if (direction == forward)
    setPWM(motor, map(speedRaw, 0, 100, 0, 127)); // Scale from [0:100] to [0:127]
  else
    setPWM(motor, map(speedRaw, 0, 100, 0, -127)); // Scale from [0:100] to [0:-127]
}

void stopMotors() {
  ST.stop();
}

void setPWM(Command motor, int power) { // Power is a value between [-127:127] where -127 is full backward and 127 is full forward
  if (motor == left)
    ST.motor(1, power);
  else
    ST.motor(2, power);
}

Motor.h

/* Copyright (C) 2014 Kristian Lauszus, TKJ Electronics. All rights reserved.

 This software may be distributed and modified under the terms of the GNU
 General Public License version 2 (GPL2) as published by the Free Software
 Foundation and appearing in the file GPL2.TXT included in the packaging of
 this file. Please note that GPL2 Section 2[b] requires that all works based
 on this software must also be made publicly available under the terms of
 the GPL2 ("Copyleft").

 Contact information
 -------------------

 Kristian Lauszus, TKJ Electronics
 Web      :  http://www.tkjelectronics.com
 e-mail   :  kristianl@tkjelectronics.com
*/

#ifndef _motor_h_
#define _motor_h_

enum Command {
  forward,
  backward,
  left,
  right,
};

void initMotors();
void moveMotor(Command motor, Command direction, double speedRaw);
void stopMotors();

#endif

patch.diff

diff --git a/BalancingRobotFullSize.ino b/BalancingRobotFullSize.ino
index e305178..fca29b1 100644
--- a/BalancingRobotFullSize.ino
+++ b/BalancingRobotFullSize.ino
@@ -18,6 +18,8 @@
 #include <Arduino.h>
 #include <Wire.h> // Standard Arduino I2C library
 #include <Kalman.h>
+#include <SoftwareSerial.h>
+#include <SabertoothSimplified.h>
 
 #include "BalancingRobotFullSize.h"
 #include "Pins.h"
diff --git a/Motor.cpp b/Motor.cpp
index 2b7ac55..89e105e 100644
--- a/Motor.cpp
+++ b/Motor.cpp
@@ -15,65 +15,38 @@
  e-mail   :  kristianl@tkjelectronics.com
 */
 
+#include <SoftwareSerial.h>
+#include <SabertoothSimplified.h>
+
 #include "Motor.h"
 #include "Pins.h"
 
-static const uint16_t PWM_FREQUENCY = 20000; // Set the PWM frequency to 20kHz
-static const uint16_t PWMVALUE = F_CPU / PWM_FREQUENCY / 2; // The frequency is given by F_CPU/(2*N*ICR) - where N is the prescaler, prescaling is used so the frequency is given by F_CPU/(2*ICR) - ICR = F_CPU/PWM_FREQUENCY/2
+SoftwareSerial SoftSerial(NOT_A_PIN, 11); // RX on no pin (unused), TX on pin 11 (to S1)
+SabertoothSimplified ST(SoftSerial); // Use SoftwareSerial as the serial port
 
-void setPWM(Command motor, uint16_t dutyCycle);
+void setPWM(Command motor, int power);
 
 void initMotors() {
-  /* Set the motor driver diagnostic pins to inputs */
-  leftF1::SetDirRead();
-  leftF2::SetDirRead();
-  rightF1::SetDirRead();
-  rightF2::SetDirRead();
-
-  /* Setup motor pins to output */
-  leftPWM::SetDirWrite();
-  leftDir::SetDirWrite();
-  rightPWM::SetDirWrite();
-  rightDir::SetDirWrite();
-
-  /* Set PWM frequency to 20kHz - see the datasheet http://www.atmel.com/Images/Atmel-8271-8-bit-AVR-Microcontroller-ATmega48A-48PA-88A-88PA-168A-168PA-328-328P_datasheet_Complete.pdf page 128-138 */
-  // Set up PWM, Phase and Frequency Correct on pin 9 (OC1A) & pin 10 (OC1B) with ICR1 as TOP using Timer1
-  TCCR1B = (1 << WGM13) | (1 << CS10); // Set PWM Phase and Frequency Correct with ICR1 as TOP and no prescaling
-  ICR1 = PWMVALUE; // ICR1 is the TOP value - this is set so the frequency is equal to 20kHz
-
-  /* Enable PWM on pin 9 (OC1A) & pin 10 (OC1B) */
-  // Clear OC1A/OC1B on compare match when up-counting
-  // Set OC1A/OC1B on compare match when down-counting
-  TCCR1A = (1 << COM1A1) | (1 << COM1B1);
-
-  stopMotor(left);
-  stopMotor(right);
+  SoftSerial.begin(38400);
+  stopMotors();
 }
 
 void moveMotor(Command motor, Command direction, double speedRaw) { // Speed is a value in percentage 0-100%
   if (speedRaw > 100)
     speedRaw = 100.0;
-  setPWM(motor, speedRaw * ((double)PWMVALUE) / 100.0); // Scale from 0-100 to 0-PWMVALUE
-  if (motor == left) {
-    if (direction == forward)
-      leftDir::Clear();
-    else
-      leftDir::Set();
-  } else {
-    if (direction == forward)
-      rightDir::Set();
-    else
-      rightDir::Clear();
-  }
+  if (direction == forward)
+    setPWM(motor, map(speedRaw, 0, 100, 0, 127)); // Scale from [0:100] to [0:127]
+  else
+    setPWM(motor, map(speedRaw, 0, 100, 0, -127)); // Scale from [0:100] to [0:-127]
 }
 
-void stopMotor(Command motor) {
-  setPWM(motor, 0); // Set low
+void stopMotors() {
+  ST.stop();
 }
 
-void setPWM(Command motor, uint16_t dutyCycle) { // dutyCycle is a value between 0-ICR1
+void setPWM(Command motor, int power) { // Power is a value between [-127:127] where -127 is full backward and 127 is full forward
   if (motor == left)
-    leftPWMReg = dutyCycle;
+    ST.motor(1, power);
   else
-    rightPWMReg = dutyCycle;
+    ST.motor(2, power);
 }
diff --git a/Motor.h b/Motor.h
index 964327e..39b6419 100644
--- a/Motor.h
+++ b/Motor.h
@@ -27,6 +27,6 @@ enum Command {
 
 void initMotors();
 void moveMotor(Command motor, Command direction, double speedRaw);
-void stopMotor(Command motor);
+void stopMotors();
 
 #endif
diff --git a/PID.cpp b/PID.cpp
index 537ff72..3ae5e1d 100644
--- a/PID.cpp
+++ b/PID.cpp
@@ -84,8 +84,7 @@ void updatePID(double restAngle, double offset, double turning, double dt) {
 
 void stopAndReset() {
   PIDValue = 0;
-  stopMotor(left);
-  stopMotor(right);
+  stopMotors();
   lastError = 0;
   integratedError = 0;
   currentSpeed = 0;
diff --git a/Pins.h b/Pins.h
index 92a006c..f5c010f 100644
--- a/Pins.h
+++ b/Pins.h
@@ -31,23 +31,6 @@
 #define buzzer P7 // Buzzer used for audio feedback
 #define deadmanButton P16 // (A2) Deadman button
 
-/* Left motor */
-#define leftDir P12
-#define leftPWM P10
-#define leftPWMReg OCR1B
-
-/* Right motor */
-#define rightDir P8
-#define rightPWM P9
-#define rightPWMReg OCR1A
-
-/* Pins connected to the motor drivers diagnostic pins */
-#define leftF1 P5
-#define leftF2 P6
-
-#define rightF1 P3
-#define rightF2 P4
-
 /* Analog pins */
 #define STEER_PIN A0
 #define VBAT_PIN  A1

PID.cpp

/* Copyright (C) 2014 Kristian Lauszus, TKJ Electronics. All rights reserved.

 This software may be distributed and modified under the terms of the GNU
 General Public License version 2 (GPL2) as published by the Free Software
 Foundation and appearing in the file GPL2.TXT included in the packaging of
 this file. Please note that GPL2 Section 2[b] requires that all works based
 on this software must also be made publicly available under the terms of
 the GPL2 ("Copyleft").

 Contact information
 -------------------

 Kristian Lauszus, TKJ Electronics
 Web      :  http://www.tkjelectronics.com
 e-mail   :  kristianl@tkjelectronics.com
*/

#include <Arduino.h>

#include "PID.h"
#include "IMU.h"
#include "Motor.h"
#include "EEPROM.h"

double PIDValue; // Calculated PID value

static double lastError; // Store last angle error
static double integratedError; // Store integrated error

static double currentSpeed; // Estimated speed from PWM value

void updatePID(double restAngle, double offset, double turning, double dt) {
  /* Update PID values */
  double error = (restAngle - pitch);
  double pTerm = cfg.Kp * error;
  integratedError += error * dt;
  //integratedError = constrain(integratedError, -1.0, 1.0); // Limit the integrated error
  double iTerm = (cfg.Ki * 100.0) * integratedError;
  double dTerm = (cfg.Kd / 100.0) * (error - lastError) / dt;
  lastError = error;
  PIDValue = pTerm + iTerm + dTerm;

  currentSpeed = (currentSpeed + PIDValue * 0.004) * 0.999; // TODO: Explain this
  //currentSpeed = constrain(currentSpeed, -50, 50);

  //Serial.print(PIDValue); Serial.write('\t'); Serial.print(currentSpeed); Serial.write('\t');

  PIDValue += currentSpeed;

  //Serial.println(PIDValue);

#if 0 // TODO: Estimate velocity and steer
  /* Steer robot sideways */
  if (turning < 0) { // Left
    turning += abs((double)wheelVelocity / velocityScaleTurning); // Scale down at high speed
    if (turning > 0)
      turning = 0;
  } else if (turning > 0) { // Right
    turning -= abs((double)wheelVelocity / velocityScaleTurning); // Scale down at high speed
    if (turning < 0)
      turning = 0;
  }
#endif

  // TODO: Turn opposite when going backwards
  double PIDLeft = PIDValue - turning;
  double PIDRight = PIDValue + turning;

  PIDLeft *= cfg.leftMotorScaler; // Compensate for difference in some of the motors
  PIDRight *= cfg.rightMotorScaler;

  //Serial.print(PIDLeft); Serial.write('\t'); Serial.println(PIDRight);

  /* Set PWM Values */
  if (PIDLeft >= 0)
    moveMotor(left, forward, PIDLeft);
  else
    moveMotor(left, backward, -PIDLeft);
  if (PIDRight >= 0)
    moveMotor(right, forward, PIDRight);
  else
    moveMotor(right, backward, -PIDRight);
}

void stopAndReset() {
  PIDValue = 0;
  stopMotors();
  lastError = 0;
  integratedError = 0;
  currentSpeed = 0;
}

Pins.h

/* Copyright (C) 2014 Kristian Lauszus, TKJ Electronics. All rights reserved.

 This software may be distributed and modified under the terms of the GNU
 General Public License version 2 (GPL2) as published by the Free Software
 Foundation and appearing in the file GPL2.TXT included in the packaging of
 this file. Please note that GPL2 Section 2[b] requires that all works based
 on this software must also be made publicly available under the terms of
 the GPL2 ("Copyleft").

 Contact information
 -------------------

 Kristian Lauszus, TKJ Electronics
 Web      :  http://www.tkjelectronics.com
 e-mail   :  kristianl@tkjelectronics.com
*/

#ifndef _pins_h_
#define _pins_h_

// I use the pins defined in avrpins.h from the USB Host library which I am also one of the developers of
// This allows to read and write directly to the port registers instead of using Arduino's slow digitalRead()/digitalWrite() functions
// The original source is available here: https://github.com/felis/USB_Host_Shield_2.0/blob/master/avrpins.h
// I do this to save processing power - see this page for more information: http://www.billporter.info/ready-set-oscillate-the-fastest-way-to-change-arduino-pins/
// Also see the Arduino port manipulation guide: http://www.arduino.cc/en/Reference/PortManipulation
#include "avrpins.h"

/* Pin connect to INT on the MPU-6050 */
#define dataReady P2

#define buzzer P7 // Buzzer used for audio feedback
#define deadmanButton P16 // (A2) Deadman button

/* Analog pins */
#define STEER_PIN A0
#define VBAT_PIN  A1
#define CS1_PIN   A6
#define CS2_PIN   A7

#endif

So if I use this code with the Balanduino which files do I need to switch out to work with the PS3 / PS4 remote. Any help would be great.

Thanks,
Scott