main.cpp

00001 #include "mbed.h"
00002 #include "SRF05.h"   //http://mbed.org/cookbook/SRF05-Ultrasonic-Ranger
00003 #define PI 3.1415926
00004 #define RPSPUNIT -0.00126766
00005 
00006 Serial pc(USBTX, USBRX);
00007 
00008 SRF05 srf(p13, p14);
00009 
00010 DigitalOut led1(LED1);
00011 DigitalOut led2(LED2);
00012 DigitalOut led4(LED4);
00013 
00014 PwmOut LF(p21); //left forward
00015 PwmOut LB(p22); //left back
00016 PwmOut RF(p23); //right forward
00017 PwmOut RB(p24); //right back
00018 AnalogIn GyroX(p19); //Gyro X
00019 Ticker ticker;
00020 
00021 float ANGLE;
00022 float GYRO;
00023 float INTEGRAL=0.0;
00024 float startangle=0;
00025 float startgyro=0;
00026 
00027 //PID loop
00028 float control;
00029 float Kp=0.5;//0.5 works
00030 float Kd=-5.0;//-5.0 works
00031 float Ki=0.5;//0.3 works
00032 
00033 float lowpassedGYRO;
00034 
00035 void stabilize();
00036 
00037 void controlbothmotors(float drive) {
00038     LF.write(drive > 0 ? drive : 0);
00039     LB.write(drive < 0 ? -drive : 0);
00040     RF.write(drive > 0 ? drive : 0);
00041     RB.write(drive < 0 ? -drive : 0);
00042 }
00043 
00044 float toprint[3];
00045 
00046 float gyros[3];
00047 I2C i2c(p9, p10);
00048 float gyrcalib[3];
00049 
00050 void calibgyro() {
00051 
00052     //activating the wiimotion +
00053     i2c.start();
00054     //device address | write
00055     i2c.write(0x53 << 1 | 0);
00056     //register address
00057     i2c.write(0xfe);
00058     //data
00059     i2c.write(0x04);
00060     i2c.stop();
00061 
00062     for (int i = 0; i < 100; i++) {
00063         //sending a 0x00 to flag that we want data
00064         i2c.start();
00065         //device address | write (note new address)
00066         i2c.write(0x52 << 1 | 0);
00067         //send 0x00
00068         i2c.write(0x00);
00069         i2c.stop();
00070 
00071         //reading the data
00072         char wmpdata[6];
00073         i2c.read(0x52 << 1 | 1, wmpdata, 6);
00074         //pc.printf("%x %x %x %x %x %x\r\n",  wmpdata[0], wmpdata[1], wmpdata[2], wmpdata[3], wmpdata[4], wmpdata[5]);
00075 
00076         gyrcalib[0] += (((wmpdata[3] >> 2) << 8) + wmpdata[0]) / 100;
00077         gyrcalib[1] += (((wmpdata[4] >> 2) << 8) + wmpdata[1]) / 100;
00078         gyrcalib[2] += (((wmpdata[5] >> 2) << 8) + wmpdata[2]) / 100;
00079 
00080         wait(0.05);
00081 
00082     }
00083 
00084 }
00085 
00086 void getgyros(float gyrodata[3]) {
00087     //sending a 0x00 to flag that we want data
00088     i2c.start();
00089     //device address | write (note new address)
00090     i2c.write(0x52 << 1 | 0);
00091     //send 0x00
00092     i2c.write(0x00);
00093     i2c.stop();
00094 
00095     //reading the data
00096     char wmpdata[6];
00097     i2c.read(0x52 << 1 | 1, wmpdata, 6);
00098 
00099     //detect if we ever went into fast mode
00100     bool fastdiscard = !(wmpdata[3] & 0x02 && wmpdata[4] & 0x02 && wmpdata[3] & 0x01);
00101 
00102     if (fastdiscard)
00103         led1 = 1;
00104     else {
00105         gyrodata[0] = RPSPUNIT * -(((wmpdata[3] >> 2) << 8) + wmpdata[0] - gyrcalib[0]);
00106         gyrodata[1] = RPSPUNIT * (((wmpdata[4] >> 2) << 8) + wmpdata[1] - gyrcalib[1]);
00107         gyrodata[2] = RPSPUNIT * (((wmpdata[5] >> 2) << 8) + wmpdata[2] - gyrcalib[2]);
00108     }
00109 
00110     //pc.printf("yaw: %f, pitch: %f, roll: %f\r\n", gyrodata[0], gyrodata[1], gyrodata[2]);
00111 
00112     //wait(0.05);
00113 }
00114 
00115 int main() {
00116 
00117     //pc.baud(115200);
00118 
00119     Timer outputtimer;
00120 
00121     calibgyro();
00122     printf("%f\r\n", gyrcalib[1]);
00123 
00124     //getstartangle and getstartgyro
00125 
00126     for (int i=0; i<=100; i++) {
00127         startangle=(startangle*i+srf.read())/(i+1);
00128         //startgyro=(startgyro*i+GyroX.read())/(i+1);
00129         wait(0.02);
00130     }
00131     startangle = (360.0/(2.0*PI))*asin((startangle-5.0)/16.0);
00132 
00133     //startangle=40.07;
00134     //lowpassedGYRO=startgyro;
00135 
00136     outputtimer.start();
00137 
00138     //balance
00139     ticker.attach(&stabilize, 0.01);
00140     while (1) {
00141         //printf("Angle in degrees : %.2f\t Gyro : %f\n\r",toprint[0],toprint[1]);
00142         //printf("Gyro : %.2f\t%.0f\n\r",abs(toprint[1]),toprint[1]);
00143         if (outputtimer.read()>30.0) {
00144             led4.write(1.0);
00145             printf("Startangle : %.2f\n\r",startangle);
00146         }
00147         wait(0.1);
00148     }
00149 
00150 }
00151 
00152 void stabilize() {
00153     led1 = 1;
00154     led2 = 0;
00155 
00156     //Kp
00157     if (srf.read()>0.0 && srf.read()<30.0)
00158         ANGLE = (360.0/(2.0*PI))*asin((srf.read()-5.0)/16.0)-startangle;
00159     //Kd
00160     getgyros(gyros);
00161     GYRO = gyros[1];
00162     //Ki
00163     float integralcap=1.0/Ki;
00164     if ((INTEGRAL+control < integralcap) && (INTEGRAL+control > -integralcap))
00165         INTEGRAL+=control;
00166     INTEGRAL*=0.98;//0.97 works
00167 
00168     //float howlowpassed=0.0;
00169     //lowpassedGYRO=howlowpassed*GyroX.read()+(1.0-howlowpassed)*lowpassedGYRO;
00170     //GYRO = GyroX.read()-lowpassedGYRO;
00171 
00172     float smooth=0.95;//0.95 works
00173     control=(1.0-smooth)*(Kp*ANGLE + Kd*GYRO + Ki*INTEGRAL)+smooth*control;
00174     controlbothmotors(control);
00175 
00176     toprint[0]=ANGLE;
00177     toprint[1]=GYRO;
00178 
00179     led1 = 0;
00180     led2 = 1;
00181 }