Mitchell Pang
RC Boat using a C# GUI over XBee communication.
Dependencies: MODSERIAL LSM9DS0 Motordriver PID mbed-rtos mbed
Diff: main.cpp
- Revision:
- 6:c0a2ac7a8c26
- Parent:
- 4:6f6a9602e92d
- Child:
- 7:bde99b0b3a86
--- a/main.cpp Wed Apr 29 17:30:02 2015 +0000
+++ b/main.cpp Thu Apr 30 20:16:00 2015 +0000
@@ -4,7 +4,7 @@
#include "PID.h"
#include "motordriver.h"
#include "MODSERIAL.h"
-//#include "IMUfilter.h"
+
//Gravity at Earth's surface in m/s/s
#define g0 9.812865328
@@ -29,7 +29,9 @@
#define LSM9DS0_XM 0x1E // Would be 0x1E if SDO_XM is LOW
#define LSM9DS0_G 0x6A // Would be 0x6A if SDO_G is LOW
#define RATE 0.05 // Define rate for PID loop
-
+//Multiply the linear and angular speed to change from [-1,1] to [-Multiplier, Multiplier]
+#define LINEAR_MULTIPLIER 5
+#define ANGULAR_MULTIPLIER 90
Serial pc(USBTX, USBRX);
LSM9DS0 imu(p28, p27, LSM9DS0_G, LSM9DS0_XM);
@@ -60,13 +62,13 @@
double w_xBias;
double w_yBias;
double w_zBias;
-
+
//Offsets for the accelerometer.
//Same as with the gyroscope.
double a_xBias;
double a_yBias;
double a_zBias;
-
+
//Accumulators used for oversampling and then averaging.
double a_xAccumulator = 0;
double a_yAccumulator = 0;
@@ -74,7 +76,7 @@
double w_xAccumulator = 0;
double w_yAccumulator = 0;
double w_zAccumulator = 0;
-
+
//Accelerometer and gyroscope readings for x, y, z axes.
double a_x;
double a_y;
@@ -87,7 +89,7 @@
double last_a_x;
double last_a_y;
double last_a_z;
-
+
//Buffer for accelerometer readings.
int readings[3];
//Number of accelerometer samples we're on.
@@ -100,12 +102,16 @@
double v_y = 0;
double v_z = 0;
double v_mag2 = 0; // linear velocity squared. sqrt() is comupationally difficult
-
- /*
+
+
//values for pwm wave to motors
+/*
float linearOutput = 0;
float angularOutput = 0;
*/
+float leftMotorCO = 0;
+float rightMotorCO = 0;
+
//setpoints from xbee
float setLinear = 0;
@@ -125,186 +131,283 @@
//Take a set of samples and average them.
void sampleGyroscope(void);
-void startPID() {
+
+
+void debug(void const *args)
+{
+ while(1) {
+ //pc.printf("linearOutput: %f angularOutput: %f\n",linearOutput, angularOutput);
+ //pc.printf("leftMotorCO: %f rightMotorCO: %f\n", leftMotorCO, rightMotorCO);
+ pc.printf("setLinear: %f setAngular: %f\n", setLinear, setAngular);
+ pc.printf("linearMagnitude: %f angularSpeed: %f\n", v_mag2, w_z);
+ pc.printf("leftMotorCO: %f rightMotorCO: %f\n", leftMotorCO, rightMotorCO);
+ Thread::wait(2000);
+ }
+}
+
+
+void startPID()
+{
linearController.setMode(AUTO_MODE);
angularController.setMode(AUTO_MODE);
usePID = 1;
}
-
-void stopPID() {
+
+void stopPID()
+{
linearController.setMode(MANUAL_MODE);
angularController.setMode(MANUAL_MODE);
usePID = 0;
}
//Variables for storing commands
-char commBuffer[2][64];
+char commBuffer [64];
int currentBuff = 0;
-void parseCommand() {
- float tempForward, tempAngular;
+
+void parseCommand()
+{
+ float *tempForward, *tempAngular;
+ volatile char forwardBuffer [4];
+ volatile char angularBuffer [4];
int tempEnabled, tempPID;
- sscanf(commBuffer[currentBuff], "F%f A%f E%d P%d\r", &tempForward, &tempAngular, &tempEnabled, &tempPID);
+ char statusChar;
+
+
+ // First char (commBuffer[0]) is header info. commBuffer[1-4] is forward direction in raw float value. CommBUffer[5-8] is reverse direction in raw float value.
+ //memcpy(&tempForward, (commBuffer + 1), 4);
+ //memcpy(&tempAngular, (commBuffer + 5), 4);
+
+
+ // now convert the char arrays into floats
+ tempForward = (float*) (commBuffer + 1);
+ tempAngular = (float*) (commBuffer + 5);
+
+
+ //tempForward = (float*) forwardBuffer;
+ //tempAngular = (float*) angularBuffer;
+
+ statusChar = commBuffer[9]; //contains data for enable and enablePID
+ tempPID = (int) (statusChar & 0x0F); //extract the last 4 bits and type cast to an int
+ tempEnabled = (int) (statusChar & 0xF0); //extract the first 4 bits and type cast to an int
//Change global variables
- setLinear = tempForward;
- setAngular = tempAngular;
+ setLinear = *tempForward * LINEAR_MULTIPLIER;
+ setAngular = *tempAngular * ANGULAR_MULTIPLIER;
enabled = tempEnabled;
-
+
if (tempPID) {
startPID();
} else {
stopPID();
}
-
+
}
-
-void parseInput(void const *args) {
+
+void parseInput(void const *args)
+{
static int i = 0;
+
while (1) {
+ /*
while (xbee.readable()) {
commBuffer[1 - currentBuff][i] = xbee.getc();
- if (commBuffer[1 - currentBuff][i] == '\r') {
- commBuffer[1 - currentBuff][i+1] = '\0';
+ if (commBuffer[1 - currentBuff][i] == 0xAA) {
i = 0;
currentBuff = 1 - currentBuff;
- parseCommand();
- commReceived = 1;
+ if (commBuffer[currentBuff][0] == 0xFF) {
+ parseCommand();
+ commReceived = 1;
+ }
+ } else {
+ ++i;
+ }
+ }
+ */
+
+ while (xbee.readable()) {
+ commBuffer[i] = xbee.getc();
+ if (commBuffer[i] == 0xAA) { //end of message char
+ i = 0;
+ if (commBuffer[0] == 0xFF) { // buffer recieved is correct length and bit. parse the data
+ parseCommand();
+ commReceived = 1;
+ }
} else {
++i;
}
}
- Thread::wait(50);
+
+
+
+
+
+
+ /*
+ while (xbee.readable()) {// something arrived from the xbee
+ commBuffer [0] = xbee.getc(); //[1 - currentBuff][i] = xbee.getc();
+ commBuffer [1] = xbee.getc();
+ if (commBuffer [0] == 0x5 && commBuffer[1] == 0x5) { //the frame from xbee has ended // [1 - currentBuff][i] == '\r') {
+ // next 4 chars (32 bits) are going to be the forward speed
+ for (int fBi = 0; fBi < 4; ++fBi) {
+ forwardBuffer[fBi] = xbee.getc();
+ }
+ for (int rBi = 0; rBi < 4; ++rBi) {
+ reverseBuffer[rBi] = xbee.getc();
+ }
+ enabled = (int) xbee.getc();
+ tempPID = (int) xbee.getc();
+ if (tempPID) {
+ startPID();
+ } else {
+ stopPID();
+ }
+ commBuffer [2] = xbee.getc();
+ commBuffer [3] = xbee.getc();
+ if (commBuffer [3] == 0xA && commBuffer [4] == 0xA) {
+ commReceived = 1;
+ }
+
+ }
+ }
+ */
+ Thread::wait(100);
}
}
-
+
-void checkCOMRecieved() {
- if (commReceived == 0) {
- stopPID();
- enabled = 0;
- led1 = led2 = led3 = led4 = 1;
+void checkCOMRecieved()
+{
+ if (commReceived == 0) {
+ stopPID();
+ enabled = 0;
+ led1 = led2 = led3 = led4 = 1;
} else {
led1 = led2 = led3 = led4 = 0;
}
commReceived = 0;
}
-void sampleAccelerometer(void const *args) {
- while(1) {
- while(usePID != 1) {
- Thread::wait(1000); //wait 1 sec and check if we are using the PID loop again.
- }
- //Have we taken enough samples?
- if (accelerometerSamples == SAMPLES) {
-
- //Average the samples, remove the bias, and calculate the acceleration
- //in m/s/s.
- a_x = ((a_xAccumulator / SAMPLES) - a_xBias) * ACCELEROMETER_GAIN;
- a_y = ((a_yAccumulator / SAMPLES) - a_yBias) * ACCELEROMETER_GAIN;
- a_z = ((a_zAccumulator / SAMPLES) - a_zBias) * ACCELEROMETER_GAIN;
-
- a_xAccumulator = 0;
- a_yAccumulator = 0;
- a_zAccumulator = 0;
- accelerometerSamples = 0;
- pcMutex.lock();
- pc.printf("a_x: %f a_y: %f a_z: %f \n",a_x,a_y,a_z);
- pcMutex.unlock();
-
- // integrate to get velocity. make sure to subtract gravity downwards!
-
- v_x = v_x + (a_x + last_a_x)/2 * ACC_RATE * SAMPLES;
- v_y = v_y + (a_y + last_a_y)/2 * ACC_RATE * SAMPLES;
- v_z = v_z + (a_z + last_a_z)/2 * ACC_RATE * SAMPLES;
-
- last_a_x = a_x;
- last_a_y = a_y;
- last_a_z = a_z;
- pcMutex.lock();
- pc.printf("v_x: %f v_y: %f v_z: %f \n",v_x,v_y,v_z);
- pcMutex.unlock();
-
- } else {
- //Take another sample.
- imu.readAccel();
- pcMutex.lock();
- pc.printf("A: ");
- pc.printf("%d", imu.ax);
- pc.printf(", ");
- pc.printf("%d",imu.ay);
- pc.printf(", ");
- pc.printf("%d\n",imu.az);
- pcMutex.unlock();
-
- a_xAccumulator += imu.ax;
- a_yAccumulator += imu.ay;
- a_zAccumulator += imu.az;
-
- accelerometerSamples++;
- Thread::wait(ACC_RATE * 1000);
- }
+void sampleAccelerometer(void const *args)
+{
+ while(1) {
+ while(usePID != 1) {
+ Thread::wait(1000); //wait 1 sec and check if we are using the PID loop again.
+ }
+ //Have we taken enough samples?
+ if (accelerometerSamples == SAMPLES) {
+
+ //Average the samples, remove the bias, and calculate the acceleration
+ //in m/s/s.
+ a_x = ((a_xAccumulator / SAMPLES) - a_xBias) * ACCELEROMETER_GAIN;
+ a_y = ((a_yAccumulator / SAMPLES) - a_yBias) * ACCELEROMETER_GAIN;
+ a_z = ((a_zAccumulator / SAMPLES) - a_zBias) * ACCELEROMETER_GAIN;
+
+ a_xAccumulator = 0;
+ a_yAccumulator = 0;
+ a_zAccumulator = 0;
+ accelerometerSamples = 0;
+ //pcMutex.lock();
+ //pc.printf("a_x: %f a_y: %f a_z: %f \n",a_x,a_y,a_z);
+ //pcMutex.unlock();
+
+ // integrate to get velocity. make sure to subtract gravity downwards!
+
+ v_x = v_x + (a_x + last_a_x)/2 * ACC_RATE * SAMPLES * 1000;
+ v_y = v_y + (a_y + last_a_y)/2 * ACC_RATE * SAMPLES * 1000;
+ v_z = v_z + (a_z + last_a_z)/2 * ACC_RATE * SAMPLES * 1000;
+
+ last_a_x = a_x;
+ last_a_y = a_y;
+ last_a_z = a_z;
+ //pcMutex.lock();
+ //pc.printf("v_x: %f v_y: %f v_z: %f \n",v_x,v_y,v_z);
+ //pcMutex.unlock();
+
+ } else {
+ //Take another sample.
+ imu.readAccel();
+ /*
+ pcMutex.lock();
+ pc.printf("A: ");
+ pc.printf("%d", imu.ax);
+ pc.printf(", ");
+ pc.printf("%d",imu.ay);
+ pc.printf(", ");
+ pc.printf("%d\n",imu.az);
+ pcMutex.unlock();
+ */
+ a_xAccumulator += imu.ax;
+ a_yAccumulator += imu.ay;
+ a_zAccumulator += imu.az;
+
+ accelerometerSamples++;
+ Thread::wait(ACC_RATE * 1000);
+ }
-}
+ }
}
-
-void calibrateAccelerometer(void) {
+
+void calibrateAccelerometer(void)
+{
led1 = 1;
-
+
a_xAccumulator = 0;
a_yAccumulator = 0;
a_zAccumulator = 0;
-
-
+
+
//Take a number of readings and average them
//to calculate the zero g offset.
for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
-
+
imu.readAccel();
-
+
a_xAccumulator += (double) imu.ax;
a_yAccumulator += (double) imu.ay;
a_zAccumulator += (double) imu.az;
wait(ACC_RATE);
-
+
}
-
+
a_xAccumulator /= CALIBRATION_SAMPLES;
a_yAccumulator /= CALIBRATION_SAMPLES;
a_zAccumulator /= CALIBRATION_SAMPLES;
-
+
//At 4mg/LSB, 250 LSBs is 1g.
a_xBias = a_xAccumulator;
a_yBias = a_yAccumulator;
//a_zBias = (a_zAccumulator - (1/0.00006103515625));
a_zBias = a_zAccumulator; // calibrate so that gravity is already out of the equation
-
+ pc.printf("A_Bias ax: %f ay: %f az: %f \n", a_xBias,a_yBias,a_zBias);
+ /*
pcMutex.lock();
- pc.printf("A_Bias: ");
+ pc.printf("A_Bias ax: %f ay: %f az: %f \n", a_xBias,a_yBias,a_zBias);
pc.printf("%f", a_xBias);
pc.printf(", ");
pc.printf("%f",a_yBias);
pc.printf(", ");
pc.printf("%f\n",a_zBias);
pcMutex.unlock();
-
+ */
a_xAccumulator = 0;
a_yAccumulator = 0;
a_zAccumulator = 0;
pc.printf("done calibrating accel\n");
led1 = 0;
}
-
-
-void calibrateGyroscope(void) {
+
+
+void calibrateGyroscope(void)
+{
led2 = 1;
w_xAccumulator = 0;
w_yAccumulator = 0;
w_zAccumulator = 0;
-
+
//Take a number of readings and average them
//to calculate the gyroscope bias offset.
for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
@@ -313,18 +416,18 @@
w_yAccumulator += (double) imu.gy;
w_zAccumulator += (double) imu.gz;
Thread::wait(GYRO_RATE * 1000);
-
+
}
-
+
//Average the samples.
w_xAccumulator /= CALIBRATION_SAMPLES;
w_yAccumulator /= CALIBRATION_SAMPLES;
w_zAccumulator /= CALIBRATION_SAMPLES;
-
+
w_xBias = w_xAccumulator;
w_yBias = w_yAccumulator;
w_zBias = w_zAccumulator;
-
+
pcMutex.lock();
pc.printf("G_Bias: ");
pc.printf("%f", w_xBias);
@@ -333,75 +436,77 @@
pc.printf(", ");
pc.printf("%f\n",w_zBias);
pcMutex.unlock();
-
+
w_xAccumulator = 0;
w_yAccumulator = 0;
w_zAccumulator = 0;
pc.printf("done calibrating gyro\n");
led2 = 0;
}
-
-void sampleGyroscope(void const *args) {
- while(1){
- while(usePID != 1) {
- Thread::wait(1000); //wait 1 sec and check if we are using the PID loop again.
+
+void sampleGyroscope(void const *args)
+{
+ while(1) {
+ while(usePID != 1) {
+ Thread::wait(1000); //wait 1 sec and check if we are using the PID loop again.
}
- //Have we taken enough samples?
- if (gyroscopeSamples == SAMPLES) {
-
- //Average the samples, remove the bias, and calculate the angular
- //velocity in deg/s.
- w_x = ((w_xAccumulator / SAMPLES) - w_xBias) * GYROSCOPE_GAIN;
- w_y = ((w_yAccumulator / SAMPLES) - w_yBias) * GYROSCOPE_GAIN;
- w_z = ((w_zAccumulator / SAMPLES) - w_zBias) * GYROSCOPE_GAIN;
- pcMutex.lock();
- pc.printf("w_x: %f w_y: %f w_z: %f \n",w_x,w_y,w_z);
- pcMutex.unlock();
- w_xAccumulator = 0;
- w_yAccumulator = 0;
- w_zAccumulator = 0;
- gyroscopeSamples = 0;
-
- } else {
- imu.readGyro();
- pcMutex.lock();
- pc.printf("G: ");
- pc.printf("%d", imu.gx);
- pc.printf(", ");
- pc.printf("%d",imu.gy);
- pc.printf(", ");
- pc.printf("%d\n",imu.gz);
- pcMutex.unlock();
-
- w_xAccumulator += imu.gx;
- w_yAccumulator += imu.gy;
- w_zAccumulator += imu.gz;
-
- gyroscopeSamples++;
+ //Have we taken enough samples?
+ if (gyroscopeSamples == SAMPLES) {
+
+ //Average the samples, remove the bias, and calculate the angular
+ //velocity in deg/s.
+ w_x = ((w_xAccumulator / SAMPLES) - w_xBias) * GYROSCOPE_GAIN;
+ w_y = ((w_yAccumulator / SAMPLES) - w_yBias) * GYROSCOPE_GAIN;
+ w_z = ((w_zAccumulator / SAMPLES) - w_zBias) * GYROSCOPE_GAIN;
+ //pcMutex.lock();
+ //pc.printf("w_x: %f w_y: %f w_z: %f \n",w_x,w_y,w_z);
+ //pcMutex.unlock();
+ w_xAccumulator = 0;
+ w_yAccumulator = 0;
+ w_zAccumulator = 0;
+ gyroscopeSamples = 0;
+
+ } else {
+ imu.readGyro();
+ /*
+ pcMutex.lock();
+ pc.printf("G: ");
+ pc.printf("%d", imu.gx);
+ pc.printf(", ");
+ pc.printf("%d",imu.gy);
+ pc.printf(", ");
+ pc.printf("%d\n",imu.gz);
+ pcMutex.unlock();
+ */
+ w_xAccumulator += imu.gx;
+ w_yAccumulator += imu.gy;
+ w_zAccumulator += imu.gz;
+
+ gyroscopeSamples++;
+ Thread::wait(GYRO_RATE * 1000);
+ }
+
Thread::wait(GYRO_RATE * 1000);
}
+}
-Thread::wait(GYRO_RATE * 1000);
-}
-}
-
+
-
-
- void setup()
+
+void setup()
{
pc.baud(9600); // Start serial at 115200 bps
// Use the begin() function to initialize the LSM9DS0 library.
// You can either call it with no parameters (the easy way):
//uint16_t status = dof.begin();
-
+
//Or call it with declarations for sensor scales and data rates:
//uint16_t status = imu.begin(dof.G_SCALE_2000DPS,
// dof.A_SCALE_2G, dof.M_SCALE_2GS);
-
+
uint16_t status = imu.begin(imu.G_SCALE_245DPS, imu.A_SCALE_2G, imu.M_SCALE_2GS,
imu.G_ODR_760_BW_100, imu.A_ODR_400, imu.M_ODR_50);
-
+
// begin() returns a 16-bit value which includes both the gyro
// and accelerometers WHO_AM_I response. You can check this to
// make sure communication was successful.
@@ -409,28 +514,32 @@
pc.printf("%x\n",status);
pc.printf("Should be 0x49D4\n");
pc.printf("\n");
-
+
// Set up PID Loops
- linearController.setInputLimits(-1.0, 1.0);
- angularController.setInputLimits(-1.0, 1.0);
- linearController.setOutputLimits(0.0, 1.0);
- angularController.setOutputLimits(0.0, 1.0);
+ linearController.setInputLimits(-LINEAR_MULTIPLIER * LINEAR_MULTIPLIER, LINEAR_MULTIPLIER * LINEAR_MULTIPLIER);
+ angularController.setInputLimits(-ANGULAR_MULTIPLIER, ANGULAR_MULTIPLIER);
+ linearController.setOutputLimits(-1.0, 1.0);
+ angularController.setOutputLimits(-1.0, 1.0);
linearController.setSetPoint(0.0);
angularController.setSetPoint(0.0);
linearController.setMode(AUTO_MODE);
angularController.setMode(AUTO_MODE);
-
+
}
-int main() {
+int main()
+{
float linearOutput = 0;
float angularOutput = 0;
-
+ /*
+ float leftMotorCO = 0;
+ float rightMotorCO = 0;
+*/
pc.printf("Starting IMU filter test...\n");
setup();
-
-
+
+
//Initialize inertial sensors.
calibrateAccelerometer();
calibrateGyroscope();
@@ -438,36 +547,37 @@
Thread accelThread(sampleAccelerometer);
Thread gyroThread(sampleGyroscope);
Thread commThread(parseInput);
+ Thread debugThread(debug);
commTicker.attach(&checkCOMRecieved, 2.0); // the address of the function to be attached (checkCOMRecieved) and the interval (2 seconds)
/*
- pcMutex.lock();
- pc.printf("done attaching tickers\n\n");
- pcMutex.unlock();
-*/
+ //pcMutex.lock();
+ pc.printf("done attaching tickers\n\n");
+ //pcMutex.unlock();
+ */
while (1) {
- //pc.printf("in loop\n");
+ //pc.printf("in loop\n");
if(usePID) {
- linearController.setSetPoint(setLinear);
+ linearController.setSetPoint(setLinear * setLinear);
angularController.setSetPoint(setAngular);
-
+
v_mag2 = v_x * v_x + v_y * v_y;
linearController.setProcessValue(v_mag2);
- angularController.setProcessValue(w_z); // w_z = degrees per second
+ angularController.setProcessValue(w_z); // w_z = degrees per second
linearOutput = linearController.compute();
angularOutput = angularController.compute();
} else {
- linearOutput = setLinear;
- angularOutput = setAngular;
+ linearOutput = setLinear/LINEAR_MULTIPLIER;
+ angularOutput = setAngular/ANGULAR_MULTIPLIER;
}
-
+
if (enabled) {
- float leftMotorCO = (linearOutput + angularOutput);
- float rightMotorCO = (linearOutput - angularOutput);
-
+ leftMotorCO = (linearOutput - angularOutput);
+ rightMotorCO = (linearOutput + angularOutput);
+
leftMotor.speed(leftMotorCO);
rightMotor.speed(rightMotorCO);
-
-
+
+
/*
leftMotor.speed(setLinear);
rightMotor.speed(setAngular);
@@ -476,15 +586,15 @@
leftMotor.coast();
rightMotor.coast();
}
-
+
Thread::wait(RATE * 1000);
- /*
- pcMutex.lock();
- pc.printf("%f,%f,%f\n",toDegrees(imuFilter.getRoll()),
- toDegrees(imuFilter.getPitch()),
- toDegrees(imuFilter.getYaw()));
- pcMutex.unlock();
-*/
+ /*
+ //pcMutex.lock();
+ pc.printf("%f,%f,%f\n",toDegrees(imuFilter.getRoll()),
+ toDegrees(imuFilter.getPitch()),
+ toDegrees(imuFilter.getYaw()));
+ //pcMutex.unlock();
+ */
}
}
\ No newline at end of file
Repository toolbox
| Export to desktop IDE |
| Build repository |
Repository details
| Type: | Program |
|---|---|
| Mbed OS support: | Mbed 2 deprecated |
| Created: | 28 Apr 2015 |
| Imports: | 28 |
| Forks: | 0 |
| Commits: | 11 |
| Dependents: | 0 |
| Dependencies: | 6 |
| Followers: | 5 |
This repository is Public (Unlisted).
