Mukund Pandiri / Mbed 2 deprecated segway-balancing

self-balancing-robot

Dependencies:   mbed mbed-rtos Motor

main.cpp@25:154c74800ade, 2020-04-23 (annotated)

Committer:
pandirimukund
Date:
Thu Apr 23 16:42:44 2020 +0000
Revision:
25:154c74800ade
Parent:
23:d2dcd3f9c309
Child:
26:dcf173d2904f
added speed method

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