main.cpp

00001 #include "mbed.h"
00002 #include "Motor.h"
00003 #include "MPU9250.h"
00004 #include "stepperMotor.h"
00005 
00006 #include "PID.h"
00007 
00008 #define INTERVAL 0.1
00009 #define Lx 0.0
00010 #define Ku 26.2//0.922000
00011 #define Pu 20.8
00012 #define Kp Ku*0.182
00013 #define Ti Pu*0.55
00014 #define Td Pu*0.35
00015 #define P Kp
00016 #define I Kp/Ti
00017 #define D Kp*Td
00018 #include<esp8266.cpp>
00019 
00020 //DigitalOut myled(LED1);
00021 Serial esp(dp16,dp15);
00022 
00023 Ticker tick;
00024 SPI spi(dp2,dp1,dp6);//(A6, A5, A4);//(p5,p6,p7)
00025 mpu9250_spi imu(spi,dp26);//,A3);//,p8)   //define the mpu9250 object
00026 Timer pi;
00027 PID pid1(P,I,D,&pi);
00028 PID pid2(P,I,D,&pi);
00029 DigitalOut led1(dp28);
00030 void init();
00031 
00032 esp8266 ESP;
00033 Stepper wheel1(dp13,dp14);//(D11,D12);//dp10,11
00034 
00035 Stepper wheel2(dp10,dp9);//(D11,D12);//dp10,11
00036 #define k1 3.8
00037 #define k2 1.6
00038 #define k3 0.1
00039 #define k4 0.1
00040 
00041 float st1()
00042 {
00043     float s1 = wheel1.pulse * 7.5;
00044     
00045     static float prev_s2 = 0;
00046     float s2 = (s1 - prev_s2)/INTERVAL;
00047     static float prev = 0;
00048     float s5 = (s2 - prev)/INTERVAL;
00049     static float prev_s3 =  0;
00050     float s3 =  imu.kalman_angle[2];
00051     float s4 = (s3 - prev_s3)/INTERVAL;
00052     prev_s2=s1;
00053     prev_s3=s3;
00054     prev = s2;
00055     //printf("%f,%f,%f,%f,%f",k1*s5,k2*s2,k3*s3,k4*s4,k1*s1+k2*s2+k3*s3+k4*s4);
00056     printf(end);
00057     return k1*s3+k2*s4+k3*s2+k4*s1;
00058     
00059 }
00060 #define k12 k1
00061 #define k22 k2
00062 #define k32 k3
00063 #define k42 k4
00064 float st2()
00065 {
00066     float s1 = wheel2.pulse * 7.5;
00067     
00068     static float prev_s2 = 0;
00069     float s2 = (s1 - prev_s2)/INTERVAL;
00070     static float prev = 0;
00071     float s5 = (s2 - prev)/INTERVAL;
00072     static float prev_s3 =  0;
00073     float s3 =  imu.kalman_angle[2];
00074     float s4 = (s3 - prev_s3)/INTERVAL;
00075     prev_s2=s1;
00076     prev = s2;
00077     prev_s3=s3;
00078     //printf("%f,%f,%f,%f,%f",k1*s5,k2*s2,k3*s3,k4*s4,k1*s1+k2*s2+k3*s3+k4*s4);
00079     printf(end);
00080     return k1*s3+k2*s4+k3*s2+k4*s1;
00081     
00082 }
00083 int main() {
00084     //while(1);
00085     //printf("st");
00086     /*DigitalOut a(dp9);
00087     DigitalOut b(dp10);
00088     while(1)
00089     {
00090         a=1;
00091         b=0;
00092         wait(0.5);
00093         a=1;
00094         b=1;
00095         wait(0.5);
00096         a=0;
00097         b=1;
00098         wait(0.5);
00099         a=0;
00100         b=0;
00101         wait(0.5);
00102         printf("rotate");
00103         printf(end);
00104     }*/
00105     
00106     init();
00107     
00108     led1 = 1;
00109     wheel1.start();
00110     wheel2.start();
00111     while(1) {
00112         double data[3];
00113         imu.read_all();
00114         imu.set_angle();
00115         imu.get_angle_acc();
00116         imu.get_angle(&data[0],&data[1],&data[2]);
00117         //printf("%10.3lf, %10.3lf, %10.3f\n",
00118         //          imu.kalman_angle[2],imu.angle[2],imu.angle_acc[2]);
00119         if(abs(imu.kalman_angle[2]) < 50)
00120         {
00121             
00122             pid1.dPoint = imu.kalman_angle[2];
00123             pid2.dPoint = imu.kalman_angle[2];
00124             pid1.PIDctrl();
00125             pid2.PIDctrl();
00126             float d1 = pid1.data , d2 = pid2.data;
00127             printf("%10.3f , %10.3f \n" , d1,d2);
00128             wheel1 = -st1() / 255; 
00129             wheel2 = -st2() / 255;        
00130         }
00131         else
00132         {
00133             pid1.pid_reset();
00134             pid2.pid_reset();
00135             wheel1 = 0;
00136             wheel2 = 0;
00137             wheel1.pulse = 0;
00138             wheel2.pulse = 0;
00139         }/*
00140         if(imu.kalman_angle[1]<0)
00141         {
00142             wheel1 = 1;
00143             wait(0.5);
00144             wheel1 = -1;
00145                         wait(0.1);
00146         }
00147         else if(imu.kalman_angle[1]>0)
00148         {
00149             wheel1 = -1;
00150             wait(0.5);
00151             wheel1 = 1;
00152             wait(0.1);            
00153         }*/
00154         wait(INTERVAL);
00155     }
00156 }
00157 
00158 void init()
00159 {
00160     //pc.baud(230400);
00161     
00162     esp.baud(115200);
00163     ESP.setSerial(&esp);
00164     led1=0;
00165     wait(1);
00166     for(int i= 0 ; i<6;i++){led1=1;wait(0.05);led1=0;wait(0.05);}
00167     wait(1);
00168     /*while(1)
00169     {
00170         int i = esp.getc();
00171         if(i == 'A')
00172         {
00173             led1 = 1;
00174             wait(0.5);
00175             esp.putc(i);
00176         }
00177         if(i == 'B')
00178         {
00179             led1 = 0;
00180             wait(0.5);
00181             esp.putc(i);
00182         }
00183     }*/
00184     //printf("sart!!");
00185     /*while(ESP.ATcommand(""))
00186     {
00187         wait(2);
00188         printf("esp8266 is no connection\n");
00189     }*/
00190     //ESP.wifiConnectionMode(2);
00191     //ESP.wifiSoftAPSetting("ESP8266","ta",5,3);
00192     
00193     //ESP.WifiFindAccesspoint();
00194     //myled = 1;
00195     /*
00196     do
00197     {
00198         wait(1);
00199         //printf("Finding ESP8266");
00200     }while(ESP.WifiFindAccesspoint("ESP8366"));
00201     while(ESP.wifiConnect("ESP8366","ryuutaka"))
00202     {
00203         wait(1);
00204 //        printf("connecting to ESP8266\n");
00205     }*/
00206     //printf("esp8266 is acyivity\n");
00207     double g[3];
00208     //Thread thread2(GET);
00209     if(imu.init(1,BITS_DLPF_CFG_188HZ)) { //INIT the mpu9250
00210         //printf("\nCouldn't initialize MPU9250 via SPI!");
00211     }
00212     char x = imu.whoami();
00213     do {
00214         x = imu.whoami();
00215         printf("WHOAMI=0x%2x\n",x); //output the I2C address to know if SPI is working, it should be 104
00216         wait(0.3);
00217     } while(x!=0x71);
00218     //printf("Gyro_scale=%u\n",);    //Set full scale range for gyros
00219     imu.set_gyro_scale(BITS_FS_2000DPS);
00220     wait(0.3);
00221     //printf("Acc_scale=%u\n",imu.set_acc_scale(BITS_FS_16G));          //Set full scale range for accs
00222     imu.set_acc_scale(BITS_FS_16G);
00223     wait(0.3);
00224     do {
00225         x=imu.AK8963_whoami();
00226         printf("AK8963 WHIAM=0x%2x\n",x);
00227         wait(0.3);
00228     } while(x!=0x48);
00229     imu.MPU_setup();
00230     wait(0.1);
00231     pid1.OutputLimits(255,-255);
00232     pid1.start();
00233     pid1.dTarget = 0;
00234     pid2.OutputLimits(255,-255);
00235     pid2.start();
00236     pid2.dTarget = 0;
00237 }