main.cpp

00001 #include "mbed.h"
00002 #include "Motor.h"
00003 #include "LSM9DS1.h"
00004 #include "math.h"
00005 
00006 #define PI 3.141592653589793
00007 #define PERIOD 0.001 //NOTE: Max sample rate is 952 Hz
00008 
00009 Serial pc(USBTX, USBRX);
00010 LSM9DS1 imu(p28, p27, 0xD6, 0x3C);
00011 Motor L(p22, p5, p6); // pwm, fwd, rev
00012 Motor R(p21, p8, p7);
00013 DigitalOut myled(LED1);
00014 
00015 float angle;
00016 float gOffset = 0;
00017 float aOffset = 0;
00018 
00019 int kp = 48; //Best performance so far acheived with kp and speedCorrection
00020 int ki;
00021 int kd;
00022 int factor;
00023 float speedCorrection = 2.0;
00024 float speed;
00025 
00026 void myCalibrate();
00027 void control();
00028 void drive();
00029 void callback();
00030 
00031 int main() {
00032     //LSM9DS1 imu(p9, p10, 0x6B, 0x1E);
00033     imu.begin();
00034     if (!imu.begin()) {
00035         pc.printf("Failed to communicate with LSM9DS1.\n");
00036     }
00037     imu.calibrate();
00038     imu.setAccelScale(2);
00039     imu.setGyroScale(2);
00040     pc.attach(&callback);
00041     myCalibrate();
00042     drive();
00043 }
00044 
00045 void control() {
00046 }
00047     
00048 void drive() {
00049     float accelAngle;
00050     float gyroAngle;
00051     float pastAccAngle = 0;
00052     float pastRawGyro = 0;
00053     float pastGyroAngle = 0;
00054     float temp;
00055     int counter = 0;
00056     float alpha = 0.50; // = T/(T+dt) where T is response time
00057     float beta = 1; // Decides how much the gyroscope data is trusted. b/t 0.9 & 1
00058     float integral = 0;
00059     float derivative = 0;
00060     float lastAngle = 0;
00061     factor = 1000;
00062     while(1) {
00063         counter++;
00064         accelAngle = 0;
00065         for (int i = 0; i < 10; i++) {
00066             imu.readAccel();
00067             imu.readGyro();
00068             accelAngle += ::atan2((float) imu.ax, (float) imu.az)*180/PI - 90;
00069             //The max we will see for this application is about 16380 (1 G), but max is 2 G's (32767 in 16 bits):
00070             gyroAngle += (1/2)*imu.gy + gOffset;
00071             wait(PERIOD);
00072         }
00073         accelAngle = accelAngle/10.0 + aOffset; //Averaging accel to get rid of some noise
00074         gyroAngle = (gyroAngle)*PERIOD - pastGyroAngle;         //Sum over a set time (10 ms) and multiply by differential time (1 ms) to integrate angular velocity
00075 
00076         temp = gyroAngle;
00077         accelAngle = (1 - alpha)*accelAngle + alpha*pastAccAngle; //Low Pass Filter
00078         //gyroAngle = (1 - alpha)*pastGyroAngle + (1 - alpha)*(gyroAngle - pastRawGyro); //High pass filter
00079         pastRawGyro = temp;
00080         angle = accelAngle*beta + gyroAngle*(1 - beta);
00081         angle -= 2; //Attempt to correct for weight distribution bias
00082         
00083         myled = 1;
00084         if (counter % (int) (0.5/(PERIOD*10)) == 0) {
00085             pc.printf("Angle by acc: %.3f\n\r", accelAngle);
00086             pc.printf("Angle by gyro: %.3f\n\r", gyroAngle);
00087             pc.printf("Overall %.3f\n\r", angle);
00088         }
00089         integral += angle*PERIOD;
00090         derivative = angle - lastAngle;
00091         speed = (kp*angle + ki*integral + kd*derivative)/factor;
00092         speed = abs(angle) > 80 ? 0 : speed;
00093         pastAccAngle = accelAngle;
00094         pastGyroAngle = gyroAngle;
00095 
00096         if (speed < 0) {
00097             speed *= speedCorrection; //Speed Correction attempts to correct weight distribution issue
00098         }                             //by driving faster when falling on the heavier side
00099         if (speed > 1) {
00100             speed = 1;
00101         } else if(speed < -1) {
00102             speed = -1;
00103         }
00104         if (counter % (int) (0.5/(PERIOD*10)) == 0) {
00105             pc.printf("speed: %.3f\n\n\r", speed);
00106         }
00107         speed *= -1;        //Had to correct direction after moving accelerometer to opposite side
00108         L.speed(speed);
00109         R.speed(speed);
00110         lastAngle = angle;
00111     }
00112 }
00113         
00114 void myCalibrate() {
00115     //Get linear offsets for accelerometer and gyroscope
00116     float gSum = 0;
00117     for (int i = 0; i < 100; i++) {
00118         imu.readAccel();
00119         imu.readGyro();
00120         gSum += imu.gy;
00121         aOffset += ::atan2((float) imu.ax, (float) imu.az)*180/PI;
00122         wait(PERIOD);
00123     }
00124     aOffset = - aOffset/100 ;
00125     gOffset = - gSum/100 ;
00126     pc.printf("Accelerometer offset: %.3f \n\r", aOffset);
00127     pc.printf("Gyroscope offset: %.3f \n\r", gOffset);
00128     wait(2);
00129 }
00130 
00131 //This function is a revised version of that from:
00132 // https://os.mbed.com/teams/ECE-4180-Spring-15/code/balance2/
00133 void callback()
00134 {
00135     speed = 0;
00136     char val;                                                   // Needed for Serial communication (need to be global?)
00137     val = pc.getc();                                            // Reat the value from Serial
00138     pc.printf("%c\n", val);                                     // Print it back to the screen
00139     if( val =='p') {                                            // If character was a 'p'
00140         pc.printf("enter kp \n");                               // Adjust kp
00141         val = pc.getc();                                        // Wait for kp value
00142         if(val == 0x2b) {                                       // If character is a plus sign
00143             kp++;                                           // Increase kp
00144         } else if (val == 0x2d) {                               // If recieved character is the minus sign
00145             kp--;
00146         } else if (val == '(') {
00147             kp-= 10;
00148         } else if (val == ')') {
00149             kp += 10;                                           // Decrease kp
00150         } else {
00151             kp = val - 48;                                      // Cast char to float
00152         }
00153         pc.printf(" kp = %d \n",kp);                            // Print current kp value to screen
00154     } else if( val == 'd') {                                    // Adjust kd
00155         pc.printf("enter kd \n");                               // Wait for kd
00156         val= pc.getc();                                         // Read value from serial
00157         if(val == '+') {                                        // If given plus sign
00158             kd++;                                               // Increase kd
00159         } else if (val == '-') {                                // If given negative sign
00160             kd--;                                               // Decrease kd
00161         } else {                                                // If given some other ascii (a number?)
00162             kd = val - 48;                                      // Set derivative gain
00163         }
00164         pc.printf(" kd = %d \n",kd);                            // Print kd back to screen
00165     } else if( val == 'i') {                                    // If given i - integral gain
00166         pc.printf("enter ki \n");                               // Prompt on screen to ask for ascii
00167         val= pc.getc();                                         // Get the input
00168         if(val == '+') {                                        // If given the plus sign
00169             ki++;                                               // Increase ki
00170         } else if (val == '-') {                                // If given the minus sign
00171             ki--;                                               // Decrease ki
00172         } else {                                                // If given some other ascii
00173             ki = val - 48;                                      // Set ki to that number
00174         }
00175         pc.printf(" ki = %d \n",ki);
00176     } else if( val == 'o') {
00177         pc.printf("enter factor \n");
00178         val= pc.getc();
00179         if(val == '+') {
00180             factor=factor+1000;
00181         } else if (val == '-') {
00182             factor=factor-1000;
00183         } else {
00184             factor=(val-48)*1000;;
00185         }
00186         pc.printf(" factor = %d \n",factor);
00187     } else if (val == 'a') {
00188         pc.printf("enter speed correction \n");
00189         val= pc.getc();
00190         if(val == '+') {
00191             speedCorrection += 0.1;
00192         } else if (val == '-') {
00193             speedCorrection -= 0.1;
00194         }
00195          pc.printf("speedCorrect = %f \n",speedCorrection);
00196     }
00197     wait(1);
00198 }