Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
BalancingRobotFullSize version for Sabertooth in simplified serial mode
/* 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
}
/* 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);
}
/* 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
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
/* 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;
}
/* 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
@415porto

This comment has been minimized.

Copy link

@415porto 415porto commented May 27, 2016

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

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment