Diff: main.cpp

Revision:

0:1faa9570d725

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Aug 07 12:47:51 2016 +0000
@@ -0,0 +1,237 @@
+#include "mbed.h"
+#include "Motor.h"
+#include "MPU9250.h"
+#include "stepperMotor.h"
+
+#include "PID.h"
+
+#define INTERVAL 0.1
+#define Lx 0.0
+#define Ku 26.2//0.922000
+#define Pu 20.8
+#define Kp Ku*0.182
+#define Ti Pu*0.55
+#define Td Pu*0.35
+#define P Kp
+#define I Kp/Ti
+#define D Kp*Td
+#include<esp8266.cpp>
+
+//DigitalOut myled(LED1);
+Serial esp(dp16,dp15);
+
+Ticker tick;
+SPI spi(dp2,dp1,dp6);//(A6, A5, A4);//(p5,p6,p7)
+mpu9250_spi imu(spi,dp26);//,A3);//,p8)   //define the mpu9250 object
+Timer pi;
+PID pid1(P,I,D,&pi);
+PID pid2(P,I,D,&pi);
+DigitalOut led1(dp28);
+void init();
+
+esp8266 ESP;
+Stepper wheel1(dp13,dp14);//(D11,D12);//dp10,11
+
+Stepper wheel2(dp10,dp9);//(D11,D12);//dp10,11
+#define k1 3.8
+#define k2 1.6
+#define k3 0.1
+#define k4 0.1
+
+float st1()
+{
+    float s1 = wheel1.pulse * 7.5;
+    
+    static float prev_s2 = 0;
+    float s2 = (s1 - prev_s2)/INTERVAL;
+    static float prev = 0;
+    float s5 = (s2 - prev)/INTERVAL;
+    static float prev_s3 =  0;
+    float s3 =  imu.kalman_angle[2];
+    float s4 = (s3 - prev_s3)/INTERVAL;
+    prev_s2=s1;
+    prev_s3=s3;
+    prev = s2;
+    //printf("%f,%f,%f,%f,%f",k1*s5,k2*s2,k3*s3,k4*s4,k1*s1+k2*s2+k3*s3+k4*s4);
+    printf(end);
+    return k1*s3+k2*s4+k3*s2+k4*s1;
+    
+}
+#define k12 k1
+#define k22 k2
+#define k32 k3
+#define k42 k4
+float st2()
+{
+    float s1 = wheel2.pulse * 7.5;
+    
+    static float prev_s2 = 0;
+    float s2 = (s1 - prev_s2)/INTERVAL;
+    static float prev = 0;
+    float s5 = (s2 - prev)/INTERVAL;
+    static float prev_s3 =  0;
+    float s3 =  imu.kalman_angle[2];
+    float s4 = (s3 - prev_s3)/INTERVAL;
+    prev_s2=s1;
+    prev = s2;
+    prev_s3=s3;
+    //printf("%f,%f,%f,%f,%f",k1*s5,k2*s2,k3*s3,k4*s4,k1*s1+k2*s2+k3*s3+k4*s4);
+    printf(end);
+    return k1*s3+k2*s4+k3*s2+k4*s1;
+    
+}
+int main() {
+    //while(1);
+    //printf("st");
+    /*DigitalOut a(dp9);
+    DigitalOut b(dp10);
+    while(1)
+    {
+        a=1;
+        b=0;
+        wait(0.5);
+        a=1;
+        b=1;
+        wait(0.5);
+        a=0;
+        b=1;
+        wait(0.5);
+        a=0;
+        b=0;
+        wait(0.5);
+        printf("rotate");
+        printf(end);
+    }*/
+    
+    init();
+    
+    led1 = 1;
+    wheel1.start();
+    wheel2.start();
+    while(1) {
+        double data[3];
+        imu.read_all();
+        imu.set_angle();
+        imu.get_angle_acc();
+        imu.get_angle(&data[0],&data[1],&data[2]);
+        //printf("%10.3lf, %10.3lf, %10.3f\n",
+        //          imu.kalman_angle[2],imu.angle[2],imu.angle_acc[2]);
+        if(abs(imu.kalman_angle[2]) < 50)
+        {
+            
+            pid1.dPoint = imu.kalman_angle[2];
+            pid2.dPoint = imu.kalman_angle[2];
+            pid1.PIDctrl();
+            pid2.PIDctrl();
+            float d1 = pid1.data , d2 = pid2.data;
+            printf("%10.3f , %10.3f \n" , d1,d2);
+            wheel1 = -st1() / 255; 
+            wheel2 = -st2() / 255;        
+        }
+        else
+        {
+            pid1.pid_reset();
+            pid2.pid_reset();
+            wheel1 = 0;
+            wheel2 = 0;
+            wheel1.pulse = 0;
+            wheel2.pulse = 0;
+        }/*
+        if(imu.kalman_angle[1]<0)
+        {
+            wheel1 = 1;
+            wait(0.5);
+            wheel1 = -1;
+                        wait(0.1);
+        }
+        else if(imu.kalman_angle[1]>0)
+        {
+            wheel1 = -1;
+            wait(0.5);
+            wheel1 = 1;
+            wait(0.1);            
+        }*/
+        wait(INTERVAL);
+    }
+}
+
+void init()
+{
+    //pc.baud(230400);
+    
+    esp.baud(115200);
+    ESP.setSerial(&esp);
+    led1=0;
+    wait(1);
+    for(int i= 0 ; i<6;i++){led1=1;wait(0.05);led1=0;wait(0.05);}
+    wait(1);
+    /*while(1)
+    {
+        int i = esp.getc();
+        if(i == 'A')
+        {
+            led1 = 1;
+            wait(0.5);
+            esp.putc(i);
+        }
+        if(i == 'B')
+        {
+            led1 = 0;
+            wait(0.5);
+            esp.putc(i);
+        }
+    }*/
+    //printf("sart!!");
+    /*while(ESP.ATcommand(""))
+    {
+        wait(2);
+        printf("esp8266 is no connection\n");
+    }*/
+    //ESP.wifiConnectionMode(2);
+    //ESP.wifiSoftAPSetting("ESP8266","ta",5,3);
+    
+    //ESP.WifiFindAccesspoint();
+    //myled = 1;
+    /*
+    do
+    {
+        wait(1);
+        //printf("Finding ESP8266");
+    }while(ESP.WifiFindAccesspoint("ESP8366"));
+    while(ESP.wifiConnect("ESP8366","ryuutaka"))
+    {
+        wait(1);
+//        printf("connecting to ESP8266\n");
+    }*/
+    //printf("esp8266 is acyivity\n");
+    double g[3];
+    //Thread thread2(GET);
+    if(imu.init(1,BITS_DLPF_CFG_188HZ)) { //INIT the mpu9250
+        //printf("\nCouldn't initialize MPU9250 via SPI!");
+    }
+    char x = imu.whoami();
+    do {
+        x = imu.whoami();
+        printf("WHOAMI=0x%2x\n",x); //output the I2C address to know if SPI is working, it should be 104
+        wait(0.3);
+    } while(x!=0x71);
+    //printf("Gyro_scale=%u\n",);    //Set full scale range for gyros
+    imu.set_gyro_scale(BITS_FS_2000DPS);
+    wait(0.3);
+    //printf("Acc_scale=%u\n",imu.set_acc_scale(BITS_FS_16G));          //Set full scale range for accs
+    imu.set_acc_scale(BITS_FS_16G);
+    wait(0.3);
+    do {
+        x=imu.AK8963_whoami();
+        printf("AK8963 WHIAM=0x%2x\n",x);
+        wait(0.3);
+    } while(x!=0x48);
+    imu.MPU_setup();
+    wait(0.1);
+    pid1.OutputLimits(255,-255);
+    pid1.start();
+    pid1.dTarget = 0;
+    pid2.OutputLimits(255,-255);
+    pid2.start();
+    pid2.dTarget = 0;
+}
\ No newline at end of file