Kiko Ishimoto / Mbed 2 deprecated OneCircleRobot

２輪に移行

Dependencies:   MPU92502 Motor2 PIDonerobot Stepper mbed

main.cpp@1:7ebc0f5f847b, 2016-08-07 (annotated)

Committer:
kikoaac
Date:
Sun Aug 07 12:50:56 2016 +0000
Revision:
1:7ebc0f5f847b
Parent:
0:1faa9570d725 
??????;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kikoaac 0:1faa9570d725 1 #include "mbed.h"
kikoaac 0:1faa9570d725 2 #include "Motor.h"
kikoaac 0:1faa9570d725 3 #include "MPU9250.h"
kikoaac 0:1faa9570d725 4 #include "stepperMotor.h"
kikoaac 0:1faa9570d725 5
kikoaac 0:1faa9570d725 6 #include "PID.h"
kikoaac 0:1faa9570d725 7
kikoaac 0:1faa9570d725 8 #define INTERVAL 0.1
kikoaac 0:1faa9570d725 9 #define Lx 0.0
kikoaac 0:1faa9570d725 10 #define Ku 26.2//0.922000
kikoaac 0:1faa9570d725 11 #define Pu 20.8
kikoaac 0:1faa9570d725 12 #define Kp Ku*0.182
kikoaac 0:1faa9570d725 13 #define Ti Pu*0.55
kikoaac 0:1faa9570d725 14 #define Td Pu*0.35
kikoaac 0:1faa9570d725 15 #define P Kp
kikoaac 0:1faa9570d725 16 #define I Kp/Ti
kikoaac 0:1faa9570d725 17 #define D Kp*Td
kikoaac 0:1faa9570d725 18 #include<esp8266.cpp>
kikoaac 0:1faa9570d725 19
kikoaac 0:1faa9570d725 20 //DigitalOut myled(LED1);
kikoaac 0:1faa9570d725 21 Serial esp(dp16,dp15);
kikoaac 0:1faa9570d725 22
kikoaac 0:1faa9570d725 23 Ticker tick;
kikoaac 0:1faa9570d725 24 SPI spi(dp2,dp1,dp6);//(A6, A5, A4);//(p5,p6,p7)
kikoaac 0:1faa9570d725 25 mpu9250_spi imu(spi,dp26);//,A3);//,p8) //define the mpu9250 object
kikoaac 0:1faa9570d725 26 Timer pi;
kikoaac 0:1faa9570d725 27 PID pid1(P,I,D,&pi);
kikoaac 0:1faa9570d725 28 PID pid2(P,I,D,&pi);
kikoaac 0:1faa9570d725 29 DigitalOut led1(dp28);
kikoaac 0:1faa9570d725 30 void init();
kikoaac 0:1faa9570d725 31
kikoaac 0:1faa9570d725 32 esp8266 ESP;
kikoaac 0:1faa9570d725 33 Stepper wheel1(dp13,dp14);//(D11,D12);//dp10,11
kikoaac 0:1faa9570d725 34
kikoaac 0:1faa9570d725 35 Stepper wheel2(dp10,dp9);//(D11,D12);//dp10,11
kikoaac 0:1faa9570d725 36 #define k1 3.8
kikoaac 0:1faa9570d725 37 #define k2 1.6
kikoaac 0:1faa9570d725 38 #define k3 0.1
kikoaac 0:1faa9570d725 39 #define k4 0.1
kikoaac 0:1faa9570d725 40
kikoaac 0:1faa9570d725 41 float st1()
kikoaac 0:1faa9570d725 42 {
kikoaac 0:1faa9570d725 43 float s1 = wheel1.pulse * 7.5;
kikoaac 0:1faa9570d725 44
kikoaac 0:1faa9570d725 45 static float prev_s2 = 0;
kikoaac 0:1faa9570d725 46 float s2 = (s1 - prev_s2)/INTERVAL;
kikoaac 0:1faa9570d725 47 static float prev = 0;
kikoaac 0:1faa9570d725 48 float s5 = (s2 - prev)/INTERVAL;
kikoaac 0:1faa9570d725 49 static float prev_s3 = 0;
kikoaac 0:1faa9570d725 50 float s3 = imu.kalman_angle[2];
kikoaac 0:1faa9570d725 51 float s4 = (s3 - prev_s3)/INTERVAL;
kikoaac 0:1faa9570d725 52 prev_s2=s1;
kikoaac 0:1faa9570d725 53 prev_s3=s3;
kikoaac 0:1faa9570d725 54 prev = s2;
kikoaac 0:1faa9570d725 55 //printf("%f,%f,%f,%f,%f",k1*s5,k2*s2,k3*s3,k4*s4,k1*s1+k2*s2+k3*s3+k4*s4);
kikoaac 0:1faa9570d725 56 printf(end);
kikoaac 0:1faa9570d725 57 return k1*s3+k2*s4+k3*s2+k4*s1;
kikoaac 0:1faa9570d725 58
kikoaac 0:1faa9570d725 59 }
kikoaac 0:1faa9570d725 60 #define k12 k1
kikoaac 0:1faa9570d725 61 #define k22 k2
kikoaac 0:1faa9570d725 62 #define k32 k3
kikoaac 0:1faa9570d725 63 #define k42 k4
kikoaac 0:1faa9570d725 64 float st2()
kikoaac 0:1faa9570d725 65 {
kikoaac 0:1faa9570d725 66 float s1 = wheel2.pulse * 7.5;
kikoaac 0:1faa9570d725 67
kikoaac 0:1faa9570d725 68 static float prev_s2 = 0;
kikoaac 0:1faa9570d725 69 float s2 = (s1 - prev_s2)/INTERVAL;
kikoaac 0:1faa9570d725 70 static float prev = 0;
kikoaac 0:1faa9570d725 71 float s5 = (s2 - prev)/INTERVAL;
kikoaac 0:1faa9570d725 72 static float prev_s3 = 0;
kikoaac 0:1faa9570d725 73 float s3 = imu.kalman_angle[2];
kikoaac 0:1faa9570d725 74 float s4 = (s3 - prev_s3)/INTERVAL;
kikoaac 0:1faa9570d725 75 prev_s2=s1;
kikoaac 0:1faa9570d725 76 prev = s2;
kikoaac 0:1faa9570d725 77 prev_s3=s3;
kikoaac 0:1faa9570d725 78 //printf("%f,%f,%f,%f,%f",k1*s5,k2*s2,k3*s3,k4*s4,k1*s1+k2*s2+k3*s3+k4*s4);
kikoaac 0:1faa9570d725 79 printf(end);
kikoaac 0:1faa9570d725 80 return k1*s3+k2*s4+k3*s2+k4*s1;
kikoaac 0:1faa9570d725 81
kikoaac 0:1faa9570d725 82 }
kikoaac 0:1faa9570d725 83 int main() {
kikoaac 0:1faa9570d725 84 //while(1);
kikoaac 0:1faa9570d725 85 //printf("st");
kikoaac 0:1faa9570d725 86 /*DigitalOut a(dp9);
kikoaac 0:1faa9570d725 87 DigitalOut b(dp10);
kikoaac 0:1faa9570d725 88 while(1)
kikoaac 0:1faa9570d725 89 {
kikoaac 0:1faa9570d725 90 a=1;
kikoaac 0:1faa9570d725 91 b=0;
kikoaac 0:1faa9570d725 92 wait(0.5);
kikoaac 0:1faa9570d725 93 a=1;
kikoaac 0:1faa9570d725 94 b=1;
kikoaac 0:1faa9570d725 95 wait(0.5);
kikoaac 0:1faa9570d725 96 a=0;
kikoaac 0:1faa9570d725 97 b=1;
kikoaac 0:1faa9570d725 98 wait(0.5);
kikoaac 0:1faa9570d725 99 a=0;
kikoaac 0:1faa9570d725 100 b=0;
kikoaac 0:1faa9570d725 101 wait(0.5);
kikoaac 0:1faa9570d725 102 printf("rotate");
kikoaac 0:1faa9570d725 103 printf(end);
kikoaac 0:1faa9570d725 104 }*/
kikoaac 0:1faa9570d725 105
kikoaac 0:1faa9570d725 106 init();
kikoaac 0:1faa9570d725 107
kikoaac 0:1faa9570d725 108 led1 = 1;
kikoaac 0:1faa9570d725 109 wheel1.start();
kikoaac 0:1faa9570d725 110 wheel2.start();
kikoaac 0:1faa9570d725 111 while(1) {
kikoaac 0:1faa9570d725 112 double data[3];
kikoaac 0:1faa9570d725 113 imu.read_all();
kikoaac 0:1faa9570d725 114 imu.set_angle();
kikoaac 0:1faa9570d725 115 imu.get_angle_acc();
kikoaac 0:1faa9570d725 116 imu.get_angle(&data[0],&data[1],&data[2]);
kikoaac 0:1faa9570d725 117 //printf("%10.3lf, %10.3lf, %10.3f\n",
kikoaac 0:1faa9570d725 118 // imu.kalman_angle[2],imu.angle[2],imu.angle_acc[2]);
kikoaac 0:1faa9570d725 119 if(abs(imu.kalman_angle[2]) < 50)
kikoaac 0:1faa9570d725 120 {
kikoaac 0:1faa9570d725 121
kikoaac 0:1faa9570d725 122 pid1.dPoint = imu.kalman_angle[2];
kikoaac 0:1faa9570d725 123 pid2.dPoint = imu.kalman_angle[2];
kikoaac 0:1faa9570d725 124 pid1.PIDctrl();
kikoaac 0:1faa9570d725 125 pid2.PIDctrl();
kikoaac 0:1faa9570d725 126 float d1 = pid1.data , d2 = pid2.data;
kikoaac 0:1faa9570d725 127 printf("%10.3f , %10.3f \n" , d1,d2);
kikoaac 0:1faa9570d725 128 wheel1 = -st1() / 255;
kikoaac 0:1faa9570d725 129 wheel2 = -st2() / 255;
kikoaac 0:1faa9570d725 130 }
kikoaac 0:1faa9570d725 131 else
kikoaac 0:1faa9570d725 132 {
kikoaac 0:1faa9570d725 133 pid1.pid_reset();
kikoaac 0:1faa9570d725 134 pid2.pid_reset();
kikoaac 0:1faa9570d725 135 wheel1 = 0;
kikoaac 0:1faa9570d725 136 wheel2 = 0;
kikoaac 0:1faa9570d725 137 wheel1.pulse = 0;
kikoaac 0:1faa9570d725 138 wheel2.pulse = 0;
kikoaac 0:1faa9570d725 139 }/*
kikoaac 0:1faa9570d725 140 if(imu.kalman_angle[1]<0)
kikoaac 0:1faa9570d725 141 {
kikoaac 0:1faa9570d725 142 wheel1 = 1;
kikoaac 0:1faa9570d725 143 wait(0.5);
kikoaac 0:1faa9570d725 144 wheel1 = -1;
kikoaac 0:1faa9570d725 145 wait(0.1);
kikoaac 0:1faa9570d725 146 }
kikoaac 0:1faa9570d725 147 else if(imu.kalman_angle[1]>0)
kikoaac 0:1faa9570d725 148 {
kikoaac 0:1faa9570d725 149 wheel1 = -1;
kikoaac 0:1faa9570d725 150 wait(0.5);
kikoaac 0:1faa9570d725 151 wheel1 = 1;
kikoaac 0:1faa9570d725 152 wait(0.1);
kikoaac 0:1faa9570d725 153 }*/
kikoaac 0:1faa9570d725 154 wait(INTERVAL);
kikoaac 0:1faa9570d725 155 }
kikoaac 0:1faa9570d725 156 }
kikoaac 0:1faa9570d725 157
kikoaac 0:1faa9570d725 158 void init()
kikoaac 0:1faa9570d725 159 {
kikoaac 0:1faa9570d725 160 //pc.baud(230400);
kikoaac 0:1faa9570d725 161
kikoaac 0:1faa9570d725 162 esp.baud(115200);
kikoaac 0:1faa9570d725 163 ESP.setSerial(&esp);
kikoaac 0:1faa9570d725 164 led1=0;
kikoaac 0:1faa9570d725 165 wait(1);
kikoaac 0:1faa9570d725 166 for(int i= 0 ; i<6;i++){led1=1;wait(0.05);led1=0;wait(0.05);}
kikoaac 0:1faa9570d725 167 wait(1);
kikoaac 0:1faa9570d725 168 /*while(1)
kikoaac 0:1faa9570d725 169 {
kikoaac 0:1faa9570d725 170 int i = esp.getc();
kikoaac 0:1faa9570d725 171 if(i == 'A')
kikoaac 0:1faa9570d725 172 {
kikoaac 0:1faa9570d725 173 led1 = 1;
kikoaac 0:1faa9570d725 174 wait(0.5);
kikoaac 0:1faa9570d725 175 esp.putc(i);
kikoaac 0:1faa9570d725 176 }
kikoaac 0:1faa9570d725 177 if(i == 'B')
kikoaac 0:1faa9570d725 178 {
kikoaac 0:1faa9570d725 179 led1 = 0;
kikoaac 0:1faa9570d725 180 wait(0.5);
kikoaac 0:1faa9570d725 181 esp.putc(i);
kikoaac 0:1faa9570d725 182 }
kikoaac 0:1faa9570d725 183 }*/
kikoaac 0:1faa9570d725 184 //printf("sart!!");
kikoaac 0:1faa9570d725 185 /*while(ESP.ATcommand(""))
kikoaac 0:1faa9570d725 186 {
kikoaac 0:1faa9570d725 187 wait(2);
kikoaac 0:1faa9570d725 188 printf("esp8266 is no connection\n");
kikoaac 0:1faa9570d725 189 }*/
kikoaac 0:1faa9570d725 190 //ESP.wifiConnectionMode(2);
kikoaac 0:1faa9570d725 191 //ESP.wifiSoftAPSetting("ESP8266","ta",5,3);
kikoaac 0:1faa9570d725 192
kikoaac 0:1faa9570d725 193 //ESP.WifiFindAccesspoint();
kikoaac 0:1faa9570d725 194 //myled = 1;
kikoaac 0:1faa9570d725 195 /*
kikoaac 0:1faa9570d725 196 do
kikoaac 0:1faa9570d725 197 {
kikoaac 0:1faa9570d725 198 wait(1);
kikoaac 0:1faa9570d725 199 //printf("Finding ESP8266");
kikoaac 0:1faa9570d725 200 }while(ESP.WifiFindAccesspoint("ESP8366"));
kikoaac 0:1faa9570d725 201 while(ESP.wifiConnect("ESP8366","ryuutaka"))
kikoaac 0:1faa9570d725 202 {
kikoaac 0:1faa9570d725 203 wait(1);
kikoaac 0:1faa9570d725 204 // printf("connecting to ESP8266\n");
kikoaac 0:1faa9570d725 205 }*/
kikoaac 0:1faa9570d725 206 //printf("esp8266 is acyivity\n");
kikoaac 0:1faa9570d725 207 double g[3];
kikoaac 0:1faa9570d725 208 //Thread thread2(GET);
kikoaac 0:1faa9570d725 209 if(imu.init(1,BITS_DLPF_CFG_188HZ)) { //INIT the mpu9250
kikoaac 0:1faa9570d725 210 //printf("\nCouldn't initialize MPU9250 via SPI!");
kikoaac 0:1faa9570d725 211 }
kikoaac 0:1faa9570d725 212 char x = imu.whoami();
kikoaac 0:1faa9570d725 213 do {
kikoaac 0:1faa9570d725 214 x = imu.whoami();
kikoaac 0:1faa9570d725 215 printf("WHOAMI=0x%2x\n",x); //output the I2C address to know if SPI is working, it should be 104
kikoaac 0:1faa9570d725 216 wait(0.3);
kikoaac 0:1faa9570d725 217 } while(x!=0x71);
kikoaac 0:1faa9570d725 218 //printf("Gyro_scale=%u\n",); //Set full scale range for gyros
kikoaac 0:1faa9570d725 219 imu.set_gyro_scale(BITS_FS_2000DPS);
kikoaac 0:1faa9570d725 220 wait(0.3);
kikoaac 0:1faa9570d725 221 //printf("Acc_scale=%u\n",imu.set_acc_scale(BITS_FS_16G)); //Set full scale range for accs
kikoaac 0:1faa9570d725 222 imu.set_acc_scale(BITS_FS_16G);
kikoaac 0:1faa9570d725 223 wait(0.3);
kikoaac 0:1faa9570d725 224 do {
kikoaac 0:1faa9570d725 225 x=imu.AK8963_whoami();
kikoaac 0:1faa9570d725 226 printf("AK8963 WHIAM=0x%2x\n",x);
kikoaac 0:1faa9570d725 227 wait(0.3);
kikoaac 0:1faa9570d725 228 } while(x!=0x48);
kikoaac 0:1faa9570d725 229 imu.MPU_setup();
kikoaac 0:1faa9570d725 230 wait(0.1);
kikoaac 0:1faa9570d725 231 pid1.OutputLimits(255,-255);
kikoaac 0:1faa9570d725 232 pid1.start();
kikoaac 0:1faa9570d725 233 pid1.dTarget = 0;
kikoaac 0:1faa9570d725 234 pid2.OutputLimits(255,-255);
kikoaac 0:1faa9570d725 235 pid2.start();
kikoaac 0:1faa9570d725 236 pid2.dTarget = 0;
kikoaac 0:1faa9570d725 237 }