Solomon Martin / Mbed 2 deprecated Stabilize

Attempt at controlling a specific single-axle robot to balance, by driving motors using filtered IMU data

Dependencies:   mbed LSM9DS1_Library Motor

main.cpp@0:3cafef4c2519, 2019-12-11 (annotated)

Committer:
Solomon_Martin
Date:
Wed Dec 11 01:39:42 2019 +0000
Revision:
0:3cafef4c2519
Control loop outline & parameter tuning via pc

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Solomon_Martin 0:3cafef4c2519 1 #include "mbed.h"
Solomon_Martin 0:3cafef4c2519 2 #include "Motor.h"
Solomon_Martin 0:3cafef4c2519 3 #include "LSM9DS1.h"
Solomon_Martin 0:3cafef4c2519 4 #include "math.h"
Solomon_Martin 0:3cafef4c2519 5
Solomon_Martin 0:3cafef4c2519 6 #define PI 3.141592653589793
Solomon_Martin 0:3cafef4c2519 7 #define PERIOD 0.001 //NOTE: Max sample rate is 952 Hz
Solomon_Martin 0:3cafef4c2519 8
Solomon_Martin 0:3cafef4c2519 9 Serial pc(USBTX, USBRX);
Solomon_Martin 0:3cafef4c2519 10 LSM9DS1 imu(p28, p27, 0xD6, 0x3C);
Solomon_Martin 0:3cafef4c2519 11 Motor L(p22, p5, p6); // pwm, fwd, rev
Solomon_Martin 0:3cafef4c2519 12 Motor R(p21, p8, p7);
Solomon_Martin 0:3cafef4c2519 13 DigitalOut myled(LED1);
Solomon_Martin 0:3cafef4c2519 14
Solomon_Martin 0:3cafef4c2519 15 float angle;
Solomon_Martin 0:3cafef4c2519 16 float gOffset = 0;
Solomon_Martin 0:3cafef4c2519 17 float aOffset = 0;
Solomon_Martin 0:3cafef4c2519 18
Solomon_Martin 0:3cafef4c2519 19 int kp = 48; //Best performance so far acheived with kp and speedCorrection
Solomon_Martin 0:3cafef4c2519 20 int ki;
Solomon_Martin 0:3cafef4c2519 21 int kd;
Solomon_Martin 0:3cafef4c2519 22 int factor;
Solomon_Martin 0:3cafef4c2519 23 float speedCorrection = 2.0;
Solomon_Martin 0:3cafef4c2519 24 float speed;
Solomon_Martin 0:3cafef4c2519 25
Solomon_Martin 0:3cafef4c2519 26 void myCalibrate();
Solomon_Martin 0:3cafef4c2519 27 void control();
Solomon_Martin 0:3cafef4c2519 28 void drive();
Solomon_Martin 0:3cafef4c2519 29 void callback();
Solomon_Martin 0:3cafef4c2519 30
Solomon_Martin 0:3cafef4c2519 31 int main() {
Solomon_Martin 0:3cafef4c2519 32 //LSM9DS1 imu(p9, p10, 0x6B, 0x1E);
Solomon_Martin 0:3cafef4c2519 33 imu.begin();
Solomon_Martin 0:3cafef4c2519 34 if (!imu.begin()) {
Solomon_Martin 0:3cafef4c2519 35 pc.printf("Failed to communicate with LSM9DS1.\n");
Solomon_Martin 0:3cafef4c2519 36 }
Solomon_Martin 0:3cafef4c2519 37 imu.calibrate();
Solomon_Martin 0:3cafef4c2519 38 imu.setAccelScale(2);
Solomon_Martin 0:3cafef4c2519 39 imu.setGyroScale(2);
Solomon_Martin 0:3cafef4c2519 40 pc.attach(&callback);
Solomon_Martin 0:3cafef4c2519 41 myCalibrate();
Solomon_Martin 0:3cafef4c2519 42 drive();
Solomon_Martin 0:3cafef4c2519 43 }
Solomon_Martin 0:3cafef4c2519 44
Solomon_Martin 0:3cafef4c2519 45 void control() {
Solomon_Martin 0:3cafef4c2519 46 }
Solomon_Martin 0:3cafef4c2519 47
Solomon_Martin 0:3cafef4c2519 48 void drive() {
Solomon_Martin 0:3cafef4c2519 49 float accelAngle;
Solomon_Martin 0:3cafef4c2519 50 float gyroAngle;
Solomon_Martin 0:3cafef4c2519 51 float pastAccAngle = 0;
Solomon_Martin 0:3cafef4c2519 52 float pastRawGyro = 0;
Solomon_Martin 0:3cafef4c2519 53 float pastGyroAngle = 0;
Solomon_Martin 0:3cafef4c2519 54 float temp;
Solomon_Martin 0:3cafef4c2519 55 int counter = 0;
Solomon_Martin 0:3cafef4c2519 56 float alpha = 0.50; // = T/(T+dt) where T is response time
Solomon_Martin 0:3cafef4c2519 57 float beta = 1; // Decides how much the gyroscope data is trusted. b/t 0.9 & 1
Solomon_Martin 0:3cafef4c2519 58 float integral = 0;
Solomon_Martin 0:3cafef4c2519 59 float derivative = 0;
Solomon_Martin 0:3cafef4c2519 60 float lastAngle = 0;
Solomon_Martin 0:3cafef4c2519 61 factor = 1000;
Solomon_Martin 0:3cafef4c2519 62 while(1) {
Solomon_Martin 0:3cafef4c2519 63 counter++;
Solomon_Martin 0:3cafef4c2519 64 accelAngle = 0;
Solomon_Martin 0:3cafef4c2519 65 for (int i = 0; i < 10; i++) {
Solomon_Martin 0:3cafef4c2519 66 imu.readAccel();
Solomon_Martin 0:3cafef4c2519 67 imu.readGyro();
Solomon_Martin 0:3cafef4c2519 68 accelAngle += ::atan2((float) imu.ax, (float) imu.az)*180/PI - 90;
Solomon_Martin 0:3cafef4c2519 69 //The max we will see for this application is about 16380 (1 G), but max is 2 G's (32767 in 16 bits):
Solomon_Martin 0:3cafef4c2519 70 gyroAngle += (1/2)*imu.gy + gOffset;
Solomon_Martin 0:3cafef4c2519 71 wait(PERIOD);
Solomon_Martin 0:3cafef4c2519 72 }
Solomon_Martin 0:3cafef4c2519 73 accelAngle = accelAngle/10.0 + aOffset; //Averaging accel to get rid of some noise
Solomon_Martin 0:3cafef4c2519 74 gyroAngle = (gyroAngle)*PERIOD - pastGyroAngle; //Sum over a set time (10 ms) and multiply by differential time (1 ms) to integrate angular velocity
Solomon_Martin 0:3cafef4c2519 75
Solomon_Martin 0:3cafef4c2519 76 temp = gyroAngle;
Solomon_Martin 0:3cafef4c2519 77 accelAngle = (1 - alpha)*accelAngle + alpha*pastAccAngle; //Low Pass Filter
Solomon_Martin 0:3cafef4c2519 78 //gyroAngle = (1 - alpha)*pastGyroAngle + (1 - alpha)*(gyroAngle - pastRawGyro); //High pass filter
Solomon_Martin 0:3cafef4c2519 79 pastRawGyro = temp;
Solomon_Martin 0:3cafef4c2519 80 angle = accelAngle*beta + gyroAngle*(1 - beta);
Solomon_Martin 0:3cafef4c2519 81 angle -= 2; //Attempt to correct for weight distribution bias
Solomon_Martin 0:3cafef4c2519 82
Solomon_Martin 0:3cafef4c2519 83 myled = 1;
Solomon_Martin 0:3cafef4c2519 84 if (counter % (int) (0.5/(PERIOD*10)) == 0) {
Solomon_Martin 0:3cafef4c2519 85 pc.printf("Angle by acc: %.3f\n\r", accelAngle);
Solomon_Martin 0:3cafef4c2519 86 pc.printf("Angle by gyro: %.3f\n\r", gyroAngle);
Solomon_Martin 0:3cafef4c2519 87 pc.printf("Overall %.3f\n\r", angle);
Solomon_Martin 0:3cafef4c2519 88 }
Solomon_Martin 0:3cafef4c2519 89 integral += angle*PERIOD;
Solomon_Martin 0:3cafef4c2519 90 derivative = angle - lastAngle;
Solomon_Martin 0:3cafef4c2519 91 speed = (kp*angle + ki*integral + kd*derivative)/factor;
Solomon_Martin 0:3cafef4c2519 92 speed = abs(angle) > 80 ? 0 : speed;
Solomon_Martin 0:3cafef4c2519 93 pastAccAngle = accelAngle;
Solomon_Martin 0:3cafef4c2519 94 pastGyroAngle = gyroAngle;
Solomon_Martin 0:3cafef4c2519 95
Solomon_Martin 0:3cafef4c2519 96 if (speed < 0) {
Solomon_Martin 0:3cafef4c2519 97 speed *= speedCorrection; //Speed Correction attempts to correct weight distribution issue
Solomon_Martin 0:3cafef4c2519 98 } //by driving faster when falling on the heavier side
Solomon_Martin 0:3cafef4c2519 99 if (speed > 1) {
Solomon_Martin 0:3cafef4c2519 100 speed = 1;
Solomon_Martin 0:3cafef4c2519 101 } else if(speed < -1) {
Solomon_Martin 0:3cafef4c2519 102 speed = -1;
Solomon_Martin 0:3cafef4c2519 103 }
Solomon_Martin 0:3cafef4c2519 104 if (counter % (int) (0.5/(PERIOD*10)) == 0) {
Solomon_Martin 0:3cafef4c2519 105 pc.printf("speed: %.3f\n\n\r", speed);
Solomon_Martin 0:3cafef4c2519 106 }
Solomon_Martin 0:3cafef4c2519 107 speed *= -1; //Had to correct direction after moving accelerometer to opposite side
Solomon_Martin 0:3cafef4c2519 108 L.speed(speed);
Solomon_Martin 0:3cafef4c2519 109 R.speed(speed);
Solomon_Martin 0:3cafef4c2519 110 lastAngle = angle;
Solomon_Martin 0:3cafef4c2519 111 }
Solomon_Martin 0:3cafef4c2519 112 }
Solomon_Martin 0:3cafef4c2519 113
Solomon_Martin 0:3cafef4c2519 114 void myCalibrate() {
Solomon_Martin 0:3cafef4c2519 115 //Get linear offsets for accelerometer and gyroscope
Solomon_Martin 0:3cafef4c2519 116 float gSum = 0;
Solomon_Martin 0:3cafef4c2519 117 for (int i = 0; i < 100; i++) {
Solomon_Martin 0:3cafef4c2519 118 imu.readAccel();
Solomon_Martin 0:3cafef4c2519 119 imu.readGyro();
Solomon_Martin 0:3cafef4c2519 120 gSum += imu.gy;
Solomon_Martin 0:3cafef4c2519 121 aOffset += ::atan2((float) imu.ax, (float) imu.az)*180/PI;
Solomon_Martin 0:3cafef4c2519 122 wait(PERIOD);
Solomon_Martin 0:3cafef4c2519 123 }
Solomon_Martin 0:3cafef4c2519 124 aOffset = - aOffset/100 ;
Solomon_Martin 0:3cafef4c2519 125 gOffset = - gSum/100 ;
Solomon_Martin 0:3cafef4c2519 126 pc.printf("Accelerometer offset: %.3f \n\r", aOffset);
Solomon_Martin 0:3cafef4c2519 127 pc.printf("Gyroscope offset: %.3f \n\r", gOffset);
Solomon_Martin 0:3cafef4c2519 128 wait(2);
Solomon_Martin 0:3cafef4c2519 129 }
Solomon_Martin 0:3cafef4c2519 130
Solomon_Martin 0:3cafef4c2519 131 //This function is a revised version of that from:
Solomon_Martin 0:3cafef4c2519 132 // https://os.mbed.com/teams/ECE-4180-Spring-15/code/balance2/
Solomon_Martin 0:3cafef4c2519 133 void callback()
Solomon_Martin 0:3cafef4c2519 134 {
Solomon_Martin 0:3cafef4c2519 135 speed = 0;
Solomon_Martin 0:3cafef4c2519 136 char val; // Needed for Serial communication (need to be global?)
Solomon_Martin 0:3cafef4c2519 137 val = pc.getc(); // Reat the value from Serial
Solomon_Martin 0:3cafef4c2519 138 pc.printf("%c\n", val); // Print it back to the screen
Solomon_Martin 0:3cafef4c2519 139 if( val =='p') { // If character was a 'p'
Solomon_Martin 0:3cafef4c2519 140 pc.printf("enter kp \n"); // Adjust kp
Solomon_Martin 0:3cafef4c2519 141 val = pc.getc(); // Wait for kp value
Solomon_Martin 0:3cafef4c2519 142 if(val == 0x2b) { // If character is a plus sign
Solomon_Martin 0:3cafef4c2519 143 kp++; // Increase kp
Solomon_Martin 0:3cafef4c2519 144 } else if (val == 0x2d) { // If recieved character is the minus sign
Solomon_Martin 0:3cafef4c2519 145 kp--;
Solomon_Martin 0:3cafef4c2519 146 } else if (val == '(') {
Solomon_Martin 0:3cafef4c2519 147 kp-= 10;
Solomon_Martin 0:3cafef4c2519 148 } else if (val == ')') {
Solomon_Martin 0:3cafef4c2519 149 kp += 10; // Decrease kp
Solomon_Martin 0:3cafef4c2519 150 } else {
Solomon_Martin 0:3cafef4c2519 151 kp = val - 48; // Cast char to float
Solomon_Martin 0:3cafef4c2519 152 }
Solomon_Martin 0:3cafef4c2519 153 pc.printf(" kp = %d \n",kp); // Print current kp value to screen
Solomon_Martin 0:3cafef4c2519 154 } else if( val == 'd') { // Adjust kd
Solomon_Martin 0:3cafef4c2519 155 pc.printf("enter kd \n"); // Wait for kd
Solomon_Martin 0:3cafef4c2519 156 val= pc.getc(); // Read value from serial
Solomon_Martin 0:3cafef4c2519 157 if(val == '+') { // If given plus sign
Solomon_Martin 0:3cafef4c2519 158 kd++; // Increase kd
Solomon_Martin 0:3cafef4c2519 159 } else if (val == '-') { // If given negative sign
Solomon_Martin 0:3cafef4c2519 160 kd--; // Decrease kd
Solomon_Martin 0:3cafef4c2519 161 } else { // If given some other ascii (a number?)
Solomon_Martin 0:3cafef4c2519 162 kd = val - 48; // Set derivative gain
Solomon_Martin 0:3cafef4c2519 163 }
Solomon_Martin 0:3cafef4c2519 164 pc.printf(" kd = %d \n",kd); // Print kd back to screen
Solomon_Martin 0:3cafef4c2519 165 } else if( val == 'i') { // If given i - integral gain
Solomon_Martin 0:3cafef4c2519 166 pc.printf("enter ki \n"); // Prompt on screen to ask for ascii
Solomon_Martin 0:3cafef4c2519 167 val= pc.getc(); // Get the input
Solomon_Martin 0:3cafef4c2519 168 if(val == '+') { // If given the plus sign
Solomon_Martin 0:3cafef4c2519 169 ki++; // Increase ki
Solomon_Martin 0:3cafef4c2519 170 } else if (val == '-') { // If given the minus sign
Solomon_Martin 0:3cafef4c2519 171 ki--; // Decrease ki
Solomon_Martin 0:3cafef4c2519 172 } else { // If given some other ascii
Solomon_Martin 0:3cafef4c2519 173 ki = val - 48; // Set ki to that number
Solomon_Martin 0:3cafef4c2519 174 }
Solomon_Martin 0:3cafef4c2519 175 pc.printf(" ki = %d \n",ki);
Solomon_Martin 0:3cafef4c2519 176 } else if( val == 'o') {
Solomon_Martin 0:3cafef4c2519 177 pc.printf("enter factor \n");
Solomon_Martin 0:3cafef4c2519 178 val= pc.getc();
Solomon_Martin 0:3cafef4c2519 179 if(val == '+') {
Solomon_Martin 0:3cafef4c2519 180 factor=factor+1000;
Solomon_Martin 0:3cafef4c2519 181 } else if (val == '-') {
Solomon_Martin 0:3cafef4c2519 182 factor=factor-1000;
Solomon_Martin 0:3cafef4c2519 183 } else {
Solomon_Martin 0:3cafef4c2519 184 factor=(val-48)*1000;;
Solomon_Martin 0:3cafef4c2519 185 }
Solomon_Martin 0:3cafef4c2519 186 pc.printf(" factor = %d \n",factor);
Solomon_Martin 0:3cafef4c2519 187 } else if (val == 'a') {
Solomon_Martin 0:3cafef4c2519 188 pc.printf("enter speed correction \n");
Solomon_Martin 0:3cafef4c2519 189 val= pc.getc();
Solomon_Martin 0:3cafef4c2519 190 if(val == '+') {
Solomon_Martin 0:3cafef4c2519 191 speedCorrection += 0.1;
Solomon_Martin 0:3cafef4c2519 192 } else if (val == '-') {
Solomon_Martin 0:3cafef4c2519 193 speedCorrection -= 0.1;
Solomon_Martin 0:3cafef4c2519 194 }
Solomon_Martin 0:3cafef4c2519 195 pc.printf("speedCorrect = %f \n",speedCorrection);
Solomon_Martin 0:3cafef4c2519 196 }
Solomon_Martin 0:3cafef4c2519 197 wait(1);
Solomon_Martin 0:3cafef4c2519 198 }