Permalink
Branch: master
Find file Copy path
Fetching contributors…
Cannot retrieve contributors at this time
380 lines (340 sloc) 11.7 KB
/* Copyright (C) 2014 Kristian Sloth Lauszus. 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 Sloth Lauszus
Web : http://www.lauszus.com
e-mail : lauszus@gmail.com
*/
#include <Arduino.h>
#include "Protocol.h"
#include "BalancingRobotFullSize.h"
#include "IMU.h"
#include "EEPROM.h"
#include "PID.h"
#include "Pins.h"
struct msg_t {
uint8_t cmd;
uint8_t length;
} __attribute__((packed)) msg;
struct pid_t {
uint16_t Kp;
uint16_t Ki;
uint16_t Kd;
} __attribute__((packed)) pid;
struct target_t {
int16_t targetAngle; // Note that this can be negative as well
} __attribute__((packed)) target;
struct turning_t {
uint8_t turningScale;
} __attribute__((packed)) turning;
struct kalman_t {
uint16_t Qangle;
uint16_t Qbias;
uint16_t Rmeasure;
} __attribute__((packed)) kalman;
struct info_t {
uint16_t speed;
int16_t current; // Note that this can be negative as well
int16_t turning; // Note that this can be negative as well
uint16_t battery;
uint32_t runTime;
} __attribute__((packed)) info;
struct imu_t {
uint16_t acc;
uint16_t gyro;
uint16_t kalman;
} __attribute__((packed)) imu;
#define SET_PID 0
#define GET_PID 1
#define SET_TARGET 2
#define GET_TARGET 3
#define SET_TURNING 4
#define GET_TURNING 5
#define SET_KALMAN 6
#define GET_KALMAN 7
#define START_INFO 8
#define STOP_INFO 9
#define START_IMU 10
#define STOP_IMU 11
const char *commandHeader = "$S>"; // Standard command header
const char *responseHeader = "$S<"; // Standard response header
static bool sendSpeed, sendImu;
static uint32_t speedTimer, imuTimer;
bool getData(uint8_t *data, uint8_t length);
void sendData(uint8_t *data, uint8_t length);
uint8_t getCheckSum(uint8_t *data, size_t length);
void initSerial() {
// Automatically configure the Bluetooth module the first time it is turned on
if (cfg.configureBtModule) {
cfg.configureBtModule = false;
updateEEPROMValues();
Serial.begin(9600);
Serial.println("AT+NAMESegway"); // Set the name to 'Segway'
delay(1000); // The module waits 1 s before it considers a message complete
Serial.println("AT+PIN0000"); // Set the pin to '0000'
delay(1000); // The module waits 1 s before it considers a message complete
Serial.println("AT+BAUD7"); // Finally change the baud rate to '57600'
}
Serial.begin(57600);
Serial.setTimeout(10); // Only wait 10ms for serial data
}
// TODO: Remove all debugging
void parseSerialData() {
if (Serial.available()) {
if (Serial.find((char*)commandHeader)) {
if (Serial.readBytes((uint8_t*)&msg, sizeof(msg)) == sizeof(msg)) {
switch (msg.cmd) {
case SET_PID:
if (msg.length == sizeof(pid)) { // Make sure that it has the right length
if (getData((uint8_t*)&pid, sizeof(pid))) { // This will read the data and check the checksum
cfg.Kp = (double)pid.Kp / 100.0;
cfg.Ki = (double)pid.Ki / 100.0;
cfg.Kd = (double)pid.Kd / 100.0;
updateEEPROMValues();
}
#if DEBUG
else
Serial.println(F("SET_PID checksum error"));
#endif
}
#if DEBUG
else {
Serial.print(F("SET_PID length error: "));
Serial.println(msg.length);
}
#endif
break;
case GET_PID:
if (msg.length == 0) {
if (getData(NULL, 0)) { // This will read the data and check the checksum
msg.cmd = GET_PID;
msg.length = sizeof(pid);
pid.Kp = cfg.Kp * 100.0;
pid.Ki = cfg.Ki * 100.0;
pid.Kd = cfg.Kd * 100.0;
sendData((uint8_t*)&pid, sizeof(pid));
}
#if DEBUG
else
Serial.println(F("GET_PID checksum error"));
#endif
}
break;
case SET_TARGET:
if (msg.length == sizeof(target)) { // Make sure that it has the right length
if (getData((uint8_t*)&target, sizeof(target))) { // This will read the data and check the checksum
cfg.targetAngle = (double)target.targetAngle / 100.0;
updateEEPROMValues();
}
#if DEBUG
else
Serial.println(F("SET_TARGET checksum error"));
#endif
}
#if DEBUG
else {
Serial.print(F("SET_TARGET length error: "));
Serial.println(msg.length);
}
#endif
break;
case GET_TARGET:
if (msg.length == 0) {
if (getData(NULL, 0)) { // This will read the data and check the checksum
msg.cmd = GET_TARGET;
msg.length = sizeof(target);
target.targetAngle = cfg.targetAngle * 100.0;
sendData((uint8_t*)&target, sizeof(target));
}
#if DEBUG
else
Serial.println(F("GET_PID checksum error"));
#endif
}
break;
case SET_TURNING:
if (msg.length == sizeof(turning)) { // Make sure that it has the right length
if (getData((uint8_t*)&turning, sizeof(turning))) { // This will read the data and check the checksum
cfg.turningScale = turning.turningScale;
updateEEPROMValues();
}
#if DEBUG
else
Serial.println(F("SET_TURNING checksum error"));
#endif
}
#if DEBUG
else {
Serial.print(F("SET_TURNING length error: "));
Serial.println(msg.length);
}
#endif
break;
case GET_TURNING:
if (msg.length == 0) {
if (getData(NULL, 0)) { // This will read the data and check the checksum
msg.cmd = GET_TURNING;
msg.length = sizeof(turning);
turning.turningScale = cfg.turningScale;
sendData((uint8_t*)&turning, sizeof(turning));
}
#if DEBUG
else
Serial.println(F("GET_TURNING checksum error"));
#endif
}
break;
case SET_KALMAN:
if (msg.length == sizeof(kalman)) { // Make sure that it has the right length
if (getData((uint8_t*)&kalman, sizeof(kalman))) { // This will read the data and check the checksum
cfg.Qangle = kalman.Qangle / 10000.0;
cfg.Qbias = kalman.Qbias / 10000.0;
cfg.Rmeasure = kalman.Rmeasure / 10000.0;
updateEEPROMValues();
}
#if DEBUG
else
Serial.println(F("SET_KALMAN checksum error"));
#endif
}
#if DEBUG
else {
Serial.print(F("SET_KALMAN length error: "));
Serial.println(msg.length);
}
#endif
break;
case GET_KALMAN:
if (msg.length == 0) {
if (getData(NULL, 0)) { // This will read the data and check the checksum
msg.cmd = GET_KALMAN;
msg.length = sizeof(kalman);
kalman.Qangle = cfg.Qangle * 10000.0;
kalman.Qbias = cfg.Qbias * 10000.0;
kalman.Rmeasure = cfg.Rmeasure * 10000.0;
sendData((uint8_t*)&kalman, sizeof(kalman));
}
#if DEBUG
else
Serial.println(F("GET_KALMAN checksum error"));
#endif
}
break;
case START_INFO:
if (msg.length == 0) {
if (getData(NULL, 0)) // This will read the data and check the checksum
sendSpeed = true;
#if DEBUG
else
Serial.println(F("START_INFO checksum error"));
#endif
}
break;
case STOP_INFO:
if (msg.length == 0) {
if (getData(NULL, 0)) // This will read the data and check the checksum
sendSpeed = false;
#if DEBUG
else
Serial.println(F("STOP_INFO checksum error"));
#endif
}
break;
case START_IMU:
if (msg.length == 0) {
if (getData(NULL, 0)) // This will read the data and check the checksum
sendImu = true;
#if DEBUG
else
Serial.println(F("START_IMU checksum error"));
#endif
}
break;
case STOP_IMU:
if (msg.length == 0) {
if (getData(NULL, 0)) // This will read the data and check the checksum
sendImu = false;
#if DEBUG
else
Serial.println(F("STOP_IMU checksum error"));
#endif
}
break;
#if DEBUG
default:
Serial.print(F("Unknown command: "));
Serial.println(msg.cmd);
break;
#endif
}
}
}
}
if (sendSpeed && millis() - speedTimer > 100) {
speedTimer = millis();
msg.cmd = START_INFO;
msg.length = sizeof(info);
info.speed = constrain(abs(PIDValue), 0, 100.0) * 100.0;
if (deadmanButton::IsSet()) {
double CS = ((double)analogRead(CS1_PIN) / 204.6 - 2.5) / 0.066 * 100.0; // 66mV/A and then multiply by 100.0
CS -= ((double)analogRead(CS2_PIN) / 204.6 - 2.5) / 0.066 * 100.0; // The motors turn opposite, so we need to subtract the value
info.current = CS;
} else
info.current = 0; // When the reset button is held low on the motor drivers, the current sensor will give out an incorrect value
info.turning = turningValue * 100.0;
info.battery = batteryLevel;
info.runTime = speedTimer;
sendData((uint8_t*)&info, sizeof(info));
} else if (sendImu && millis() - imuTimer > 100) {
imuTimer = millis();
msg.cmd = START_IMU;
msg.length = sizeof(imu);
imu.acc = accAngle * 100.0;
imu.gyro = gyroAngle * 100.0;
imu.kalman = pitch * 100.0;
sendData((uint8_t*)&imu, sizeof(imu));
}
}
// Message protocol (Inspired by MultiWii):
// Request:
// Header: $S>
// cmd: uint8_t
// n length of data: uint8_t
// Data: n uint8_t
// Checksum (calculated from cmd, length and data)
// Response:
// Header: $S<
// cmd: uint8_t
// n length of data: uint8_t
// Data: n uint8_t
// Checksum (calculated from cmd, length and data)
// Carriage return and line feed ("\r\n")
// All floats/doubles are multiplied by 100 before sending
// Except the Kalman values which are multiplied by 10000 before sending
bool getData(uint8_t *data, uint8_t length) {
if (Serial.readBytes(data, length) != length) // Read data into buffer
return false;
uint8_t checksum;
if (Serial.readBytes(&checksum, sizeof(checksum)) != sizeof(checksum)) // Read the checksum
return false;
return (getCheckSum((uint8_t*)&msg, sizeof(msg)) ^ getCheckSum(data, length)) == checksum; // The checksum is calculated from the length, command and the data
}
void sendData(uint8_t *data, uint8_t length) {
Serial.write(responseHeader);
Serial.write((uint8_t*)&msg, sizeof(msg));
Serial.write(data, length);
Serial.write(getCheckSum((uint8_t*)&msg, sizeof(msg)) ^ getCheckSum(data, length)); // The checksum is calculated from the length, command and the data
Serial.println(); // Print carriage return and line feed as well, so it is easy to figure out the line ending in Java
}
uint8_t getCheckSum(uint8_t *data, size_t length) {
uint8_t checksum = 0;
for (size_t i = 0; i < length; i++)
checksum ^= data[i]; // Calculate checksum
return checksum;
}