Revision 41:b9c8d527dd2b, committed 2020-04-27
- Comitter:
- pandirimukund
- Date:
- Mon Apr 27 16:57:40 2020 +0000
- Parent:
- 39:80b565a355f3
- Commit message:
- added comments and cleaned up code
Changed in this revision
--- 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
}
}
Mukund Pandiri
