Mukund Pandiri / Mbed 2 deprecated segway-balancing

self-balancing-robot

Dependencies:   mbed mbed-rtos Motor

main.cpp@35:02166ac9de6f, 2020-04-24 (annotated)

Committer:
pandirimukund
Date:
Fri Apr 24 19:49:41 2020 +0000
Revision:
35:02166ac9de6f
Parent:
34:d6c0cf60cecd
Child:
36:e1bbf24a1b66
added some code to make updating easier

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 12:1fc4b3d94397 10 Ticker bluetooth;
pandirimukund 12:1fc4b3d94397 11 Serial pc(USBTX, USBRX);
pandirimukund 12:1fc4b3d94397 12
pandirimukund 13:8d8ac3189984 13
pandirimukund 12:1fc4b3d94397 14 Mutex parametersmutex;
pandirimukund 15:f0f19572c4a5 15 Mutex angleMutex;
pandirimukund 35:02166ac9de6f 16 Mutex imuMutex;
pandirimukund 35:02166ac9de6f 17
pandirimukund 12:1fc4b3d94397 18 Serial blue(p28, p27);
pandirimukund 12:1fc4b3d94397 19
pandirimukund 13:8d8ac3189984 20 LSM9DS1 imu(p9, p10, 0xD6, 0x3C);
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 float rp = 50;
pandirimukund 34:d6c0cf60cecd 27 float rd = 60;
pandirimukund 34:d6c0cf60cecd 28 float ri = 20;
pandirimukund 12:1fc4b3d94397 29 float desired_angle = 0;
pandirimukund 12:1fc4b3d94397 30
pandirimukund 12:1fc4b3d94397 31 float speed = 0;
pandirimukund 12:1fc4b3d94397 32
pandirimukund 12:1fc4b3d94397 33 float pAngle = 0;
pandirimukund 12:1fc4b3d94397 34 float dAngle = 0;
pandirimukund 12:1fc4b3d94397 35 float iAngle = 0;
pandirimukund 12:1fc4b3d94397 36
pandirimukund 12:1fc4b3d94397 37 int time_segment = 5; //Update the speed every 5 milliseconds
pandirimukund 12:1fc4b3d94397 38
lrucker7 16:f9e3df933304 39 void get_current_angle();
pandirimukund 12:1fc4b3d94397 40
pandirimukund 17:afde478daa01 41 float angleBias = 0;
pandirimukund 17:afde478daa01 42
pandirimukund 30:b1718c4edcd7 43 Motor leftWheel(p21, p24, p23); // pwm, fwd, rev leftWheel(p21);
pandirimukund 31:aa1ea0973c96 44 Motor rightWheel(p22, p25, p26); // pwm, fwd, rev rightWheel(p22);
pandirimukund 26:dcf173d2904f 45
pandirimukund 35:02166ac9de6f 46 void calibrate_imu();
pandirimukund 35:02166ac9de6f 47
pandirimukund 12:1fc4b3d94397 48
pandirimukund 12:1fc4b3d94397 49 ///////////////////////////////////////////////////////////////////////////////
pandirimukund 12:1fc4b3d94397 50 ///////////////////////////// Bluetooth Section ///////////////////////////////
pandirimukund 12:1fc4b3d94397 51 ///////////////////////////////////////////////////////////////////////////////
pandirimukund 12:1fc4b3d94397 52 void bluetooth_update() {
pandirimukund 12:1fc4b3d94397 53 char bnum = 0;
pandirimukund 12:1fc4b3d94397 54 char bhit = 0;
pandirimukund 12:1fc4b3d94397 55 while (1) {
pandirimukund 12:1fc4b3d94397 56 if (blue.getc() == '!') {
pandirimukund 12:1fc4b3d94397 57 if (blue.getc() == 'B') { //button data packet
pandirimukund 12:1fc4b3d94397 58 bnum = blue.getc(); //button number
pandirimukund 12:1fc4b3d94397 59 //pc.printf("%d",bnum);
pandirimukund 12:1fc4b3d94397 60 bhit = blue.getc(); //1=hit, 0=release
pandirimukund 12:1fc4b3d94397 61 parametersmutex.lock();
pandirimukund 12:1fc4b3d94397 62 if (blue.getc() == char(~('!' + 'B' + bnum + bhit))) { //checksum OK?
pandirimukund 12:1fc4b3d94397 63 switch (bnum) {
pandirimukund 12:1fc4b3d94397 64 case '1': //number button 1
pandirimukund 12:1fc4b3d94397 65 if (bhit == '1') {
pandirimukund 35:02166ac9de6f 66 rd += 10;
pandirimukund 12:1fc4b3d94397 67 } else {
pandirimukund 12:1fc4b3d94397 68 //add release code here
pandirimukund 12:1fc4b3d94397 69 }
pandirimukund 12:1fc4b3d94397 70 break;
pandirimukund 12:1fc4b3d94397 71 case '2': //number button 2
pandirimukund 12:1fc4b3d94397 72 if (bhit == '1') {
pandirimukund 35:02166ac9de6f 73 ri += 10;
pandirimukund 12:1fc4b3d94397 74 } else {
pandirimukund 12:1fc4b3d94397 75 //add release code here
pandirimukund 12:1fc4b3d94397 76 }
pandirimukund 12:1fc4b3d94397 77 break;
pandirimukund 12:1fc4b3d94397 78 case '3': //number button 3
pandirimukund 12:1fc4b3d94397 79 if (bhit == '1') {
pandirimukund 35:02166ac9de6f 80 rd -= 10;
pandirimukund 12:1fc4b3d94397 81 } else {
pandirimukund 12:1fc4b3d94397 82 //add release code here
pandirimukund 12:1fc4b3d94397 83 }
pandirimukund 12:1fc4b3d94397 84 break;
pandirimukund 12:1fc4b3d94397 85 case '4': //number button 4
pandirimukund 12:1fc4b3d94397 86 if (bhit == '1') {
pandirimukund 35:02166ac9de6f 87 ri -= 10;
pandirimukund 12:1fc4b3d94397 88 } else {
pandirimukund 12:1fc4b3d94397 89 //add release code here
pandirimukund 12:1fc4b3d94397 90 }
pandirimukund 12:1fc4b3d94397 91 break;
pandirimukund 12:1fc4b3d94397 92 case '5': //button 5 up arrow
pandirimukund 12:1fc4b3d94397 93 if (bhit == '1') {
pandirimukund 35:02166ac9de6f 94 rp += 10;
pandirimukund 12:1fc4b3d94397 95 } else {
pandirimukund 12:1fc4b3d94397 96 //add release code here
pandirimukund 12:1fc4b3d94397 97 }
pandirimukund 12:1fc4b3d94397 98 break;
pandirimukund 12:1fc4b3d94397 99 case '6': //button 6 down arrow
pandirimukund 12:1fc4b3d94397 100 if (bhit == '1') {
pandirimukund 35:02166ac9de6f 101 rp -= 10;
pandirimukund 12:1fc4b3d94397 102 } else {
pandirimukund 12:1fc4b3d94397 103 //add release code here
pandirimukund 12:1fc4b3d94397 104 }
pandirimukund 12:1fc4b3d94397 105 break;
pandirimukund 12:1fc4b3d94397 106 case '7': //button 7 left arrow
pandirimukund 12:1fc4b3d94397 107 if (bhit == '1') {
pandirimukund 12:1fc4b3d94397 108 desired_angle -= 1;
pandirimukund 12:1fc4b3d94397 109 } else {
pandirimukund 12:1fc4b3d94397 110 //add release code here
pandirimukund 12:1fc4b3d94397 111 }
pandirimukund 12:1fc4b3d94397 112 break;
pandirimukund 12:1fc4b3d94397 113 case '8': //button 8 right arrow
pandirimukund 12:1fc4b3d94397 114 if (bhit == '1') {
pandirimukund 12:1fc4b3d94397 115 desired_angle += 1;
pandirimukund 12:1fc4b3d94397 116 } else {
pandirimukund 12:1fc4b3d94397 117 //add release code here
pandirimukund 12:1fc4b3d94397 118 }
pandirimukund 12:1fc4b3d94397 119 break;
pandirimukund 12:1fc4b3d94397 120 default:
pandirimukund 12:1fc4b3d94397 121 break;
pandirimukund 12:1fc4b3d94397 122 }
pandirimukund 12:1fc4b3d94397 123 }
pandirimukund 35:02166ac9de6f 124 angleMutex.lock();
pandirimukund 35:02166ac9de6f 125 pAngle = 0;
pandirimukund 35:02166ac9de6f 126 dAngle = 0;
pandirimukund 35:02166ac9de6f 127 iAngle = 0;
pandirimukund 35:02166ac9de6f 128
pandirimukund 35:02166ac9de6f 129 imuMutex.lock();
pandirimukund 35:02166ac9de6f 130 calibrate_imu();
pandirimukund 35:02166ac9de6f 131 imuMutex.unlock();
pandirimukund 35:02166ac9de6f 132
pandirimukund 35:02166ac9de6f 133 angleMutex.unlock();
pandirimukund 12:1fc4b3d94397 134 parametersmutex.unlock();
pandirimukund 12:1fc4b3d94397 135 }
pandirimukund 12:1fc4b3d94397 136 }
pandirimukund 12:1fc4b3d94397 137 Thread::wait(100);
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 25:154c74800ade 145 if(speed > 1) speed = 1;
pandirimukund 25:154c74800ade 146 if (speed < -1) speed = -1;
lrucker7 32:c3676d4df853 147 leftWheel.speed(speed*(-1));
lrucker7 32:c3676d4df853 148 rightWheel.speed(speed*(-1));
pandirimukund 25:154c74800ade 149 }
pandirimukund 25:154c74800ade 150
pandirimukund 25:154c74800ade 151
pandirimukund 12:1fc4b3d94397 152
pandirimukund 12:1fc4b3d94397 153 ///////////////////////////////////////////////////////////////////////////////
pandirimukund 12:1fc4b3d94397 154 ///////////////////////////// Control System Updates///////////////////////////
pandirimukund 12:1fc4b3d94397 155 ///////////////////////////////////////////////////////////////////////////////
pandirimukund 12:1fc4b3d94397 156 //make the calls to IMU here and this should be another thread
pandirimukund 12:1fc4b3d94397 157 void update_system() {
lrucker7 16:f9e3df933304 158 while(1){
lrucker7 16:f9e3df933304 159 get_current_angle();
pandirimukund 25:154c74800ade 160
pandirimukund 26:dcf173d2904f 161 angleMutex.lock();
pandirimukund 27:d053966e9320 162 speed = -1*(rp*pAngle + ri*iAngle + rd*dAngle)/(70 * 150 * 3);
pandirimukund 26:dcf173d2904f 163 set_wheel_speed(speed);
pandirimukund 26:dcf173d2904f 164 angleMutex.unlock();
pandirimukund 26:dcf173d2904f 165
lrucker7 16:f9e3df933304 166 //pc.printf("this is running 100");
lrucker7 16:f9e3df933304 167 Thread::wait(10);
lrucker7 16:f9e3df933304 168 }
pandirimukund 12:1fc4b3d94397 169 }
pandirimukund 12:1fc4b3d94397 170
pandirimukund 12:1fc4b3d94397 171
pandirimukund 12:1fc4b3d94397 172 ///////////////////////////////////////////////////////////////////////////////
pandirimukund 12:1fc4b3d94397 173 ///////////////////////////// IMU STUFF////////////////////////////////////////
pandirimukund 12:1fc4b3d94397 174 ///////////////////////////////////////////////////////////////////////////////
pandirimukund 12:1fc4b3d94397 175 void calibrate_imu() {
pandirimukund 17:afde478daa01 176 for(int i = 0; i < 500; i++){
pandirimukund 17:afde478daa01 177 imu.readGyro();
pandirimukund 17:afde478daa01 178 angleBias += imu.gy;
pandirimukund 17:afde478daa01 179 }
pandirimukund 17:afde478daa01 180 angleBias /= 500;
pandirimukund 12:1fc4b3d94397 181 }
pandirimukund 12:1fc4b3d94397 182
pandirimukund 12:1fc4b3d94397 183 void get_current_angle() {
pandirimukund 13:8d8ac3189984 184 // return;
pandirimukund 35:02166ac9de6f 185 imuMutex.lock();
pandirimukund 13:8d8ac3189984 186 imu.readGyro();
pandirimukund 17:afde478daa01 187 int gyro = -(imu.gy-angleBias) * .01;
pandirimukund 35:02166ac9de6f 188 imuMutex.unlock();
lrucker7 16:f9e3df933304 189 //pc.printf("gyro:%f",gyro);
pandirimukund 15:f0f19572c4a5 190 angleMutex.lock();
pandirimukund 27:d053966e9320 191 dAngle = pAngle;
pandirimukund 27:d053966e9320 192
pandirimukund 27:d053966e9320 193 pAngle += 1*gyro; //+ 0.02*acc;
pandirimukund 27:d053966e9320 194 pAngle -= desired_angle*70;
pandirimukund 27:d053966e9320 195
pandirimukund 27:d053966e9320 196 dAngle = pAngle - dAngle;
pandirimukund 27:d053966e9320 197
pandirimukund 27:d053966e9320 198 iAngle += pAngle * 0.01;
pandirimukund 27:d053966e9320 199
pandirimukund 15:f0f19572c4a5 200 angleMutex.unlock();
pandirimukund 12:1fc4b3d94397 201 }
pandirimukund 12:1fc4b3d94397 202
pandirimukund 12:1fc4b3d94397 203
pandirimukund 12:1fc4b3d94397 204
pandirimukund 12:1fc4b3d94397 205
pandirimukund 12:1fc4b3d94397 206 ///////////////////////////////////////////////////////////////////////////////
pandirimukund 12:1fc4b3d94397 207 ///////////////////////////// Running Main Function////////////////////////////
pandirimukund 12:1fc4b3d94397 208 ///////////////////////////////////////////////////////////////////////////////
pandirimukund 12:1fc4b3d94397 209
emilmont 1:491820ee784d 210 int main() {
pandirimukund 12:1fc4b3d94397 211 pc.printf("this is running");
lrucker7 23:d2dcd3f9c309 212
pandirimukund 12:1fc4b3d94397 213 Thread bluetooth;
pandirimukund 12:1fc4b3d94397 214 Thread system_update;
emilmont 1:491820ee784d 215
pandirimukund 14:f9c2cf6643cf 216 imu.begin();
lrucker7 23:d2dcd3f9c309 217 calibrate_imu();
pandirimukund 12:1fc4b3d94397 218 bluetooth.start(bluetooth_update);
pandirimukund 12:1fc4b3d94397 219 system_update.start(update_system);
pandirimukund 12:1fc4b3d94397 220
pandirimukund 12:1fc4b3d94397 221 //bluetooth.attach(&bluetooth_update, 0.1);
pandirimukund 12:1fc4b3d94397 222 while (1) {
pandirimukund 12:1fc4b3d94397 223 //bluetooth_update();
lrucker7 16:f9e3df933304 224 //parametersmutex.lock();
lrucker7 16:f9e3df933304 225 // pc.printf("rp: %f, rd: %f, ri: %f, desired_angle: %f\n", rp, rd, ri, desired_angle);
lrucker7 16:f9e3df933304 226 // parametersmutex.unlock();
pandirimukund 12:1fc4b3d94397 227
pandirimukund 15:f0f19572c4a5 228 angleMutex.lock();
lrucker7 29:ed9f7144a6a7 229 pc.printf("pAngle: %f", pAngle/70);
pandirimukund 34:d6c0cf60cecd 230 pc.printf(" speed: %f", speed);
pandirimukund 34:d6c0cf60cecd 231 parametersmutex.lock();
pandirimukund 34:d6c0cf60cecd 232 pc.printf("rp: %f, rd: %f, ri: %f, desired_angle: %f\n\r", rp, rd, ri, desired_angle);
pandirimukund 34:d6c0cf60cecd 233 parametersmutex.unlock();
pandirimukund 15:f0f19572c4a5 234 angleMutex.unlock();
pandirimukund 15:f0f19572c4a5 235
pandirimukund 15:f0f19572c4a5 236
lrucker7 16:f9e3df933304 237 //get_current_angle();
pandirimukund 14:f9c2cf6643cf 238
pandirimukund 12:1fc4b3d94397 239
pandirimukund 12:1fc4b3d94397 240 Thread::wait(100);
emilmont 1:491820ee784d 241 }
emilmont 1:491820ee784d 242 }
pandirimukund 12:1fc4b3d94397 243