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

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
You can’t perform that action at this time.