Formal Report

SegwayFaultAuto-Balancing 2-Wheeled Platform with Obstacle

Avoidance

Patrick Lloyd

Intelligent Machine Design LabEEL 4665, Fall 2014Professors: Dr. A. Antonio Arroyo, Dr. Eric M. SchwartzTeaching Assistants: Andy Gray, Nick Cox

University of Florida Department of Electrical and Computer Engineering

Table of ContentsAbstract......................................................................................................................................................2

Executive Summary...................................................................................................................................3

Introduction................................................................................................................................................4

Integrated System.......................................................................................................................................6

Mobile Platform.........................................................................................................................................7

Actuation....................................................................................................................................................8

Sensors.......................................................................................................................................................9

Behaviors.................................................................................................................................................10

Experimental Layout and Results............................................................................................................11

Conclusion...............................................................................................................................................12

Documentation.........................................................................................................................................13

Appendices...............................................................................................................................................14

2

Abstract

In many emergency situations such as a collapsed building or contaminated factory, autonomousrobotic navigation of debris and rubbish requires novel control methods. The aim of this project is tocreate a mechanically simple, two-wheeled driving platform to investigate self-balancing algorithms foran “inverted pendulum”-style robot with a nine-degree-of-freedom inertial measurement unit (9-DOFIMU). The robot uses the IMU to determine uprightness and adjust the motor speed accordingly to stayin the vertical position. For the demonstration, the robot will drive forward and backward whilemaintaining upright stability. Using built-in sonar sensors, it will also navigate away from obstacles.

3

Executive Summary

The SegwayFault self-balancing robot consists of two 350 RPM 12V motors, two motor controllers, a9-DOF IMU, front and rear sonar range finders, and a 5000mAh battery. The actuators and sensors areall controlled with an Arduino Uno running at 16 MHz using 5V logic. The primary software routinesused to maintain uprightness are a quaternion-based accelerometer/gyroscope fusing algorithm as wellas a PID controller for wheel speed. The robot can balance on its own for several seconds beforetoppling over but it is clear that the robot can respond to changes in pitch and attempt to correct itself.

In order to avoid obstacles, the robot would detect objects closer than six inches and dynamically adjustits pitch set-point to move away from the obstacle until it is at least six inches away. Then the robotwould re-zero its pitch set-point and stand in place. Since robust self-balancing was not fullyimplemented, this feature was never added to the functionality of SegwayFault but all hardware existsto interface with it.

4

Introduction

PremiseIn many emergency situations such as a collapsed building or contaminated factory, rescue workers areforced to navigate dangerous situations to find trapped or injured victims. In order to use robots as firstresponders in these situations, robust and unique control systems need to be implemented to navigatethrough treacherous terrain. One such control scheme is the two-wheeled balancing method used by theSegway. This is also a first step in designing accurate, anthropomorphic robots that mimic humanbehaviors like walking. With the introduction of inexpensive inertial measurement units and opensource tools like Arduino, these behaviors are easy to implement.

Scope and ObjectivesThe Intelligent Machine Design Lab class is structured such that system integration and endfunctionality is valued above subsystem design. With this in mind, almost none of the software orhardware solutions will be designed and implemented from the ground up. For example, with theexception of a a custom board to connect modules to the Arduino, all electronics such asmicrocontrollers, motor controllers, and sensor boards are are manufactured by third parties. The coderelies open-source libraries and borrows some code from an existing implementation of a Segway-stylerobot. The scope of the project is to get all of these subsystems to function cohesively together withminimal customization.The objectives of the project are as follows:

• Remain upright while providing robust control of robot as it moves • Have the robot move through an arbitrary indoor space and using its full array of sensors,

navigate around obstacles• Have at least three hours of endurance

5

Walk-ThroughThe report is broken down into the following nine sections:

• Integrated System – High-level organization of robot and accessory systems• Mobile Platform – Physical robot frame that holds the sensors, batteries, etc.• Actuation – Components that move or effect the pose of the platform or sensors• Sensors – Components that provide information about the surrounding environment• Behaviors – Breakdown of tasks and operations to complete while running• Experimental Layout and Results – System setup and data presentation• Conclusion – Summary and lessons learned• Documentation – References and bibliography• Appendices – Code, circuit diagrams, and supplementary material

6

Integrated System

Figure 1: High-level system flow chart

The integrated system consists of two primary sensors which are polled consistently by the Arduinomicrocontroller. The IMU provides tilt information of the robot and the sonar provide obstacledetection and avoidance information about the environment. Once the microcontroller has determinedthe necessary set-point (either to stay upright, move forward, or move backwards), it adjusts the motorvelocity by sending a square wave to the motor controllers corresponding to wheel speed.

7

Mobile Platform

Since time is a very real constraint for this project (four months to complete), the goal of the mobile platform is to be as mechanically simple as possible. The lack of sophistication was chosen in order to maximize time spent on developing the software functionality instead of debugging and optimizing mechanical systems. With that in mind, the mobile platform will be defined by the following features:

• Simple wooden rectangular base approximately 30cm wide and 10cm long to house Arduino,

IMU, motor controllers, and sonar on platform• Total height approximately 30cm• Two wheels driven by two 30:1 metal gearmotors (37Dx54L mm) with encoders• Brackets attached to base to provide increased support for motors• Sonar sensors fixed to front of wooden base

8

Actuation

Since the mechanical aspects of SegwayFault are minimal, only one kind of actuator is used: metalgearmotors for locomotion and stability.

30:1 Metal Gearmotor 37Dx54L mm with 48 CPR Encoder• Function: Provides locomotive drive for platform; encoder data aids localization and speed

control• Quantity: 2• Vendor: Pololu• Part Number: 2286

Figure 2: Gearmotor with attached encoder

9

Sensors

Nine Degree of Freedom IMU (Special Sensor)• Function: Breakout board for the 9-DOF LSM9DS0 from STMicroelectronics• Quantity: 1• Vendor: Sparkfun• Part Number: SEN-12636

Figure 3: LSM9DS0 inertial measurement unit breakout

10

Sonar Range Finders• Function: Obstacle avoidance• Quantity: 2-4• Vendor: MaxBotix• Part Number: MB1010 LV-MaxSonar-EZ1

Figure 4: MB1010 LV-MaxSonar-EZ1

11

Behaviors

Stabilization and Motion• Determine uprightness at bootup

◦ pitch angle = 0 degrees for standing in place, positive for forward, negative for backward• Poll IMU and calculate pitch• Based on angle, adjust motor speed to achieve set-point

Obstacle Avoidance• Begin standing in place• Poll sonar. Object detected?

◦ Yes: Adjust angle set-point away from object. Move away from object. Poll sensor to detect

if obstacle still present. Repeat◦ No: Continue standing in place

12

Experimental Layout and Results

Figure 5: Final robotic platforms with sensors assembled

In order to test the functionality of the sensors, simple Arduino sketches were written to poll the sensorsand publish their values on the serial device. The device is then read by a host computer and analyzed using Excel. Below are the results:

13

Figure 6: Plot of robot pitch angle, target angle, system error, and PID output percentage

Figure 7: Plot of PID output percentage, right wheel speed, and left wheel speed

14

Figure 8: Front and back sonar distance measurements

15

Conclusion

It can be seen from the results section that the robot can successfully adjust its behavior in response tochanges in pitch and can detect obstacles nearby. In Figure 7, the PID output oscillates and eventuallysaturates which leads to a possibility of too much gain in the PID loop. The control system currentlydoes not integrate wheel speeds into the PID control system but in future development, if paired withtimer interrupts, a very accurate wheel velocity and position can be fused with accelerometer andgyroscope readings to minimize error in the system and more accurately gage where the robot is inspace. Also, instead of a complicated quaternion algorithm to determine pitch, this application wouldprobably be better suited with a Kalman filter to merge the sensors to generate output.

16

Documentation

[1] Maxbotix Inc., LV-MaxSonar-EZ1 High Performance Sonar Range Finder, MB1010 Datasheet, 2014.

[2] HiTec Multiplex, HS-322HD Standard Heavy Duty Servo, HiTec Servo Product Description Page, 2014.

[3] Sparkfun Electronics, 9 Degrees of Freedom - Razor IMU, Sparkfun Product Description Page, 2014.

[4] Pololu Corporation, 75:1 Metal Gearmotor 25Dx54L mm with 48 CPR Encoder, Pololu ProductDescription Page, 2014

[5] Pololu Corporation, Snap-Action Switch with 15.6mm Bump Lever: 3-Pin, SPDT, 5A, Pololu Product Description Page, 2014

17

Appendices

Appendix A: Balance.ino#include "balance.h"

uint32_t timer = 0;uint32_t pidTimer = 0;uint32_t printTimer = 0;float target = 0.0;

void setup(){ Serial.begin(115200); // Initialize IMU imuInit(); // Refresh the IMU 1000 times to remove steady state error for(int i = 0; i < 1000; i++){ imuRefreshValues(); }

// Initializes motors and encoders on each side initMotor(right); initMotor(left); initEncoder(right); initEncoder(left); // Stops everything and clears encoder counters stopMotorsResetEnc();

//target = getRoll(true); target = 0; // Zeroes everything and sets the target angle// setTargetAngle(getPitch(TRUE));// setTargetPos(getWheelsPosition());}

void loop(){ timer = micros(); imuRefreshValues();

18

updatePID(target, (double)(timer - pidTimer) / 1000000.0); pidTimer = timer; if(timer - printTimer > 1000000){ updateTarget(); Serial.print(target); //Serial.print(getRoll(true)); //Serial.print("\t\t"); //Serial.println(getRoll(false)); printTimer = timer; }}

void updateTarget(){ word accumulate = 0; for(int i = 0; i<64; i++){ accumulate += analogRead(A5); } accumulate = accumulate >> 6; target = ((float)accumulate * 0.01955034213) - 10.0;}

19

Appendix B: IMU.ino#include <Wire.h>#include <SPI.h> // Included for SFE_LSM9DS0 library#include <SFE_LSM9DS0.h>

#define LSM9DS0_CSG 8 // CSG connected to Arduino pin 9#define LSM9DS0_CSXM 9 // CSXM connected to Arduino pin 10

///////////////// SPI Setup /////////////////// No interrupt pins being used, only pollingLSM9DS0 dof(MODE_SPI, LSM9DS0_CSG, LSM9DS0_CSXM);

// global constants for 9 DoF fusion and AHRS (Attitude and Heading Reference System)#define GyroMeasError PI * (40.0f / 180.0f) // gyroscope measurement error in rads/s (shown as 3 deg/s)#define GyroMeasDrift PI * (0.0f / 180.0f) // gyroscope measurement drift in rad/s/s (shown as 0.0 deg/s/s)

// There is a tradeoff in the beta parameter between accuracy and response speed.// In the original Madgwick study, beta of 0.041 (corresponding to GyroMeasError of 2.7 degrees/s) was found to give optimal accuracy.// However, with this value, the LSM9SD0 response time is about 10 seconds to a stable initial quaternion.// Subsequent changes also require a longish lag time to a stable output, not fast enough for a quadcopter or robot car!// By increasing beta (GyroMeasError) by about a factor of fifteen, the response time constant is reduced to ~2 sec// I haven't noticed any reduction in solution accuracy. This is essentially the I coefficient in a PID control sense;// the bigger the feedback coefficient, the faster the solution converges, usually at the expense of accuracy.// In any case, this is the free parameter in the Madgwick filtering and fusion scheme.#define beta sqrt(3.0f / 4.0f) * GyroMeasError // compute beta#define zeta sqrt(3.0f / 4.0f) * GyroMeasDrift // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value#define Kp 2.0f * 5.0f // these are the free parameters in the Mahony filter and fusion scheme, Kp for proportional feedback, Ki for integral#define Ki 0.0f

float pitch, yaw, roll, heading;float deltat = 0.0f; // integration interval for both filter schemesuint32_t lastUpdate = 0; // used to calculate integration intervaluint32_t Now = 0; // used to calculate integration interval

20

float abias[3] = {0, 0, 0}, gbias[3] = {0, 0, 0};float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data valuesfloat q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // vector to hold quaternionfloat eInt[3] = {0.0f, 0.0f, 0.0f}; // vector to hold integral error for Mahony methodfloat temperature = 0.0;

void imuInit(){ uint32_t dof_status = dof.begin(); Serial.print("LSM9DS0 WHO_AM_I's returned: 0x"); Serial.println(dof_status, HEX); Serial.println("Should be 0x49D4"); Serial.println(); // Set data output ranges; choose lowest ranges for maximum resolution // Accelerometer scale can be: A_SCALE_2G, A_SCALE_4G, A_SCALE_6G, A_SCALE_8G, or A_SCALE_16G dof.setAccelScale(dof.A_SCALE_2G);

// Gyro scale can be: G_SCALE__245, G_SCALE__500, or G_SCALE__2000DPS dof.setGyroScale(dof.G_SCALE_245DPS);

// Magnetometer scale can be: M_SCALE_2GS, M_SCALE_4GS, M_SCALE_8GS, M_SCALE_12GS dof.setMagScale(dof.M_SCALE_2GS);

// Set output data rates // Accelerometer output data rate (ODR) can be: A_ODR_3125 (3.225 Hz), A_ODR_625 (6.25 Hz), A_ODR_125 (12.5 Hz), // A_ODR_25, A_ODR_50, A_ODR_100, A_ODR_200, A_ODR_400, A_ODR_800, A_ODR_1600 (1600 Hz) dof.setAccelODR(dof.A_ODR_1600); // Set accelerometer update rate at 100 Hz

// Accelerometer anti-aliasing filter rate can be 50, 194, 362, or 763 Hz // Anti-aliasing acts like a low-pass filter allowing oversampling of accelerometer and rejection of high-frequency spurious noise. // Strategy here is to effectively oversample accelerometer at 100 Hz and use a 50 Hz anti-aliasing (low-pass) filter frequency // to get a smooth ~150 Hz filter update rate dof.setAccelABW(dof.A_ABW_50); // Choose lowest filter setting for low noise

// Gyro output data rates can be: 95 Hz (bandwidth 12.5 or 25 Hz), 190 Hz (bandwidth 12.5, 25, 50, or 70 Hz) // 380 Hz (bandwidth 20, 25, 50, 100 Hz), or 760 Hz (bandwidth 30, 35, 50, 100 Hz) dof.setGyroODR(dof.G_ODR_760_BW_30); // Set gyro update rate to 190 Hz with the smallest

21

bandwidth for low noise

// Magnetometer output data rate can be: 3.125 (ODR_3125), 6.25 (ODR_625), 12.5 (ODR_125), 25,50, or 100 Hz dof.setMagODR(dof.M_ODR_125); // Set magnetometer to update every 80 ms // Use the FIFO mode to average accelerometer and gyro readings to calculate the biases, which can then be removed from // all subsequent measurements. dof.calLSM9DS0(gbias, abias);}

void imuRefreshValues(){ dof.readGyro(); // Read raw gyro data gx = dof.calcGyro(dof.gx) - gbias[0]; // Convert to degrees per seconds, remove gyro biases gy = dof.calcGyro(dof.gy) - gbias[1]; gz = dof.calcGyro(dof.gz) - gbias[2]; dof.readAccel(); // Read raw accelerometer data ax = dof.calcAccel(dof.ax) - abias[0]; // Convert to g's, remove accelerometer biases ay = dof.calcAccel(dof.ay) - abias[1]; az = dof.calcAccel(dof.az) - abias[2]; // Don't necessarily need heading & temp data. Will comment out if too slow dof.readMag(); // Read raw magnetometer data mx = dof.calcMag(dof.mx); // Convert to Gauss and correct for calibration my = dof.calcMag(dof.my); mz = dof.calcMag(dof.mz);

//dof.readTemp(); //temperature = 21.0 + (float) dof.temperature/8.;

// Update time calculations Now = micros(); deltat = ((Now - lastUpdate)/1000000.0f); // set integration time by time elapsed since last filter update lastUpdate = Now; MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, mx, my, mz); //MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, mx, my, mz);}

float getRoll(boolean degrees)

22

{ roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]); if(degrees == true) { roll *= 180.0f / PI; } return roll;}

float getPitch(boolean degrees){ pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])); if(degrees == true) { pitch *= 180.0f / PI; } return pitch;}

float getYaw(){ yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]); yaw *= 180.0f / PI; yaw += 5.8; // Declination at Danville, California is -5 degrees 41 minutes return yaw;}

void imuPrintRaw(){ Serial.print("ax = "); Serial.print((int)1000*ax); Serial.print(" ay = "); Serial.print((int)1000*ay); Serial.print(" az = "); Serial.print((int)1000*az); Serial.println(" mg"); Serial.print("gx = "); Serial.print( gx, 2); Serial.print(" gy = "); Serial.print( gy, 2); Serial.print(" gz = "); Serial.print( gz, 2); Serial.println(" deg/s"); Serial.print("mx = "); Serial.print( (int)1000*mx); Serial.print(" my = "); Serial.print( (int)1000*my); Serial.print(" mz = "); Serial.print( (int)1000*mz); Serial.println(" mG"); Serial.print("temperature = "); Serial.println(temperature, 2); Serial.print("Yaw, Pitch, Roll: "); Serial.print(yaw, 2); Serial.print(", "); Serial.print(pitch, 2); Serial.print(", "); Serial.println(roll, 2); Serial.print("q0 = "); Serial.print(q[0]); Serial.print(" qx = "); Serial.print(q[1]);

23

Serial.print(" qy = "); Serial.print(q[2]); Serial.print(" qz = "); Serial.println(q[3]); Serial.print("filter rate = "); Serial.println(1.0f/deltat, 1);}

// Implementation of Sebastian Madgwick's "...efficient orientation filter for... inertial/magnetic sensor arrays"// (see http://www.x-io.co.uk/category/open-source/ for examples and more details)// which fuses acceleration, rotation rate, and magnetic moments to produce a quaternion-based estimate of absolute// device orientation -- which can be converted to yaw, pitch, and roll. Useful for stabilizing quadcopters, etc.// The performance of the orientation filter is at least as good as conventional Kalman-based filtering algorithms// but is much less computationally intensive---it can be performed on a 3.3 V Pro Mini operating at 8 MHz!void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz){ float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name local variable for readability float norm; float hx, hy, _2bx, _2bz; float s1, s2, s3, s4; float qDot1, qDot2, qDot3, qDot4;

// Auxiliary variables to avoid repeated arithmetic float _2q1mx; float _2q1my; float _2q1mz; float _2q2mx; float _4bx; float _4bz; float _2q1 = 2.0f * q1; float _2q2 = 2.0f * q2; float _2q3 = 2.0f * q3; float _2q4 = 2.0f * q4; float _2q1q3 = 2.0f * q1 * q3; float _2q3q4 = 2.0f * q3 * q4; float q1q1 = q1 * q1; float q1q2 = q1 * q2; float q1q3 = q1 * q3; float q1q4 = q1 * q4; float q2q2 = q2 * q2; float q2q3 = q2 * q3; float q2q4 = q2 * q4; float q3q3 = q3 * q3;

24

float q3q4 = q3 * q4; float q4q4 = q4 * q4;

// Normalise accelerometer measurement norm = sqrt(ax * ax + ay * ay + az * az); if (norm == 0.0f) return; // handle NaN norm = 1.0f/norm; ax *= norm; ay *= norm; az *= norm;

// Normalise magnetometer measurement norm = sqrt(mx * mx + my * my + mz * mz); if (norm == 0.0f) return; // handle NaN norm = 1.0f/norm; mx *= norm; my *= norm; mz *= norm;

// Reference direction of Earth's magnetic field _2q1mx = 2.0f * q1 * mx; _2q1my = 2.0f * q1 * my; _2q1mz = 2.0f * q1 * mz; _2q2mx = 2.0f * q2 * mx; hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4; hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4; _2bx = sqrt(hx * hx + hy * hy); _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4; _4bx = 2.0f * _2bx; _4bz = 2.0f * _2bz; // Gradient decent algorithm corrective step s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - ax) + _2q2 * (2.0f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - ax) + _2q1 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz *q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - ax) + _2q4 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4

25

- q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx* q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - ax) + _2q3 * (2.0f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz *q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude norm = 1.0f/norm; s1 *= norm; s2 *= norm; s3 *= norm; s4 *= norm;

// Compute rate of change of quaternion qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1; qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2; qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3; qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4;

// Integrate to yield quaternion q1 += qDot1 * deltat; q2 += qDot2 * deltat; q3 += qDot3 * deltat; q4 += qDot4 * deltat; norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion norm = 1.0f/norm; q[0] = q1 * norm; q[1] = q2 * norm; q[2] = q3 * norm; q[3] = q4 * norm;}

// Similar to Madgwick scheme but uses proportional and integral filtering on the error between estimated reference vectors and// measured ones.void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz){ float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name local variable for readability float norm; float hx, hy, bx, bz; float vx, vy, vz, wx, wy, wz; float ex, ey, ez; float pa, pb, pc;

// Auxiliary variables to avoid repeated arithmetic

26

float q1q1 = q1 * q1; float q1q2 = q1 * q2; float q1q3 = q1 * q3; float q1q4 = q1 * q4; float q2q2 = q2 * q2; float q2q3 = q2 * q3; float q2q4 = q2 * q4; float q3q3 = q3 * q3; float q3q4 = q3 * q4; float q4q4 = q4 * q4; // Normalise accelerometer measurement norm = sqrt(ax * ax + ay * ay + az * az); if (norm == 0.0f) return; // handle NaN norm = 1.0f / norm; // use reciprocal for division ax *= norm; ay *= norm; az *= norm; // Normalise magnetometer measurement norm = sqrt(mx * mx + my * my + mz * mz); if (norm == 0.0f) return; // handle NaN norm = 1.0f / norm; // use reciprocal for division mx *= norm; my *= norm; mz *= norm; // Reference direction of Earth's magnetic field hx = 2.0f * mx * (0.5f - q3q3 - q4q4) + 2.0f * my * (q2q3 - q1q4) + 2.0f * mz * (q2q4 + q1q3); hy = 2.0f * mx * (q2q3 + q1q4) + 2.0f * my * (0.5f - q2q2 - q4q4) + 2.0f * mz * (q3q4 - q1q2); bx = sqrt((hx * hx) + (hy * hy)); bz = 2.0f * mx * (q2q4 - q1q3) + 2.0f * my * (q3q4 + q1q2) + 2.0f * mz * (0.5f - q2q2 - q3q3); // Estimated direction of gravity and magnetic field vx = 2.0f * (q2q4 - q1q3); vy = 2.0f * (q1q2 + q3q4); vz = q1q1 - q2q2 - q3q3 + q4q4; wx = 2.0f * bx * (0.5f - q3q3 - q4q4) + 2.0f * bz * (q2q4 - q1q3); wy = 2.0f * bx * (q2q3 - q1q4) + 2.0f * bz * (q1q2 + q3q4); wz = 2.0f * bx * (q1q3 + q2q4) + 2.0f * bz * (0.5f - q2q2 - q3q3); // Error is cross product between estimated direction and measured direction of gravity ex = (ay * vz - az * vy) + (my * wz - mz * wy); ey = (az * vx - ax * vz) + (mz * wx - mx * wz); ez = (ax * vy - ay * vx) + (mx * wy - my * wx); if (Ki > 0.0f){ eInt[0] += ex; // accumulate integral error

27

eInt[1] += ey; eInt[2] += ez; }else{ eInt[0] = 0.0f; // prevent integral wind up eInt[1] = 0.0f; eInt[2] = 0.0f; } // Apply feedback terms gx = gx + Kp * ex + Ki * eInt[0]; gy = gy + Kp * ey + Ki * eInt[1]; gz = gz + Kp * ez + Ki * eInt[2]; // Integrate rate of change of quaternion pa = q2; pb = q3; pc = q4; q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * deltat); q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * deltat); q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * deltat); q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * deltat); // Normalise quaternion norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); norm = 1.0f / norm; q[0] = q1 * norm; q[1] = q2 * norm; q[2] = q3 * norm; q[3] = q4 * norm;}

28

Appenxix C: Motor.ino/* Most stuff from: Kristian Lauszus, TKJ Electronics Web : http://www.tkjelectronics.com e-mail : kristianl@tkjelectronics.com*///#include "balance.h"#include <Encoder.h>#include <Servo.h>

// Disables attachInterrup() and associated overhead#define ENCODER_OPTIMIZE_INTERRUPTS

// Used with Servo Library#define leftMotorPin 6#define rightMotorPin 5

// Used with Encoder Library#define rightEncoderAPin 2#define rightEncoderBPin 7

#define leftEncoderAPin 3#define leftEncoderBPin 4

int32_t targetPosition;float targetAngle;

float motorAngle;

const float positionScaleA = 600.0; const float positionScaleB = 800.0;const float positionScaleC = 1000.0;const float positionScaleD = 500.0;

const float P = 18.0;const float I = 0.5;const float D = 2.5;

const float velocityScaleMove = 70.0;const float velocityScaleStop = 60.0;

float pTerm = 0.0;float error = 0.0;float integratedError = 0.0; float iTerm = 0.0;float dTerm = 0.0;

29

float lastError = 0;float PIDValue = 0;

Encoder rightEncoder(rightEncoderAPin, rightEncoderBPin);Encoder leftEncoder(leftEncoderAPin, leftEncoderBPin);

Servo rightMotor;Servo leftMotor;

int32_t readLeftEncoder(){ // The encoders decrease when motors are traveling forward and increase when traveling backward return leftEncoder.read();}

int32_t readRightEncoder(){ return (-1) * rightEncoder.read();}

int32_t getWheelsPosition(){ return ( leftEncoder.read() + rightEncoder.read() );}

/*Sends speed command to motor of choice in specified direction.

motor: [left, right]direction: [forward, backward]speed: percentage of maximum speed [0.0 <-> 100.0]

*** If shit gets whack in here, you may need to change thewrite command to writeMicroseconds() for more control. May notbe necessary with

*/void moveMotor(Command motor, Command direction, double speedRaw){ // Error catching double speed = constrain(speedRaw, 0.0, 100.0);

// Arduino Servo library requires servo angle as input. // This maps the motor speed and direction between 0 and 180 degrees motorAngle = 9.0 * speed / 10.0;

30

/* TODO: GET THESE RIGHT FOR DEFINED FORWARD AND BACKWARD DIRECTIONS */ if (motor == left) { if (direction == forward) leftMotor.write((90.0 + motorAngle)); else leftMotor.write((90.0 - motorAngle)); } else { if (direction == forward) rightMotor.write((90.0 + motorAngle)); else rightMotor.write((90.0 - motorAngle)); }}

void stopMotor(Command motor){ if (motor == left) { leftMotor.write(90.0); } else { rightMotor.write(90.0); }}

void stopMotorsResetEnc(){ stopMotor(left); stopMotor(right); rightEncoder.write(0); leftEncoder.write(0); delay(5);}

void initMotor(Command motor){ if(motor == right) { pinMode(rightMotorPin, OUTPUT); rightMotor.attach(rightMotorPin); } else if(motor == left) {

31

pinMode(leftMotorPin, OUTPUT); leftMotor.attach(leftMotorPin); }}

// Actually already initializes them without this but whatevervoid initEncoder(Command encoder){ if(encoder == right) { pinMode(rightEncoderAPin, INPUT); pinMode(rightEncoderBPin, INPUT); rightEncoder.write(0); } else if(encoder == left) { pinMode(leftEncoderAPin, INPUT); pinMode(leftEncoderBPin, INPUT); leftEncoder.write(0); }}

void setTargetPosition(int32_t target){ targetPosition = target;}

void setTargetAngle(float target){ targetAngle = target;}

//Updates PID controller stuff

void updatePID(double restAngle, double dt) { float roll = 0.0; imuRefreshValues(); roll = getRoll(true); error = (restAngle - roll); pTerm = P * error; integratedError += error * dt; integratedError = constrain(integratedError, -1.0, 1.0); // Limit the integrated error iTerm = (I * 100.0) * integratedError;

32

dTerm = (D / 100.0) * (error - lastError) / dt; lastError = error; PIDValue = pTerm + iTerm + dTerm; PIDValue = constrain(PIDValue, -100.0, 100.0); Serial.print("PID Value: "); Serial.println(PIDValue); if(PIDValue > 0) { moveMotor(right, backward, PIDValue); moveMotor(left, backward, PIDValue); } else { moveMotor(right, forward, -PIDValue); moveMotor(left, forward, -PIDValue); }}

33

Appendix D: Sonar.ino/*-----------Functions------------*/ //Function to print the arrays.void printArray(int *a, int n) {

for (int i = 0; i < n; i++) { Serial.print(a[i], DEC); Serial.print(' '); }

Serial.println();

}

//Sorting function// sort function (Author: Bill Gentles, Nov. 12, 2010)void isort(int *a, int n){// *a is an array pointer function

for (int i = 1; i < n; ++i) { int j = a[i]; int k; for (k = i - 1; (k >= 0) && (j < a[k]); k--) { a[k + 1] = a[k]; } a[k + 1] = j; }

}

//Mode function, returning the mode or median.int mode(int *x,int n){

int i = 0; int count = 0; int maxCount = 0; int mode = 0; int bimodal; int prevCount = 0;

while(i<(n-1)){

prevCount=count; count=0;

34

while(x[i]==x[i+1]){

count++; i++; }

if(count>prevCount&count>maxCount){ mode=x[i]; maxCount=count; bimodal=0; } if(count==0){ i++; } if(count==maxCount){//If the dataset has 2 or more modes. bimodal=1; } if(mode==0||bimodal==1){//Return the median if there is no mode. mode=x[(n/2)]; } return mode; }

}

35