Last active
May 27, 2016 03:55
-
-
Save Lauszus/3f727b1fa3ca692a8bdb to your computer and use it in GitHub Desktop.
BalancingRobotFullSize version for Sabertooth in simplified serial mode
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
/* 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 | |
} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
/* 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); | |
} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
/* 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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
/* 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; | |
} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
/* 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 |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
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