Mukund Pandiri / Mbed 2 deprecated segway-selfBalancingRobot

segway_self balancing robot 4180 project

Dependencies:   mbed mbed-rtos LSM9DS1_Library

main.cpp@14:89bb16d03b15, 2020-04-18 (annotated)

Committer:
pandirimukund
Date:
Sat Apr 18 21:10:35 2020 +0000
Revision:
14:89bb16d03b15
Parent:
13:c00ddcfea79f 
added basic imu functionality;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pandirimukund 0:49139eec4c55 1 #include "mbed.h"
lrucker7 11:c669b4dc1f9f 2 #include "rtos.h"
pandirimukund 14:89bb16d03b15 3 #include "LSM9DS1.h"
pandirimukund 0:49139eec4c55 4
pandirimukund 0:49139eec4c55 5
pandirimukund 13:c00ddcfea79f 6 ///////////////////////////////////////////////////////////////////////////////
pandirimukund 13:c00ddcfea79f 7 ///////////////////////////// Variable Initialization//////////////////////////
pandirimukund 13:c00ddcfea79f 8 ///////////////////////////////////////////////////////////////////////////////
pandirimukund 7:8af26d50c006 9 Ticker bluetooth;
pandirimukund 7:8af26d50c006 10 Serial pc(USBTX, USBRX);
pandirimukund 7:8af26d50c006 11
lrucker7 11:c669b4dc1f9f 12 Mutex parametersmutex;
pandirimukund 13:c00ddcfea79f 13 Serial blue(p28, p27);
pandirimukund 7:8af26d50c006 14
pandirimukund 14:89bb16d03b15 15 LSM9D1 imu(p9, p10, 0xD6, 0x3C);
pandirimukund 14:89bb16d03b15 16
pandirimukund 14:89bb16d03b15 17
pandirimukund 13:c00ddcfea79f 18 ///////////////////////////////////////////////////////////////////////////////
pandirimukund 13:c00ddcfea79f 19 ///////////////////////////// Control System Variables/////////////////////////
pandirimukund 13:c00ddcfea79f 20 ///////////////////////////////////////////////////////////////////////////////
pandirimukund 7:8af26d50c006 21 float rp = 50;
pandirimukund 7:8af26d50c006 22 float rd = 51;
pandirimukund 7:8af26d50c006 23 float ri = 50;
pandirimukund 7:8af26d50c006 24 float desired_angle = 0;
pandirimukund 13:c00ddcfea79f 25
pandirimukund 14:89bb16d03b15 26 float speed = 0;
pandirimukund 14:89bb16d03b15 27
pandirimukund 14:89bb16d03b15 28 float pAngle = 0;
pandirimukund 14:89bb16d03b15 29 float dAngle = 0;
pandirimukund 14:89bb16d03b15 30 float iAngle = 0;
pandirimukund 14:89bb16d03b15 31
pandirimukund 14:89bb16d03b15 32 time_segment = 5; //Update the speed every 5 milliseconds
pandirimukund 14:89bb16d03b15 33
pandirimukund 13:c00ddcfea79f 34
pandirimukund 7:8af26d50c006 35
pandirimukund 13:c00ddcfea79f 36 ///////////////////////////////////////////////////////////////////////////////
pandirimukund 13:c00ddcfea79f 37 ///////////////////////////// Bluetooth Section ///////////////////////////////
pandirimukund 13:c00ddcfea79f 38 ///////////////////////////////////////////////////////////////////////////////
pandirimukund 7:8af26d50c006 39 void bluetooth_update() {
pandirimukund 12:980c06d63425 40 char bnum = 0;
pandirimukund 12:980c06d63425 41 char bhit = 0;
pandirimukund 12:980c06d63425 42 while (1) {
pandirimukund 12:980c06d63425 43 if (blue.getc() == '!') {
pandirimukund 12:980c06d63425 44 if (blue.getc() == 'B') { //button data packet
pandirimukund 12:980c06d63425 45 bnum = blue.getc(); //button number
pandirimukund 12:980c06d63425 46 //pc.printf("%d",bnum);
pandirimukund 12:980c06d63425 47 bhit = blue.getc(); //1=hit, 0=release
pandirimukund 12:980c06d63425 48 parametersmutex.lock();
pandirimukund 12:980c06d63425 49 if (blue.getc() == char(~('!' + 'B' + bnum + bhit))) { //checksum OK?
pandirimukund 12:980c06d63425 50 myled = bnum - '0'; //current button number will appear on LEDs
pandirimukund 12:980c06d63425 51 switch (bnum) {
pandirimukund 7:8af26d50c006 52 case '1': //number button 1
pandirimukund 12:980c06d63425 53 if (bhit == '1') {
pandirimukund 7:8af26d50c006 54 rd += 1;
pandirimukund 7:8af26d50c006 55 } else {
pandirimukund 7:8af26d50c006 56 //add release code here
pandirimukund 7:8af26d50c006 57 }
pandirimukund 7:8af26d50c006 58 break;
pandirimukund 7:8af26d50c006 59 case '2': //number button 2
pandirimukund 12:980c06d63425 60 if (bhit == '1') {
pandirimukund 7:8af26d50c006 61 ri += 1;
pandirimukund 7:8af26d50c006 62 } else {
pandirimukund 7:8af26d50c006 63 //add release code here
pandirimukund 7:8af26d50c006 64 }
pandirimukund 7:8af26d50c006 65 break;
pandirimukund 7:8af26d50c006 66 case '3': //number button 3
pandirimukund 12:980c06d63425 67 if (bhit == '1') {
pandirimukund 7:8af26d50c006 68 rd -= 1;
pandirimukund 7:8af26d50c006 69 } else {
pandirimukund 7:8af26d50c006 70 //add release code here
pandirimukund 7:8af26d50c006 71 }
pandirimukund 7:8af26d50c006 72 break;
pandirimukund 7:8af26d50c006 73 case '4': //number button 4
pandirimukund 12:980c06d63425 74 if (bhit == '1') {
pandirimukund 7:8af26d50c006 75 ri -= 1;
pandirimukund 7:8af26d50c006 76 } else {
pandirimukund 7:8af26d50c006 77 //add release code here
pandirimukund 7:8af26d50c006 78 }
pandirimukund 7:8af26d50c006 79 break;
pandirimukund 7:8af26d50c006 80 case '5': //button 5 up arrow
pandirimukund 12:980c06d63425 81 if (bhit == '1') {
pandirimukund 7:8af26d50c006 82 rp += 1;
pandirimukund 7:8af26d50c006 83 } else {
pandirimukund 7:8af26d50c006 84 //add release code here
pandirimukund 7:8af26d50c006 85 }
pandirimukund 7:8af26d50c006 86 break;
pandirimukund 7:8af26d50c006 87 case '6': //button 6 down arrow
pandirimukund 12:980c06d63425 88 if (bhit == '1') {
pandirimukund 7:8af26d50c006 89 rp -= 1;
pandirimukund 7:8af26d50c006 90 } else {
pandirimukund 7:8af26d50c006 91 //add release code here
pandirimukund 7:8af26d50c006 92 }
pandirimukund 7:8af26d50c006 93 break;
pandirimukund 7:8af26d50c006 94 case '7': //button 7 left arrow
pandirimukund 12:980c06d63425 95 if (bhit == '1') {
pandirimukund 7:8af26d50c006 96 desired_angle -= 1;
pandirimukund 7:8af26d50c006 97 } else {
pandirimukund 7:8af26d50c006 98 //add release code here
pandirimukund 7:8af26d50c006 99 }
pandirimukund 7:8af26d50c006 100 break;
pandirimukund 7:8af26d50c006 101 case '8': //button 8 right arrow
pandirimukund 12:980c06d63425 102 if (bhit == '1') {
pandirimukund 7:8af26d50c006 103 desired_angle += 1;
pandirimukund 7:8af26d50c006 104 } else {
pandirimukund 7:8af26d50c006 105 //add release code here
pandirimukund 7:8af26d50c006 106 }
pandirimukund 7:8af26d50c006 107 break;
pandirimukund 7:8af26d50c006 108 default:
pandirimukund 7:8af26d50c006 109 break;
pandirimukund 12:980c06d63425 110 }
pandirimukund 7:8af26d50c006 111 }
pandirimukund 12:980c06d63425 112 parametersmutex.unlock();
pandirimukund 7:8af26d50c006 113 }
pandirimukund 7:8af26d50c006 114 }
pandirimukund 12:980c06d63425 115 Thread::wait(100);
lrucker7 11:c669b4dc1f9f 116 }
pandirimukund 0:49139eec4c55 117 }
pandirimukund 7:8af26d50c006 118
pandirimukund 13:c00ddcfea79f 119
pandirimukund 13:c00ddcfea79f 120 ///////////////////////////////////////////////////////////////////////////////
pandirimukund 13:c00ddcfea79f 121 ///////////////////////////// Control System Updates///////////////////////////
pandirimukund 13:c00ddcfea79f 122 ///////////////////////////////////////////////////////////////////////////////
pandirimukund 13:c00ddcfea79f 123 //make the calls to IMU here and this should be another thread
pandirimukund 13:c00ddcfea79f 124 void update_system() {
pandirimukund 13:c00ddcfea79f 125 return;
pandirimukund 13:c00ddcfea79f 126 }
pandirimukund 13:c00ddcfea79f 127
pandirimukund 13:c00ddcfea79f 128
pandirimukund 13:c00ddcfea79f 129 ///////////////////////////////////////////////////////////////////////////////
pandirimukund 13:c00ddcfea79f 130 ///////////////////////////// IMU STUFF////////////////////////////////////////
pandirimukund 13:c00ddcfea79f 131 ///////////////////////////////////////////////////////////////////////////////
pandirimukund 13:c00ddcfea79f 132 void calibrate_imu() {
pandirimukund 13:c00ddcfea79f 133 return;
pandirimukund 13:c00ddcfea79f 134 }
pandirimukund 13:c00ddcfea79f 135
pandirimukund 13:c00ddcfea79f 136 void get_current_angle() {
pandirimukund 14:89bb16d03b15 137 imu.readGyro();
pandirimukund 13:c00ddcfea79f 138 }
pandirimukund 13:c00ddcfea79f 139
pandirimukund 13:c00ddcfea79f 140
pandirimukund 13:c00ddcfea79f 141
pandirimukund 13:c00ddcfea79f 142
pandirimukund 13:c00ddcfea79f 143 ///////////////////////////////////////////////////////////////////////////////
pandirimukund 13:c00ddcfea79f 144 ///////////////////////////// Running Main Function////////////////////////////
pandirimukund 13:c00ddcfea79f 145 ///////////////////////////////////////////////////////////////////////////////
pandirimukund 13:c00ddcfea79f 146
pandirimukund 7:8af26d50c006 147 int main() {
pandirimukund 7:8af26d50c006 148 pc.printf("this is running");
lrucker7 11:c669b4dc1f9f 149 Thread bluetooth;
pandirimukund 13:c00ddcfea79f 150 Thread system_update;
pandirimukund 14:89bb16d03b15 151
pandirimukund 14:89bb16d03b15 152 imu.begin();
pandirimukund 14:89bb16d03b15 153
lrucker7 11:c669b4dc1f9f 154 bluetooth.start(bluetooth_update);
pandirimukund 13:c00ddcfea79f 155 system_update.start(update_system);
pandirimukund 14:89bb16d03b15 156
lrucker7 9:28821420f7a4 157 //bluetooth.attach(&bluetooth_update, 0.1);
pandirimukund 12:980c06d63425 158 while (1) {
lrucker7 11:c669b4dc1f9f 159 //bluetooth_update();
lrucker7 11:c669b4dc1f9f 160 parametersmutex.lock();
pandirimukund 14:89bb16d03b15 161 // pc.printf("rp: %f, rd: %f, ri: %f, desired_angle: %f\n", rp, rd, ri, desired_angle);
lrucker7 11:c669b4dc1f9f 162 parametersmutex.unlock();
pandirimukund 14:89bb16d03b15 163
pandirimukund 14:89bb16d03b15 164 get_current_angle();
pandirimukund 14:89bb16d03b15 165 pc.printf("gyro: %d %d %d\n\r", imu.gx, imu.gy, imu.gz);
pandirimukund 14:89bb16d03b15 166
lrucker7 11:c669b4dc1f9f 167 Thread::wait(100);
pandirimukund 7:8af26d50c006 168 }
pandirimukund 7:8af26d50c006 169 }
pandirimukund 7:8af26d50c006 170