Mukund Pandiri / Mbed 2 deprecated segway-balancing

self-balancing-robot

Dependencies:   mbed mbed-rtos Motor

main.cpp@41:b9c8d527dd2b, 2020-04-27 (annotated)

Committer:
pandirimukund
Date:
Mon Apr 27 16:57:40 2020 +0000
Revision:
41:b9c8d527dd2b
Parent:
39:80b565a355f3 
added comments and cleaned up code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
emilmont 1:491820ee784d 1 #include "mbed.h"
mbed_official 11:0309bef74ba8 2 #include "rtos.h"
pandirimukund 13:8d8ac3189984 3 #include "LSM9DS1.h"
pandirimukund 30:b1718c4edcd7 4 #include "Motor.h"
pandirimukund 12:1fc4b3d94397 5
pandirimukund 12:1fc4b3d94397 6
pandirimukund 12:1fc4b3d94397 7 ///////////////////////////////////////////////////////////////////////////////
pandirimukund 12:1fc4b3d94397 8 ///////////////////////////// Variable Initialization//////////////////////////
pandirimukund 12:1fc4b3d94397 9 ///////////////////////////////////////////////////////////////////////////////
pandirimukund 41:b9c8d527dd2b 10 Serial pc(USBTX, USBRX); // serial setup for debug
pandirimukund 12:1fc4b3d94397 11
pandirimukund 13:8d8ac3189984 12
pandirimukund 41:b9c8d527dd2b 13 // Setup mutexes for the parameters and the angle values
pandirimukund 12:1fc4b3d94397 14 Mutex parametersmutex;
pandirimukund 15:f0f19572c4a5 15 Mutex angleMutex;
pandirimukund 41:b9c8d527dd2b 16
pandirimukund 12:1fc4b3d94397 17
pandirimukund 41:b9c8d527dd2b 18 Serial blue(p28, p27); // setup bluetooth device
pandirimukund 41:b9c8d527dd2b 19
pandirimukund 41:b9c8d527dd2b 20 LSM9DS1 imu(p9, p10, 0xD6, 0x3C); // setup IMU
pandirimukund 12:1fc4b3d94397 21
pandirimukund 12:1fc4b3d94397 22
pandirimukund 12:1fc4b3d94397 23 ///////////////////////////////////////////////////////////////////////////////
pandirimukund 12:1fc4b3d94397 24 ///////////////////////////// Control System Variables/////////////////////////
pandirimukund 12:1fc4b3d94397 25 ///////////////////////////////////////////////////////////////////////////////
pandirimukund 12:1fc4b3d94397 26
pandirimukund 41:b9c8d527dd2b 27 // parameters for control system
pandirimukund 41:b9c8d527dd2b 28 float rp = 50; // proportion parameter
pandirimukund 41:b9c8d527dd2b 29 float rd = 60; // derivative parameter
pandirimukund 41:b9c8d527dd2b 30 float ri = 20; // integral parameter
pandirimukund 41:b9c8d527dd2b 31 float desired_angle = 0; // the angle the bot should lean at
pandirimukund 41:b9c8d527dd2b 32
pandirimukund 41:b9c8d527dd2b 33 volatile float speed = 0; // speed of the robot
pandirimukund 41:b9c8d527dd2b 34 volatile float turnSpeed = 0; // turn speed when turning
pandirimukund 12:1fc4b3d94397 35
pandirimukund 41:b9c8d527dd2b 36 float pAngle = 0; // angle error
pandirimukund 41:b9c8d527dd2b 37 float dAngle = 0; // change in angle error
pandirimukund 41:b9c8d527dd2b 38 float iAngle = 0; // integral of angle error
pandirimukund 12:1fc4b3d94397 39
pandirimukund 12:1fc4b3d94397 40
pandirimukund 41:b9c8d527dd2b 41 float angleBias = 0; // the angle bias set during calibration
pandirimukund 12:1fc4b3d94397 42
pandirimukund 41:b9c8d527dd2b 43 Motor leftWheel(p21, p24, p23); // pwm, fwd, rev leftWheel(p21);
pandirimukund 41:b9c8d527dd2b 44 Motor rightWheel(p22, p25, p26); // pwm, fwd, rev rightWheel(p22);
pandirimukund 26:dcf173d2904f 45
pandirimukund 39:80b565a355f3 46 bool isTurning = false;
pandirimukund 39:80b565a355f3 47
pandirimukund 12:1fc4b3d94397 48
pandirimukund 41:b9c8d527dd2b 49
pandirimukund 41:b9c8d527dd2b 50 ///////////////////////////////////////////////////////////////////////////////
pandirimukund 41:b9c8d527dd2b 51 ///////////////////////////// Defining Function ///////////////////////////////
pandirimukund 41:b9c8d527dd2b 52 ///////////////////////////////////////////////////////////////////////////////
pandirimukund 41:b9c8d527dd2b 53 void get_current_angle();
pandirimukund 41:b9c8d527dd2b 54
pandirimukund 41:b9c8d527dd2b 55
pandirimukund 12:1fc4b3d94397 56 ///////////////////////////////////////////////////////////////////////////////
pandirimukund 12:1fc4b3d94397 57 ///////////////////////////// Bluetooth Section ///////////////////////////////
pandirimukund 12:1fc4b3d94397 58 ///////////////////////////////////////////////////////////////////////////////
pandirimukund 12:1fc4b3d94397 59 void bluetooth_update() {
pandirimukund 12:1fc4b3d94397 60 char bnum = 0;
pandirimukund 12:1fc4b3d94397 61 char bhit = 0;
pandirimukund 12:1fc4b3d94397 62 while (1) {
pandirimukund 12:1fc4b3d94397 63 if (blue.getc() == '!') {
pandirimukund 12:1fc4b3d94397 64 if (blue.getc() == 'B') { //button data packet
pandirimukund 12:1fc4b3d94397 65 bnum = blue.getc(); //button number
pandirimukund 12:1fc4b3d94397 66 bhit = blue.getc(); //1=hit, 0=release
pandirimukund 41:b9c8d527dd2b 67 parametersmutex.lock(); // set mutex since parameters are being updated
pandirimukund 12:1fc4b3d94397 68 if (blue.getc() == char(~('!' + 'B' + bnum + bhit))) { //checksum OK?
pandirimukund 12:1fc4b3d94397 69 switch (bnum) {
pandirimukund 12:1fc4b3d94397 70 case '1': //number button 1
pandirimukund 12:1fc4b3d94397 71 if (bhit == '1') {
pandirimukund 41:b9c8d527dd2b 72 rd += 10;
pandirimukund 12:1fc4b3d94397 73 } else {
pandirimukund 12:1fc4b3d94397 74 //add release code here
pandirimukund 12:1fc4b3d94397 75 }
pandirimukund 12:1fc4b3d94397 76 break;
pandirimukund 12:1fc4b3d94397 77 case '2': //number button 2
pandirimukund 12:1fc4b3d94397 78 if (bhit == '1') {
pandirimukund 41:b9c8d527dd2b 79 ri += 10;
pandirimukund 12:1fc4b3d94397 80 } else {
pandirimukund 12:1fc4b3d94397 81 //add release code here
pandirimukund 12:1fc4b3d94397 82 }
pandirimukund 12:1fc4b3d94397 83 break;
pandirimukund 12:1fc4b3d94397 84 case '3': //number button 3
pandirimukund 12:1fc4b3d94397 85 if (bhit == '1') {
pandirimukund 41:b9c8d527dd2b 86 rd -= 10;
pandirimukund 12:1fc4b3d94397 87 } else {
pandirimukund 12:1fc4b3d94397 88 //add release code here
pandirimukund 12:1fc4b3d94397 89 }
pandirimukund 12:1fc4b3d94397 90 break;
pandirimukund 12:1fc4b3d94397 91 case '4': //number button 4
pandirimukund 12:1fc4b3d94397 92 if (bhit == '1') {
pandirimukund 41:b9c8d527dd2b 93 ri -= 10;
pandirimukund 12:1fc4b3d94397 94 } else {
pandirimukund 12:1fc4b3d94397 95 //add release code here
pandirimukund 12:1fc4b3d94397 96 }
pandirimukund 12:1fc4b3d94397 97 break;
pandirimukund 12:1fc4b3d94397 98 case '5': //button 5 up arrow
pandirimukund 12:1fc4b3d94397 99 if (bhit == '1') {
pandirimukund 41:b9c8d527dd2b 100 rp += 10;
pandirimukund 12:1fc4b3d94397 101 } else {
pandirimukund 12:1fc4b3d94397 102 //add release code here
pandirimukund 12:1fc4b3d94397 103 }
pandirimukund 12:1fc4b3d94397 104 break;
pandirimukund 12:1fc4b3d94397 105 case '6': //button 6 down arrow
pandirimukund 12:1fc4b3d94397 106 if (bhit == '1') {
pandirimukund 41:b9c8d527dd2b 107 rp -= 10;
pandirimukund 12:1fc4b3d94397 108 } else {
pandirimukund 12:1fc4b3d94397 109 //add release code here
pandirimukund 12:1fc4b3d94397 110 }
pandirimukund 12:1fc4b3d94397 111 break;
pandirimukund 12:1fc4b3d94397 112 case '7': //button 7 left arrow
pandirimukund 12:1fc4b3d94397 113 if (bhit == '1') {
pandirimukund 41:b9c8d527dd2b 114 // desired_angle -= 2; //uncomment to change desired angle on press
pandirimukund 41:b9c8d527dd2b 115 turnSpeed = .3; // comment to turn off turning
pandirimukund 39:80b565a355f3 116 isTurning = !isTurning;
pandirimukund 12:1fc4b3d94397 117 } else {
pandirimukund 12:1fc4b3d94397 118 //add release code here
pandirimukund 12:1fc4b3d94397 119 }
pandirimukund 12:1fc4b3d94397 120 break;
pandirimukund 12:1fc4b3d94397 121 case '8': //button 8 right arrow
pandirimukund 12:1fc4b3d94397 122 if (bhit == '1') {
pandirimukund 41:b9c8d527dd2b 123 // desired_angle += 2; //uncomment to change desired angle on press
pandirimukund 41:b9c8d527dd2b 124 turnSpeed = .3; // comment to turn off turning
pandirimukund 39:80b565a355f3 125 isTurning = !isTurning;
pandirimukund 12:1fc4b3d94397 126 } else {
pandirimukund 12:1fc4b3d94397 127 //add release code here
pandirimukund 12:1fc4b3d94397 128 }
pandirimukund 12:1fc4b3d94397 129 break;
pandirimukund 12:1fc4b3d94397 130 default:
pandirimukund 12:1fc4b3d94397 131 break;
pandirimukund 12:1fc4b3d94397 132 }
pandirimukund 12:1fc4b3d94397 133 }
pandirimukund 41:b9c8d527dd2b 134 parametersmutex.unlock(); // unlocking parameters mutex
pandirimukund 12:1fc4b3d94397 135 }
pandirimukund 12:1fc4b3d94397 136 }
pandirimukund 41:b9c8d527dd2b 137 Thread::wait(100); // read bluetooth every tenth of a second
emilmont 1:491820ee784d 138 }
emilmont 1:491820ee784d 139 }
pandirimukund 12:1fc4b3d94397 140
pandirimukund 25:154c74800ade 141 ///////////////////////////////////////////////////////////////////////////////
pandirimukund 25:154c74800ade 142 ///////////////////////////// update motor speeds///////////////////////////
pandirimukund 25:154c74800ade 143 ///////////////////////////////////////////////////////////////////////////////
pandirimukund 25:154c74800ade 144 void set_wheel_speed(float speed) {
pandirimukund 41:b9c8d527dd2b 145 if (isTurning) { // set bot to turning if enabled
pandirimukund 39:80b565a355f3 146 leftWheel.speed(turnSpeed*(1));
pandirimukund 39:80b565a355f3 147 rightWheel.speed(turnSpeed*(-1));
pandirimukund 41:b9c8d527dd2b 148 } else { // otherwise set the wheel speeds to the current speeds and max out at 1 (library requirements)
pandirimukund 39:80b565a355f3 149 if(speed > 1) speed = 1;
pandirimukund 39:80b565a355f3 150 if (speed < -1) speed = -1;
pandirimukund 39:80b565a355f3 151 leftWheel.speed(speed*(-1));
pandirimukund 39:80b565a355f3 152 rightWheel.speed(speed*(-1));
pandirimukund 39:80b565a355f3 153 }
pandirimukund 25:154c74800ade 154 }
pandirimukund 25:154c74800ade 155
pandirimukund 25:154c74800ade 156
pandirimukund 12:1fc4b3d94397 157
pandirimukund 12:1fc4b3d94397 158 ///////////////////////////////////////////////////////////////////////////////
pandirimukund 12:1fc4b3d94397 159 ///////////////////////////// Control System Updates///////////////////////////
pandirimukund 12:1fc4b3d94397 160 ///////////////////////////////////////////////////////////////////////////////
pandirimukund 12:1fc4b3d94397 161 //make the calls to IMU here and this should be another thread
pandirimukund 12:1fc4b3d94397 162 void update_system() {
lrucker7 16:f9e3df933304 163 while(1){
pandirimukund 41:b9c8d527dd2b 164 get_current_angle(); // get the current angle from function
pandirimukund 25:154c74800ade 165
pandirimukund 41:b9c8d527dd2b 166 angleMutex.lock(); // going to update angle values so lock mutex
pandirimukund 41:b9c8d527dd2b 167 speed = -1*(rp*pAngle + ri*iAngle + rd*dAngle)/(70 * 150 * 3); //set the speed based on angle error values
pandirimukund 41:b9c8d527dd2b 168 set_wheel_speed(speed); // set the actual wheel speed
pandirimukund 41:b9c8d527dd2b 169 angleMutex.unlock(); // unlock the mutex
pandirimukund 26:dcf173d2904f 170
pandirimukund 41:b9c8d527dd2b 171 Thread::wait(10); // update the sysetem every one hundreth of a second
lrucker7 16:f9e3df933304 172 }
pandirimukund 12:1fc4b3d94397 173 }
pandirimukund 12:1fc4b3d94397 174
pandirimukund 12:1fc4b3d94397 175
pandirimukund 12:1fc4b3d94397 176 ///////////////////////////////////////////////////////////////////////////////
pandirimukund 12:1fc4b3d94397 177 ///////////////////////////// IMU STUFF////////////////////////////////////////
pandirimukund 12:1fc4b3d94397 178 ///////////////////////////////////////////////////////////////////////////////
pandirimukund 41:b9c8d527dd2b 179
pandirimukund 41:b9c8d527dd2b 180 //used to find the angle bias
pandirimukund 12:1fc4b3d94397 181 void calibrate_imu() {
pandirimukund 17:afde478daa01 182 for(int i = 0; i < 500; i++){
pandirimukund 17:afde478daa01 183 imu.readGyro();
pandirimukund 17:afde478daa01 184 angleBias += imu.gy;
pandirimukund 17:afde478daa01 185 }
pandirimukund 17:afde478daa01 186 angleBias /= 500;
pandirimukund 12:1fc4b3d94397 187 }
pandirimukund 12:1fc4b3d94397 188
pandirimukund 41:b9c8d527dd2b 189
pandirimukund 41:b9c8d527dd2b 190
pandirimukund 12:1fc4b3d94397 191 void get_current_angle() {
pandirimukund 41:b9c8d527dd2b 192 imu.readGyro(); // read the gyro to get velocity of angle
pandirimukund 41:b9c8d527dd2b 193 int gyro = -(imu.gy-angleBias) * .01; // multiply by time to get the change in angle from before
lrucker7 16:f9e3df933304 194 //pc.printf("gyro:%f",gyro);
pandirimukund 41:b9c8d527dd2b 195 angleMutex.lock(); // lock angle mutex
pandirimukund 41:b9c8d527dd2b 196 dAngle = pAngle; // save the old angle to be used later
pandirimukund 27:d053966e9320 197
pandirimukund 41:b9c8d527dd2b 198 pAngle += 1*gyro; //+ 0.02*acc; //update the proportion error based on gyro
pandirimukund 41:b9c8d527dd2b 199 pAngle -= desired_angle*70; // get error based on the desired angle - 70 just random constant that worked well
pandirimukund 27:d053966e9320 200
pandirimukund 41:b9c8d527dd2b 201 dAngle = pAngle - dAngle; // get the change in angle error
pandirimukund 27:d053966e9320 202
pandirimukund 41:b9c8d527dd2b 203 iAngle += pAngle * 0.01; // update the integral of the angle error
pandirimukund 27:d053966e9320 204
pandirimukund 41:b9c8d527dd2b 205 angleMutex.unlock(); // unlock the mutex
pandirimukund 12:1fc4b3d94397 206 }
pandirimukund 12:1fc4b3d94397 207
pandirimukund 12:1fc4b3d94397 208
pandirimukund 12:1fc4b3d94397 209
pandirimukund 12:1fc4b3d94397 210
pandirimukund 12:1fc4b3d94397 211 ///////////////////////////////////////////////////////////////////////////////
pandirimukund 12:1fc4b3d94397 212 ///////////////////////////// Running Main Function////////////////////////////
pandirimukund 12:1fc4b3d94397 213 ///////////////////////////////////////////////////////////////////////////////
pandirimukund 12:1fc4b3d94397 214
emilmont 1:491820ee784d 215 int main() {
pandirimukund 41:b9c8d527dd2b 216
pandirimukund 41:b9c8d527dd2b 217 // create the threads
pandirimukund 12:1fc4b3d94397 218 Thread bluetooth;
pandirimukund 12:1fc4b3d94397 219 Thread system_update;
emilmont 1:491820ee784d 220
pandirimukund 41:b9c8d527dd2b 221 // start and calibrate imu
pandirimukund 14:f9c2cf6643cf 222 imu.begin();
lrucker7 23:d2dcd3f9c309 223 calibrate_imu();
pandirimukund 41:b9c8d527dd2b 224
pandirimukund 41:b9c8d527dd2b 225 // start the threads
pandirimukund 12:1fc4b3d94397 226 bluetooth.start(bluetooth_update);
pandirimukund 12:1fc4b3d94397 227 system_update.start(update_system);
pandirimukund 12:1fc4b3d94397 228
pandirimukund 12:1fc4b3d94397 229 while (1) {
pandirimukund 12:1fc4b3d94397 230
pandirimukund 41:b9c8d527dd2b 231 // print out values to debug
pandirimukund 15:f0f19572c4a5 232 angleMutex.lock();
lrucker7 29:ed9f7144a6a7 233 pc.printf("pAngle: %f", pAngle/70);
pandirimukund 34:d6c0cf60cecd 234 pc.printf(" speed: %f", speed);
pandirimukund 34:d6c0cf60cecd 235 parametersmutex.lock();
pandirimukund 34:d6c0cf60cecd 236 pc.printf("rp: %f, rd: %f, ri: %f, desired_angle: %f\n\r", rp, rd, ri, desired_angle);
pandirimukund 34:d6c0cf60cecd 237 parametersmutex.unlock();
pandirimukund 15:f0f19572c4a5 238 angleMutex.unlock();
pandirimukund 15:f0f19572c4a5 239
pandirimukund 15:f0f19572c4a5 240
pandirimukund 14:f9c2cf6643cf 241
pandirimukund 41:b9c8d527dd2b 242 Thread::wait(100); //print out 10 times a second
emilmont 1:491820ee784d 243 }
emilmont 1:491820ee784d 244 }
pandirimukund 12:1fc4b3d94397 245