ECE 4180 Spring 15 / Mbed 2 deprecated balanceRobo

uses PID to balance a robot

Dependencies:   LSM9DS0 mbed

main.cpp@1:0ef8f077473e, 2015-03-04 (annotated)

Committer:
nicovanduijn
Date:
Wed Mar 04 18:10:47 2015 +0000
Revision:
1:0ef8f077473e
Parent:
0:a1e1e939ee6c 
Improved comments.; Yet to do: calibrate() function to initialize, more comments, testing

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nicovanduijn 0:a1e1e939ee6c 1 #include "mbed.h" // mbed library
nicovanduijn 0:a1e1e939ee6c 2 #include "LSM9DS0.h" // 9axis IMU
nicovanduijn 0:a1e1e939ee6c 3 #include "math.h"
nicovanduijn 0:a1e1e939ee6c 4
nicovanduijn 0:a1e1e939ee6c 5 #define LSM9DS0_XM_ADDR 0x1D // Would be 0x1E if SDO_XM is LOW
nicovanduijn 0:a1e1e939ee6c 6 #define LSM9DS0_G_ADDR 0x6B // Would be 0x6A if SDO_G is LOW
nicovanduijn 0:a1e1e939ee6c 7
nicovanduijn 0:a1e1e939ee6c 8 PwmOut motorSpeedLeft(p21); // PWM port to control speed of left motor
nicovanduijn 0:a1e1e939ee6c 9 PwmOut motorSpeedRight(p22); // PWM port to control speed of right motor
nicovanduijn 0:a1e1e939ee6c 10 DigitalOut motorDirLeft(p23); // Digital pin for direction of left motor
nicovanduijn 0:a1e1e939ee6c 11 DigitalOut NmotorDirLeft(p24); // Usually inverse of motorDirLeft
nicovanduijn 0:a1e1e939ee6c 12 DigitalOut motorDirRight(p26); // Digital pin for direction of right motor
nicovanduijn 0:a1e1e939ee6c 13 DigitalOut NmotorDirRight(p25); // Usually inverse of motorDirRight
nicovanduijn 0:a1e1e939ee6c 14 LSM9DS0 imu(p9, p10, LSM9DS0_G_ADDR, LSM9DS0_XM_ADDR); // Creates on object for IMU
nicovanduijn 0:a1e1e939ee6c 15 Serial pc(USBTX, USBRX); // Creates virtual Serial connection though USB
nicovanduijn 0:a1e1e939ee6c 16 DigitalOut debug(p20); // Digital output Pin for debugging purposes
nicovanduijn 0:a1e1e939ee6c 17 Timer t; // Timer needed to integrate
nicovanduijn 0:a1e1e939ee6c 18 float kp; // Proportional gain
nicovanduijn 0:a1e1e939ee6c 19 float kd; // Derivative gain
nicovanduijn 0:a1e1e939ee6c 20 float ki; // Integrative gain
nicovanduijn 0:a1e1e939ee6c 21 char val; // Needed for Serial communication (need to be global?)
nicovanduijn 1:0ef8f077473e 22 int timer1=0; // Variable for timer
nicovanduijn 1:0ef8f077473e 23 int timer2=0; // Variable 2 for timer
nicovanduijn 0:a1e1e939ee6c 24 void drive(float); // Function declaration for driving the motors
nicovanduijn 0:a1e1e939ee6c 25 void callback(); // Interrupt triggered function for serial communication
nicovanduijn 0:a1e1e939ee6c 26 float PID(float); // Function to do PID control
nicovanduijn 0:a1e1e939ee6c 27
nicovanduijn 0:a1e1e939ee6c 28
nicovanduijn 0:a1e1e939ee6c 29 int main()
nicovanduijn 0:a1e1e939ee6c 30 {
nicovanduijn 0:a1e1e939ee6c 31
nicovanduijn 0:a1e1e939ee6c 32 uint16_t status = imu.begin(); // Use the begin() function to initialize the LSM9DS0 library.
nicovanduijn 0:a1e1e939ee6c 33 pc.printf("LSM9DS0 WHO_AM_I's returned: 0x%X\n", status); // Make sure communication is working
nicovanduijn 0:a1e1e939ee6c 34 pc.printf("Should be 0x49D4\n\n");
nicovanduijn 1:0ef8f077473e 35 float alpha=0; // Local to hold angle
nicovanduijn 0:a1e1e939ee6c 36 // float speed; // Local to hold motor speed
nicovanduijn 0:a1e1e939ee6c 37 //float omega; // Local to hold angular velocity
nicovanduijn 0:a1e1e939ee6c 38 float desiredAngle=0; // Later set this angle with a setup function
nicovanduijn 0:a1e1e939ee6c 39 pc.attach(&callback); // Attach interrupt to serial communication
nicovanduijn 0:a1e1e939ee6c 40 t.start(); // Start the timer
nicovanduijn 1:0ef8f077473e 41
nicovanduijn 0:a1e1e939ee6c 42 while(1) {
nicovanduijn 0:a1e1e939ee6c 43 timer2=timer1; // Remember the previous timer
nicovanduijn 0:a1e1e939ee6c 44 timer1=t.read(); // Read the current timer
nicovanduijn 0:a1e1e939ee6c 45 imu.readGyro(); // Read the gyro
nicovanduijn 0:a1e1e939ee6c 46 imu.readAccel(); // Read the Accelerometer
nicovanduijn 0:a1e1e939ee6c 47 alpha=0.98*(alpha+imu.gx*(timer1-timer2))+0.02*(asin(imu.az)); // Complementary filter
nicovanduijn 0:a1e1e939ee6c 48 drive(PID(alpha-desiredAngle)); // PID control on the error
nicovanduijn 0:a1e1e939ee6c 49
nicovanduijn 0:a1e1e939ee6c 50
nicovanduijn 0:a1e1e939ee6c 51 /*
nicovanduijn 0:a1e1e939ee6c 52 alpha=asin(imu.az)*180/3.141;
nicovanduijn 0:a1e1e939ee6c 53 omega=imu.gx;
nicovanduijn 0:a1e1e939ee6c 54 speed=(kp*alpha+(kd/10.0)*omega)/100;
nicovanduijn 0:a1e1e939ee6c 55 drive(speed);
nicovanduijn 0:a1e1e939ee6c 56 pc.printf("%f\n", speed);
nicovanduijn 0:a1e1e939ee6c 57 wait(.1);
nicovanduijn 0:a1e1e939ee6c 58
nicovanduijn 0:a1e1e939ee6c 59
nicovanduijn 0:a1e1e939ee6c 60 data[0]=imu.ax; // x value of acceleration
nicovanduijn 0:a1e1e939ee6c 61 data[1]=imu.ay; // y value of acceleration
nicovanduijn 0:a1e1e939ee6c 62 data[2]=imu.az; // z value of acceleration
nicovanduijn 0:a1e1e939ee6c 63 */
nicovanduijn 0:a1e1e939ee6c 64 // pc.printf("Accelerometer Data:\nx: %f\ny: %f\nz: %f\n\n",imu.ax,imu.ay,imu.az);
nicovanduijn 0:a1e1e939ee6c 65 /*
nicovanduijn 0:a1e1e939ee6c 66 data[3]=imu.gx; // x value of gyro
nicovanduijn 0:a1e1e939ee6c 67 data[4]=imu.gy; // y value of gyro
nicovanduijn 0:a1e1e939ee6c 68 data[5]=imu.gz; // z value of gyro
nicovanduijn 0:a1e1e939ee6c 69 */
nicovanduijn 0:a1e1e939ee6c 70 //pc.printf("Gyroscope Data:\nx: %f\ny: %f\nz: %f\n\n",imu.gx,imu.gy,imu.gz);
nicovanduijn 0:a1e1e939ee6c 71
nicovanduijn 0:a1e1e939ee6c 72
nicovanduijn 0:a1e1e939ee6c 73 }
nicovanduijn 0:a1e1e939ee6c 74 }
nicovanduijn 0:a1e1e939ee6c 75 ////////////////////////////////////////////////////////
nicovanduijn 0:a1e1e939ee6c 76 // drive function
nicovanduijn 0:a1e1e939ee6c 77 void drive(float speed)
nicovanduijn 0:a1e1e939ee6c 78 {
nicovanduijn 1:0ef8f077473e 79 int direction=0;
nicovanduijn 0:a1e1e939ee6c 80 if (speed>0)direction=1;
nicovanduijn 1:0ef8f077473e 81 if (speed<0)direction=2;
nicovanduijn 0:a1e1e939ee6c 82 if(speed==0)direction=0;
nicovanduijn 0:a1e1e939ee6c 83 // pc.printf("%f %d\n", speed, direction);
nicovanduijn 0:a1e1e939ee6c 84 switch( direction) { // depending on what direction was passed
nicovanduijn 0:a1e1e939ee6c 85 case 0: // stop case
nicovanduijn 0:a1e1e939ee6c 86 motorSpeedLeft=0;
nicovanduijn 0:a1e1e939ee6c 87 motorSpeedRight=0;
nicovanduijn 0:a1e1e939ee6c 88 motorDirLeft=0;
nicovanduijn 0:a1e1e939ee6c 89 NmotorDirLeft=0;
nicovanduijn 0:a1e1e939ee6c 90 motorDirRight=0;
nicovanduijn 0:a1e1e939ee6c 91 NmotorDirRight=0;
nicovanduijn 0:a1e1e939ee6c 92 break;
nicovanduijn 0:a1e1e939ee6c 93 case 1: // forward case
nicovanduijn 0:a1e1e939ee6c 94 motorSpeedLeft=speed;
nicovanduijn 0:a1e1e939ee6c 95 motorSpeedRight=speed;
nicovanduijn 0:a1e1e939ee6c 96 motorDirLeft=1;
nicovanduijn 0:a1e1e939ee6c 97 NmotorDirLeft=0;
nicovanduijn 0:a1e1e939ee6c 98 motorDirRight=1;
nicovanduijn 0:a1e1e939ee6c 99 NmotorDirRight=0;
nicovanduijn 0:a1e1e939ee6c 100 break;
nicovanduijn 0:a1e1e939ee6c 101 case 2: // backwards
nicovanduijn 0:a1e1e939ee6c 102 motorSpeedLeft=-speed;
nicovanduijn 0:a1e1e939ee6c 103 motorSpeedRight=-speed;
nicovanduijn 0:a1e1e939ee6c 104 motorDirLeft=0;
nicovanduijn 0:a1e1e939ee6c 105 NmotorDirLeft=1;
nicovanduijn 0:a1e1e939ee6c 106 motorDirRight=0;
nicovanduijn 0:a1e1e939ee6c 107 NmotorDirRight=1;
nicovanduijn 0:a1e1e939ee6c 108 break;
nicovanduijn 0:a1e1e939ee6c 109 default: // catch-all
nicovanduijn 0:a1e1e939ee6c 110 motorSpeedLeft=0;
nicovanduijn 0:a1e1e939ee6c 111 motorSpeedRight=0;
nicovanduijn 0:a1e1e939ee6c 112 motorDirLeft=0;
nicovanduijn 0:a1e1e939ee6c 113 NmotorDirLeft=0;
nicovanduijn 0:a1e1e939ee6c 114 motorDirRight=0;
nicovanduijn 0:a1e1e939ee6c 115 NmotorDirRight=0;
nicovanduijn 0:a1e1e939ee6c 116 break;
nicovanduijn 0:a1e1e939ee6c 117 }
nicovanduijn 0:a1e1e939ee6c 118 }
nicovanduijn 0:a1e1e939ee6c 119
nicovanduijn 0:a1e1e939ee6c 120
nicovanduijn 0:a1e1e939ee6c 121 void callback() {
nicovanduijn 0:a1e1e939ee6c 122 val = pc.getc(); // Reat the value from Serial
nicovanduijn 0:a1e1e939ee6c 123 pc.printf("%c\n", val); // Print it back to the screen
nicovanduijn 0:a1e1e939ee6c 124 if( val =='p') // If character was a 'p'
nicovanduijn 0:a1e1e939ee6c 125 {
nicovanduijn 0:a1e1e939ee6c 126 pc.printf("enter kp \n"); // Adjust kp
nicovanduijn 0:a1e1e939ee6c 127 val = pc.getc(); // Wait for kp value
nicovanduijn 0:a1e1e939ee6c 128 if(val == '+')
nicovanduijn 0:a1e1e939ee6c 129 {
nicovanduijn 0:a1e1e939ee6c 130 kp++;
nicovanduijn 0:a1e1e939ee6c 131 }else if (val == '-')
nicovanduijn 0:a1e1e939ee6c 132 {
nicovanduijn 0:a1e1e939ee6c 133 kp--;
nicovanduijn 0:a1e1e939ee6c 134 }else
nicovanduijn 0:a1e1e939ee6c 135 {
nicovanduijn 0:a1e1e939ee6c 136 kp = val - 48; // Cast char to float
nicovanduijn 0:a1e1e939ee6c 137 }
nicovanduijn 0:a1e1e939ee6c 138 pc.printf(" kp = %f \n",kp);
nicovanduijn 0:a1e1e939ee6c 139 }
nicovanduijn 0:a1e1e939ee6c 140 else if( val == 'd') // Adjust kd
nicovanduijn 0:a1e1e939ee6c 141 {
nicovanduijn 0:a1e1e939ee6c 142 pc.printf("enter kd \n"); // Wait for kd
nicovanduijn 0:a1e1e939ee6c 143 val= pc.getc();
nicovanduijn 0:a1e1e939ee6c 144 if(val == '+')
nicovanduijn 0:a1e1e939ee6c 145 {
nicovanduijn 0:a1e1e939ee6c 146 kd++;
nicovanduijn 0:a1e1e939ee6c 147 }else if (val == '-')
nicovanduijn 0:a1e1e939ee6c 148 {
nicovanduijn 0:a1e1e939ee6c 149 kd--;
nicovanduijn 0:a1e1e939ee6c 150 }else
nicovanduijn 0:a1e1e939ee6c 151 {
nicovanduijn 0:a1e1e939ee6c 152 kd = val - 48;
nicovanduijn 0:a1e1e939ee6c 153 }
nicovanduijn 0:a1e1e939ee6c 154 pc.printf(" kd = %f \n",kd);
nicovanduijn 0:a1e1e939ee6c 155 }
nicovanduijn 0:a1e1e939ee6c 156 else if( val == 'i')
nicovanduijn 0:a1e1e939ee6c 157 {
nicovanduijn 0:a1e1e939ee6c 158 pc.printf("enter ki \n");
nicovanduijn 0:a1e1e939ee6c 159 val= pc.getc();
nicovanduijn 0:a1e1e939ee6c 160 if(val == '+')
nicovanduijn 0:a1e1e939ee6c 161 {
nicovanduijn 0:a1e1e939ee6c 162 ki++;
nicovanduijn 0:a1e1e939ee6c 163 }else if (val == '-')
nicovanduijn 0:a1e1e939ee6c 164 {
nicovanduijn 0:a1e1e939ee6c 165 ki--;
nicovanduijn 0:a1e1e939ee6c 166 }else
nicovanduijn 0:a1e1e939ee6c 167 {
nicovanduijn 0:a1e1e939ee6c 168 ki = val - 48;
nicovanduijn 0:a1e1e939ee6c 169 }
nicovanduijn 0:a1e1e939ee6c 170 pc.printf(" ki = %f \n",kd);
nicovanduijn 0:a1e1e939ee6c 171 }
nicovanduijn 0:a1e1e939ee6c 172
nicovanduijn 0:a1e1e939ee6c 173 }
nicovanduijn 0:a1e1e939ee6c 174
nicovanduijn 1:0ef8f077473e 175 float PID(float error){ // Function to calculate speed given the error
nicovanduijn 1:0ef8f077473e 176 static float previousError=0; // Static to keep track of previously passed error
nicovanduijn 1:0ef8f077473e 177 float integratedError=0; // Local to hold the integrated error
nicovanduijn 1:0ef8f077473e 178 float derivativeError=0; // Local for the derivative
nicovanduijn 1:0ef8f077473e 179 float speed=0; // Local for the speed
nicovanduijn 1:0ef8f077473e 180 integratedError+=error*(timer1-timer2); // Integrate aka. summing up the error*dt
nicovanduijn 1:0ef8f077473e 181 derivativeError=(error-previousError)/(timer1-timer2); // Differentiate aka dy/dt
nicovanduijn 1:0ef8f077473e 182 speed=ki*integratedError+kp*error+kd*derivativeError; // PID control
nicovanduijn 1:0ef8f077473e 183 previousError=error; // Remember error for next time
nicovanduijn 1:0ef8f077473e 184 return speed; // Return speed
nicovanduijn 0:a1e1e939ee6c 185 }