Diff: main.cpp

Revision:

41:b9c8d527dd2b

Parent:

39:80b565a355f3

diff -r 80b565a355f3 -r b9c8d527dd2b main.cpp
--- a/main.cpp	Mon Apr 27 16:28:16 2020 +0000
+++ b/main.cpp	Mon Apr 27 16:57:40 2020 +0000
@@ -7,44 +7,52 @@
 ///////////////////////////////////////////////////////////////////////////////
 ///////////////////////////// Variable Initialization//////////////////////////
 ///////////////////////////////////////////////////////////////////////////////
-Ticker bluetooth;
-Serial pc(USBTX, USBRX);
+Serial pc(USBTX, USBRX);                          // serial setup for debug
 
 
+// Setup mutexes for the parameters and the angle values
 Mutex parametersmutex;
 Mutex angleMutex;
-Serial blue(p28, p27);
+
 
-LSM9DS1 imu(p9, p10, 0xD6, 0x3C);
+Serial blue(p28, p27);                             // setup bluetooth device
+
+LSM9DS1 imu(p9, p10, 0xD6, 0x3C);                  // setup IMU  
 
 
 ///////////////////////////////////////////////////////////////////////////////
 ///////////////////////////// Control System Variables/////////////////////////
 ///////////////////////////////////////////////////////////////////////////////
-float rp = 50;
-float rd = 60;
-float ri = 20;
-float desired_angle = 0;
 
-volatile float speed = 0;
-volatile float turnSpeed = 0;
+// parameters for control system 
+float rp = 50;                              // proportion parameter           
+float rd = 60;                              // derivative parameter
+float ri = 20;                              // integral parameter
+float desired_angle = 0;                    // the angle the bot should lean at
+
+volatile float speed = 0;                   // speed of the robot
+volatile float turnSpeed = 0;               // turn speed when turning
 
-float pAngle = 0;
-float dAngle = 0;
-float iAngle = 0;
+float pAngle = 0;                           // angle error
+float dAngle = 0;                           // change in angle error
+float iAngle = 0;                           // integral of angle error
 
-int time_segment = 5;                        //Update the speed every 5 milliseconds
 
-void get_current_angle();
+float angleBias = 0;                        // the angle bias set during calibration
 
-float angleBias = 0;
-
-Motor leftWheel(p21, p24, p23); // pwm, fwd, rev leftWheel(p21);
-Motor rightWheel(p22, p25, p26); // pwm, fwd, rev rightWheel(p22);
+Motor leftWheel(p21, p24, p23);             // pwm, fwd, rev leftWheel(p21);
+Motor rightWheel(p22, p25, p26);            // pwm, fwd, rev rightWheel(p22);
 
 bool isTurning = false;
 
 
+
+///////////////////////////////////////////////////////////////////////////////
+///////////////////////////// Defining Function ///////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+void get_current_angle();                   
+
+
 ///////////////////////////////////////////////////////////////////////////////
 ///////////////////////////// Bluetooth Section ///////////////////////////////
 ///////////////////////////////////////////////////////////////////////////////
@@ -55,57 +63,56 @@
         if (blue.getc() == '!') {
             if (blue.getc() == 'B') { //button data packet
                 bnum = blue.getc(); //button number
-                //pc.printf("%d",bnum);
                 bhit = blue.getc(); //1=hit, 0=release
-                parametersmutex.lock();
+                parametersmutex.lock();        // set mutex since parameters are being updated
                 if (blue.getc() == char(~('!' + 'B' + bnum + bhit))) { //checksum OK?
                     switch (bnum) {
                     case '1': //number button 1
                         if (bhit == '1') {
-                            rd += 1;
+                            rd += 10;
                         } else {
                             //add release code here
                         }
                         break;
                     case '2': //number button 2
                         if (bhit == '1') {
-                            ri += 1;
+                            ri += 10;
                         } else {
                             //add release code here
                         }
                         break;
                     case '3': //number button 3
                         if (bhit == '1') {
-                            rd -= 1;
+                            rd -= 10;
                         } else {
                             //add release code here
                         }
                         break;
                     case '4': //number button 4
                         if (bhit == '1') {
-                            ri -= 1;
+                            ri -= 10;
                         } else {
                             //add release code here
                         }
                         break;
                     case '5': //button 5 up arrow
                         if (bhit == '1') {
-                            rp += 1;
+                            rp += 10;
                         } else {
                             //add release code here
                         }
                         break;
                     case '6': //button 6 down arrow
                         if (bhit == '1') {
-                            rp -= 1;
+                            rp -= 10;
                         } else {
                             //add release code here
                         }
                         break;
                     case '7': //button 7 left arrow
                         if (bhit == '1') {
-//                            desired_angle -= 1;
-                            turnSpeed = .3;
+//                            desired_angle -= 2;     //uncomment to change desired angle on press
+                            turnSpeed = .3;           // comment to turn off turning
                             isTurning = !isTurning;
                         } else {
                             //add release code here
@@ -113,8 +120,8 @@
                         break;
                     case '8': //button 8 right arrow
                         if (bhit == '1') {
-//                            desired_angle += 1;
-                            turnSpeed = .3;
+//                            desired_angle += 2;     //uncomment to change desired angle on press
+                            turnSpeed = .3;           // comment to turn off turning
                             isTurning = !isTurning;
                         } else {
                             //add release code here
@@ -124,10 +131,10 @@
                         break;
                     }
                 }
-                parametersmutex.unlock();
+                parametersmutex.unlock();            // unlocking parameters mutex
             }
         }
-        Thread::wait(100);
+        Thread::wait(100);                           // read bluetooth every tenth of a second
     }
 }
 
@@ -135,10 +142,10 @@
 ///////////////////////////// update motor speeds///////////////////////////
 ///////////////////////////////////////////////////////////////////////////////
 void set_wheel_speed(float speed) {
-    if (isTurning) {
+    if (isTurning) {     // set bot to turning if enabled
         leftWheel.speed(turnSpeed*(1));
         rightWheel.speed(turnSpeed*(-1));
-    } else {
+    } else {            // otherwise set the wheel speeds to the current speeds and max out at 1 (library requirements)
         if(speed > 1) speed = 1;
         if (speed < -1) speed = -1;
         leftWheel.speed(speed*(-1));
@@ -154,15 +161,14 @@
 //make the calls to IMU here and this should be another thread
 void update_system() {
     while(1){
-        get_current_angle();
+        get_current_angle();                       // get the current angle from function
         
-        angleMutex.lock();
-        speed = -1*(rp*pAngle + ri*iAngle + rd*dAngle)/(70 * 150 * 3);
-        set_wheel_speed(speed); 
-        angleMutex.unlock();
+        angleMutex.lock();                         // going to update angle values so lock mutex
+        speed = -1*(rp*pAngle + ri*iAngle + rd*dAngle)/(70 * 150 * 3);     //set the speed based on angle error values
+        set_wheel_speed(speed);                    // set the actual wheel speed
+        angleMutex.unlock();                       // unlock the mutex
         
-        //pc.printf("this is running 100");
-        Thread::wait(10);
+        Thread::wait(10);                          // update the sysetem every one hundreth of a second
     }
 }
 
@@ -170,6 +176,8 @@
 ///////////////////////////////////////////////////////////////////////////////
 ///////////////////////////// IMU STUFF////////////////////////////////////////
 ///////////////////////////////////////////////////////////////////////////////
+
+//used to find the angle bias
 void calibrate_imu() {
     for(int i = 0; i < 500; i++){
         imu.readGyro();
@@ -178,22 +186,23 @@
     angleBias /= 500;
 }
 
+
+
 void get_current_angle() {
-//    return;
-    imu.readGyro();
-    int gyro = -(imu.gy-angleBias) * .01;
+    imu.readGyro();                                 // read the gyro to get velocity of angle
+    int gyro = -(imu.gy-angleBias) * .01;           // multiply by time to get the change in angle from before
     //pc.printf("gyro:%f",gyro);
-    angleMutex.lock();
-    dAngle = pAngle;
+    angleMutex.lock();                              // lock angle mutex 
+    dAngle = pAngle;                                // save the old angle to be used later
     
-    pAngle += 1*gyro; //+ 0.02*acc;
-    pAngle -= desired_angle*70;
+    pAngle += 1*gyro; //+ 0.02*acc;                 //update the proportion error based on gyro 
+    pAngle -= desired_angle*70;                     // get error based on the desired angle - 70 just random constant that worked well
     
-    dAngle = pAngle - dAngle;
+    dAngle = pAngle - dAngle;                       // get the change in angle error
     
-    iAngle += pAngle * 0.01;
+    iAngle += pAngle * 0.01;                        // update the integral of the angle error
     
-    angleMutex.unlock();
+    angleMutex.unlock();                            // unlock the mutex
 }
 
 
@@ -204,23 +213,22 @@
 ///////////////////////////////////////////////////////////////////////////////
 
 int main() {
-    pc.printf("this is running");
-    
+
+    // create the threads    
     Thread bluetooth;
     Thread system_update;
     
+    // start and calibrate imu
     imu.begin();
     calibrate_imu();
+    
+    // start the threads
     bluetooth.start(bluetooth_update);
     system_update.start(update_system);
     
-    //bluetooth.attach(&bluetooth_update, 0.1);
     while (1) {
-        //bluetooth_update();
-        //parametersmutex.lock();
-//        pc.printf("rp: %f, rd: %f, ri: %f, desired_angle: %f\n", rp, rd, ri, desired_angle);
-//        parametersmutex.unlock();
         
+        // print out values to debug
         angleMutex.lock();
         pc.printf("pAngle: %f", pAngle/70);
         pc.printf("    speed: %f", speed);
@@ -230,10 +238,8 @@
         angleMutex.unlock();
         
         
-        //get_current_angle();
         
-        
-        Thread::wait(100);
+        Thread::wait(100);  //print out 10 times a second
     }
 }