ECE 4180 Spring 15 / Mbed 2 deprecated balanceRobo

uses PID to balance a robot

Dependencies:   LSM9DS0 mbed

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

Committer:
nicovanduijn
Date:
Wed Mar 04 17:54:08 2015 +0000
Revision:
0:a1e1e939ee6c
Child:
1:0ef8f077473e
PID control untested so far

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 0:a1e1e939ee6c 22 int timer1=0; // Variable for timer (need to be global?)
nicovanduijn 0:a1e1e939ee6c 23 int timer2=0; // Variable 2 for timer (need to be global?)
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 0:a1e1e939ee6c 35 float alpha; // 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 0:a1e1e939ee6c 41
nicovanduijn 0:a1e1e939ee6c 42
nicovanduijn 0:a1e1e939ee6c 43 while(1) {
nicovanduijn 0:a1e1e939ee6c 44 timer2=timer1; // Remember the previous timer
nicovanduijn 0:a1e1e939ee6c 45 timer1=t.read(); // Read the current timer
nicovanduijn 0:a1e1e939ee6c 46 imu.readGyro(); // Read the gyro
nicovanduijn 0:a1e1e939ee6c 47 imu.readAccel(); // Read the Accelerometer
nicovanduijn 0:a1e1e939ee6c 48 alpha=0.98*(alpha+imu.gx*(timer1-timer2))+0.02*(asin(imu.az)); // Complementary filter
nicovanduijn 0:a1e1e939ee6c 49 drive(PID(alpha-desiredAngle)); // PID control on the error
nicovanduijn 0:a1e1e939ee6c 50
nicovanduijn 0:a1e1e939ee6c 51
nicovanduijn 0:a1e1e939ee6c 52 /*
nicovanduijn 0:a1e1e939ee6c 53 alpha=asin(imu.az)*180/3.141;
nicovanduijn 0:a1e1e939ee6c 54 omega=imu.gx;
nicovanduijn 0:a1e1e939ee6c 55 speed=(kp*alpha+(kd/10.0)*omega)/100;
nicovanduijn 0:a1e1e939ee6c 56 drive(speed);
nicovanduijn 0:a1e1e939ee6c 57 pc.printf("%f\n", speed);
nicovanduijn 0:a1e1e939ee6c 58 wait(.1);
nicovanduijn 0:a1e1e939ee6c 59
nicovanduijn 0:a1e1e939ee6c 60
nicovanduijn 0:a1e1e939ee6c 61 data[0]=imu.ax; // x value of acceleration
nicovanduijn 0:a1e1e939ee6c 62 data[1]=imu.ay; // y value of acceleration
nicovanduijn 0:a1e1e939ee6c 63 data[2]=imu.az; // z value of acceleration
nicovanduijn 0:a1e1e939ee6c 64 */
nicovanduijn 0:a1e1e939ee6c 65 // pc.printf("Accelerometer Data:\nx: %f\ny: %f\nz: %f\n\n",imu.ax,imu.ay,imu.az);
nicovanduijn 0:a1e1e939ee6c 66 /*
nicovanduijn 0:a1e1e939ee6c 67 data[3]=imu.gx; // x value of gyro
nicovanduijn 0:a1e1e939ee6c 68 data[4]=imu.gy; // y value of gyro
nicovanduijn 0:a1e1e939ee6c 69 data[5]=imu.gz; // z value of gyro
nicovanduijn 0:a1e1e939ee6c 70 */
nicovanduijn 0:a1e1e939ee6c 71 //pc.printf("Gyroscope Data:\nx: %f\ny: %f\nz: %f\n\n",imu.gx,imu.gy,imu.gz);
nicovanduijn 0:a1e1e939ee6c 72
nicovanduijn 0:a1e1e939ee6c 73
nicovanduijn 0:a1e1e939ee6c 74 }
nicovanduijn 0:a1e1e939ee6c 75 }
nicovanduijn 0:a1e1e939ee6c 76 ////////////////////////////////////////////////////////
nicovanduijn 0:a1e1e939ee6c 77 // drive function
nicovanduijn 0:a1e1e939ee6c 78 void drive(float speed)
nicovanduijn 0:a1e1e939ee6c 79 {
nicovanduijn 0:a1e1e939ee6c 80 int direction;
nicovanduijn 0:a1e1e939ee6c 81 if (speed>0)direction=1;
nicovanduijn 0:a1e1e939ee6c 82 if (speed<0) direction=2;
nicovanduijn 0:a1e1e939ee6c 83 if(speed==0)direction=0;
nicovanduijn 0:a1e1e939ee6c 84 // pc.printf("%f %d\n", speed, direction);
nicovanduijn 0:a1e1e939ee6c 85 switch( direction) { // depending on what direction was passed
nicovanduijn 0:a1e1e939ee6c 86 case 0: // stop case
nicovanduijn 0:a1e1e939ee6c 87 motorSpeedLeft=0;
nicovanduijn 0:a1e1e939ee6c 88 motorSpeedRight=0;
nicovanduijn 0:a1e1e939ee6c 89 motorDirLeft=0;
nicovanduijn 0:a1e1e939ee6c 90 NmotorDirLeft=0;
nicovanduijn 0:a1e1e939ee6c 91 motorDirRight=0;
nicovanduijn 0:a1e1e939ee6c 92 NmotorDirRight=0;
nicovanduijn 0:a1e1e939ee6c 93 break;
nicovanduijn 0:a1e1e939ee6c 94 case 1: // forward case
nicovanduijn 0:a1e1e939ee6c 95 motorSpeedLeft=speed;
nicovanduijn 0:a1e1e939ee6c 96 motorSpeedRight=speed;
nicovanduijn 0:a1e1e939ee6c 97 motorDirLeft=1;
nicovanduijn 0:a1e1e939ee6c 98 NmotorDirLeft=0;
nicovanduijn 0:a1e1e939ee6c 99 motorDirRight=1;
nicovanduijn 0:a1e1e939ee6c 100 NmotorDirRight=0;
nicovanduijn 0:a1e1e939ee6c 101 break;
nicovanduijn 0:a1e1e939ee6c 102 case 2: // backwards
nicovanduijn 0:a1e1e939ee6c 103 motorSpeedLeft=-speed;
nicovanduijn 0:a1e1e939ee6c 104 motorSpeedRight=-speed;
nicovanduijn 0:a1e1e939ee6c 105 motorDirLeft=0;
nicovanduijn 0:a1e1e939ee6c 106 NmotorDirLeft=1;
nicovanduijn 0:a1e1e939ee6c 107 motorDirRight=0;
nicovanduijn 0:a1e1e939ee6c 108 NmotorDirRight=1;
nicovanduijn 0:a1e1e939ee6c 109 break;
nicovanduijn 0:a1e1e939ee6c 110 default: // catch-all
nicovanduijn 0:a1e1e939ee6c 111 motorSpeedLeft=0;
nicovanduijn 0:a1e1e939ee6c 112 motorSpeedRight=0;
nicovanduijn 0:a1e1e939ee6c 113 motorDirLeft=0;
nicovanduijn 0:a1e1e939ee6c 114 NmotorDirLeft=0;
nicovanduijn 0:a1e1e939ee6c 115 motorDirRight=0;
nicovanduijn 0:a1e1e939ee6c 116 NmotorDirRight=0;
nicovanduijn 0:a1e1e939ee6c 117 break;
nicovanduijn 0:a1e1e939ee6c 118 }
nicovanduijn 0:a1e1e939ee6c 119 }
nicovanduijn 0:a1e1e939ee6c 120
nicovanduijn 0:a1e1e939ee6c 121
nicovanduijn 0:a1e1e939ee6c 122 void callback() {
nicovanduijn 0:a1e1e939ee6c 123 val = pc.getc(); // Reat the value from Serial
nicovanduijn 0:a1e1e939ee6c 124 pc.printf("%c\n", val); // Print it back to the screen
nicovanduijn 0:a1e1e939ee6c 125 if( val =='p') // If character was a 'p'
nicovanduijn 0:a1e1e939ee6c 126 {
nicovanduijn 0:a1e1e939ee6c 127 pc.printf("enter kp \n"); // Adjust kp
nicovanduijn 0:a1e1e939ee6c 128 val = pc.getc(); // Wait for kp value
nicovanduijn 0:a1e1e939ee6c 129 if(val == '+')
nicovanduijn 0:a1e1e939ee6c 130 {
nicovanduijn 0:a1e1e939ee6c 131 kp++;
nicovanduijn 0:a1e1e939ee6c 132 }else if (val == '-')
nicovanduijn 0:a1e1e939ee6c 133 {
nicovanduijn 0:a1e1e939ee6c 134 kp--;
nicovanduijn 0:a1e1e939ee6c 135 }else
nicovanduijn 0:a1e1e939ee6c 136 {
nicovanduijn 0:a1e1e939ee6c 137 kp = val - 48; // Cast char to float
nicovanduijn 0:a1e1e939ee6c 138 }
nicovanduijn 0:a1e1e939ee6c 139 pc.printf(" kp = %f \n",kp);
nicovanduijn 0:a1e1e939ee6c 140 }
nicovanduijn 0:a1e1e939ee6c 141 else if( val == 'd') // Adjust kd
nicovanduijn 0:a1e1e939ee6c 142 {
nicovanduijn 0:a1e1e939ee6c 143 pc.printf("enter kd \n"); // Wait for kd
nicovanduijn 0:a1e1e939ee6c 144 val= pc.getc();
nicovanduijn 0:a1e1e939ee6c 145 if(val == '+')
nicovanduijn 0:a1e1e939ee6c 146 {
nicovanduijn 0:a1e1e939ee6c 147 kd++;
nicovanduijn 0:a1e1e939ee6c 148 }else if (val == '-')
nicovanduijn 0:a1e1e939ee6c 149 {
nicovanduijn 0:a1e1e939ee6c 150 kd--;
nicovanduijn 0:a1e1e939ee6c 151 }else
nicovanduijn 0:a1e1e939ee6c 152 {
nicovanduijn 0:a1e1e939ee6c 153 kd = val - 48;
nicovanduijn 0:a1e1e939ee6c 154 }
nicovanduijn 0:a1e1e939ee6c 155 pc.printf(" kd = %f \n",kd);
nicovanduijn 0:a1e1e939ee6c 156 }
nicovanduijn 0:a1e1e939ee6c 157 else if( val == 'i')
nicovanduijn 0:a1e1e939ee6c 158 {
nicovanduijn 0:a1e1e939ee6c 159 pc.printf("enter ki \n");
nicovanduijn 0:a1e1e939ee6c 160 val= pc.getc();
nicovanduijn 0:a1e1e939ee6c 161 if(val == '+')
nicovanduijn 0:a1e1e939ee6c 162 {
nicovanduijn 0:a1e1e939ee6c 163 ki++;
nicovanduijn 0:a1e1e939ee6c 164 }else if (val == '-')
nicovanduijn 0:a1e1e939ee6c 165 {
nicovanduijn 0:a1e1e939ee6c 166 ki--;
nicovanduijn 0:a1e1e939ee6c 167 }else
nicovanduijn 0:a1e1e939ee6c 168 {
nicovanduijn 0:a1e1e939ee6c 169 ki = val - 48;
nicovanduijn 0:a1e1e939ee6c 170 }
nicovanduijn 0:a1e1e939ee6c 171 pc.printf(" ki = %f \n",kd);
nicovanduijn 0:a1e1e939ee6c 172 }
nicovanduijn 0:a1e1e939ee6c 173
nicovanduijn 0:a1e1e939ee6c 174 }
nicovanduijn 0:a1e1e939ee6c 175
nicovanduijn 0:a1e1e939ee6c 176 float PID(float error){
nicovanduijn 0:a1e1e939ee6c 177 static float previousError;
nicovanduijn 0:a1e1e939ee6c 178 float integratedError;
nicovanduijn 0:a1e1e939ee6c 179 float derivativeError;
nicovanduijn 0:a1e1e939ee6c 180 float speed;
nicovanduijn 0:a1e1e939ee6c 181 integratedError+=error;
nicovanduijn 0:a1e1e939ee6c 182 derivativeError=error-previousError;
nicovanduijn 0:a1e1e939ee6c 183 speed=ki*integratedError+kp*error+kd*derivativeError;
nicovanduijn 0:a1e1e939ee6c 184 previousError=error;
nicovanduijn 0:a1e1e939ee6c 185 return speed;
nicovanduijn 0:a1e1e939ee6c 186 }