ECE 4180 Spring 15
/
balanceRobo
uses PID to balance a robot
main.cpp
- Committer:
- nicovanduijn
- Date:
- 2015-03-04
- Revision:
- 0:a1e1e939ee6c
- Child:
- 1:0ef8f077473e
File content as of revision 0:a1e1e939ee6c:
#include "mbed.h" // mbed library #include "LSM9DS0.h" // 9axis IMU #include "math.h" #define LSM9DS0_XM_ADDR 0x1D // Would be 0x1E if SDO_XM is LOW #define LSM9DS0_G_ADDR 0x6B // Would be 0x6A if SDO_G is LOW PwmOut motorSpeedLeft(p21); // PWM port to control speed of left motor PwmOut motorSpeedRight(p22); // PWM port to control speed of right motor DigitalOut motorDirLeft(p23); // Digital pin for direction of left motor DigitalOut NmotorDirLeft(p24); // Usually inverse of motorDirLeft DigitalOut motorDirRight(p26); // Digital pin for direction of right motor DigitalOut NmotorDirRight(p25); // Usually inverse of motorDirRight LSM9DS0 imu(p9, p10, LSM9DS0_G_ADDR, LSM9DS0_XM_ADDR); // Creates on object for IMU Serial pc(USBTX, USBRX); // Creates virtual Serial connection though USB DigitalOut debug(p20); // Digital output Pin for debugging purposes Timer t; // Timer needed to integrate float kp; // Proportional gain float kd; // Derivative gain float ki; // Integrative gain char val; // Needed for Serial communication (need to be global?) int timer1=0; // Variable for timer (need to be global?) int timer2=0; // Variable 2 for timer (need to be global?) void drive(float); // Function declaration for driving the motors void callback(); // Interrupt triggered function for serial communication float PID(float); // Function to do PID control int main() { uint16_t status = imu.begin(); // Use the begin() function to initialize the LSM9DS0 library. pc.printf("LSM9DS0 WHO_AM_I's returned: 0x%X\n", status); // Make sure communication is working pc.printf("Should be 0x49D4\n\n"); float alpha; // Local to hold angle // float speed; // Local to hold motor speed //float omega; // Local to hold angular velocity float desiredAngle=0; // Later set this angle with a setup function pc.attach(&callback); // Attach interrupt to serial communication t.start(); // Start the timer while(1) { timer2=timer1; // Remember the previous timer timer1=t.read(); // Read the current timer imu.readGyro(); // Read the gyro imu.readAccel(); // Read the Accelerometer alpha=0.98*(alpha+imu.gx*(timer1-timer2))+0.02*(asin(imu.az)); // Complementary filter drive(PID(alpha-desiredAngle)); // PID control on the error /* alpha=asin(imu.az)*180/3.141; omega=imu.gx; speed=(kp*alpha+(kd/10.0)*omega)/100; drive(speed); pc.printf("%f\n", speed); wait(.1); data[0]=imu.ax; // x value of acceleration data[1]=imu.ay; // y value of acceleration data[2]=imu.az; // z value of acceleration */ // pc.printf("Accelerometer Data:\nx: %f\ny: %f\nz: %f\n\n",imu.ax,imu.ay,imu.az); /* data[3]=imu.gx; // x value of gyro data[4]=imu.gy; // y value of gyro data[5]=imu.gz; // z value of gyro */ //pc.printf("Gyroscope Data:\nx: %f\ny: %f\nz: %f\n\n",imu.gx,imu.gy,imu.gz); } } //////////////////////////////////////////////////////// // drive function void drive(float speed) { int direction; if (speed>0)direction=1; if (speed<0) direction=2; if(speed==0)direction=0; // pc.printf("%f %d\n", speed, direction); switch( direction) { // depending on what direction was passed case 0: // stop case motorSpeedLeft=0; motorSpeedRight=0; motorDirLeft=0; NmotorDirLeft=0; motorDirRight=0; NmotorDirRight=0; break; case 1: // forward case motorSpeedLeft=speed; motorSpeedRight=speed; motorDirLeft=1; NmotorDirLeft=0; motorDirRight=1; NmotorDirRight=0; break; case 2: // backwards motorSpeedLeft=-speed; motorSpeedRight=-speed; motorDirLeft=0; NmotorDirLeft=1; motorDirRight=0; NmotorDirRight=1; break; default: // catch-all motorSpeedLeft=0; motorSpeedRight=0; motorDirLeft=0; NmotorDirLeft=0; motorDirRight=0; NmotorDirRight=0; break; } } void callback() { val = pc.getc(); // Reat the value from Serial pc.printf("%c\n", val); // Print it back to the screen if( val =='p') // If character was a 'p' { pc.printf("enter kp \n"); // Adjust kp val = pc.getc(); // Wait for kp value if(val == '+') { kp++; }else if (val == '-') { kp--; }else { kp = val - 48; // Cast char to float } pc.printf(" kp = %f \n",kp); } else if( val == 'd') // Adjust kd { pc.printf("enter kd \n"); // Wait for kd val= pc.getc(); if(val == '+') { kd++; }else if (val == '-') { kd--; }else { kd = val - 48; } pc.printf(" kd = %f \n",kd); } else if( val == 'i') { pc.printf("enter ki \n"); val= pc.getc(); if(val == '+') { ki++; }else if (val == '-') { ki--; }else { ki = val - 48; } pc.printf(" ki = %f \n",kd); } } float PID(float error){ static float previousError; float integratedError; float derivativeError; float speed; integratedError+=error; derivativeError=error-previousError; speed=ki*integratedError+kp*error+kd*derivativeError; previousError=error; return speed; }