Mukund Pandiri / Mbed 2 deprecated segway-balancing

self-balancing-robot

Dependencies:   mbed mbed-rtos Motor

main.cpp@30:b1718c4edcd7, 2020-04-23 (annotated)

Committer:
pandirimukund
Date:
Thu Apr 23 20:18:51 2020 +0000
Revision:
30:b1718c4edcd7
Parent:
29:ed9f7144a6a7
Child:
31:aa1ea0973c96
added motor library;

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