Sakai Ritaro / Mbed 2 deprecated Drone

記念祭のドローンのプログラム

Dependencies:   mbed MPU6050 nRF24L01P FastPWM

main.cpp@0:2067d58d510b, 2018-01-06 (annotated)

Committer:
ritarosakai
Date:
Sat Jan 06 03:17:13 2018 +0000
Revision:
0:2067d58d510b
Child:
1:930f9e34f513
First version;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ritarosakai 0:2067d58d510b 1 #include "mbed.h"
ritarosakai 0:2067d58d510b 2 #include "MPU6050.h"
ritarosakai 0:2067d58d510b 3 #include "nRF24L01P.h"
ritarosakai 0:2067d58d510b 4 #include "FastPWM.h"
ritarosakai 0:2067d58d510b 5
ritarosakai 0:2067d58d510b 6 nRF24L01P nrf(A6,A5,A4,D6,D3,A1);
ritarosakai 0:2067d58d510b 7 MPU6050 mpu(D4,D5);
ritarosakai 0:2067d58d510b 8 DigitalOut cpu(D2);/*CPU負荷表示LED
ritarosakai 0:2067d58d510b 9 点灯...ループ内エラー
ritarosakai 0:2067d58d510b 10 薄く点灯...ループ回転中
ritarosakai 0:2067d58d510b 11 0.2秒に0.1秒点灯...ジャイロセンサーのオフセット計測中
ritarosakai 0:2067d58d510b 12 0.6秒に0.1秒点灯...無線断絶
ritarosakai 0:2067d58d510b 13 1秒に0.5秒点灯...加速度センサー認識エラー
ritarosakai 0:2067d58d510b 14 2.1秒に0.1秒点灯...バッテリー低下
ritarosakai 0:2067d58d510b 15 消灯...電源未接続または過度な傾きによる制御停止
ritarosakai 0:2067d58d510b 16 */
ritarosakai 0:2067d58d510b 17 AnalogIn batin(A3);//バッテリー電圧(1/3.4倍)
ritarosakai 0:2067d58d510b 18 Serial pc(USBTX,USBRX);
ritarosakai 0:2067d58d510b 19 FastPWM pwm1(D9);
ritarosakai 0:2067d58d510b 20 FastPWM pwm2(D10);
ritarosakai 0:2067d58d510b 21 FastPWM pwm3(D11);
ritarosakai 0:2067d58d510b 22 FastPWM pwm4(D12);
ritarosakai 0:2067d58d510b 23
ritarosakai 0:2067d58d510b 24 char rxdata[4],txdata[4];
ritarosakai 0:2067d58d510b 25 Ticker sampler;
ritarosakai 0:2067d58d510b 26 float axin,ayin,azin,gxin,gyin,gzin;
ritarosakai 0:2067d58d510b 27 float ax,ay,az,gx,gy,gz,kx,ky,kz,gx_offset,gy_offset,gz_offset,out,gx_old,gy_old;
ritarosakai 0:2067d58d510b 28 float bat,px,py,ix,iy,dx,dy,ddx,ddy,dx_old,dy_old,kxin,kyin,axin_old,ayin_old,kx_old,ky_old;
ritarosakai 0:2067d58d510b 29 float out1,out2,out3,out4,outx,outy,yo,yop,yoi;
ritarosakai 0:2067d58d510b 30 float pxgain=0.01,pygain=0.01;//PID制御のパラメーター
ritarosakai 0:2067d58d510b 31 float ixgain=0.01,iygain=0.01;
ritarosakai 0:2067d58d510b 32 float dxgain=0.003,dygain=0.003;
ritarosakai 0:2067d58d510b 33 float ddxgain=0,ddygain=0;//PIDD制御用
ritarosakai 0:2067d58d510b 34 float yopgain=0.02;//ヨー方向PI制御パラメーター
ritarosakai 0:2067d58d510b 35 float yoigain=0.02;
ritarosakai 0:2067d58d510b 36 float z=-0.1;//上昇値の初期値
ritarosakai 0:2067d58d510b 37 float samp=0.005;//制御周波数(s)
ritarosakai 0:2067d58d510b 38 float maxout=0.99f;//モーターPWM最大
ritarosakai 0:2067d58d510b 39 short i,res;
ritarosakai 0:2067d58d510b 40 int times,nosignal;
ritarosakai 0:2067d58d510b 41 bool enable=1;
ritarosakai 0:2067d58d510b 42 bool timelimit=1;//タイムカウント有効
ritarosakai 0:2067d58d510b 43 float rxkx,rxky;
ritarosakai 0:2067d58d510b 44
ritarosakai 0:2067d58d510b 45 void sample()
ritarosakai 0:2067d58d510b 46 {
ritarosakai 0:2067d58d510b 47 cpu=1;//LED点灯
ritarosakai 0:2067d58d510b 48 nosignal++;
ritarosakai 0:2067d58d510b 49 mpu.read(&gxin,&gyin,&gzin,&axin,&ayin,&azin);//6軸センサー読み取り
ritarosakai 0:2067d58d510b 50 bat=(batin.read()*12.65f + 0.2f)*0.1f+bat*0.9f;//バッテリー電圧読み取り
ritarosakai 0:2067d58d510b 51 if(nrf.readable()) {
ritarosakai 0:2067d58d510b 52 nrf.read( NRF24L01P_PIPE_P0,rxdata,sizeof(rxdata));
ritarosakai 0:2067d58d510b 53 //pc.printf("%2d,%2d,%2d,%2d\r\n",rxdata[0],rxdata[1],rxdata[2],rxdata[3]);
ritarosakai 0:2067d58d510b 54 }
ritarosakai 0:2067d58d510b 55 if(rxdata[3]==1) {
ritarosakai 0:2067d58d510b 56 //times=500;
ritarosakai 0:2067d58d510b 57 nosignal=0;//nosignalをリセット
ritarosakai 0:2067d58d510b 58 rxdata[3]=0;
ritarosakai 0:2067d58d510b 59 }
ritarosakai 0:2067d58d510b 60 if(rxdata[0]==0) {
ritarosakai 0:2067d58d510b 61 rxdata[0]=128;//デフォルト128
ritarosakai 0:2067d58d510b 62 }
ritarosakai 0:2067d58d510b 63 if(rxdata[1]==0) {
ritarosakai 0:2067d58d510b 64 rxdata[1]=128;//デフォルト128
ritarosakai 0:2067d58d510b 65 }
ritarosakai 0:2067d58d510b 66 z=rxdata[2]*0.00485f + 0.3f;//上昇のモーターオフセットの計算
ritarosakai 0:2067d58d510b 67 if(z<0.4f){//可変抵抗が一定値以下のときはモーター出力0,角度の積分値リセット
ritarosakai 0:2067d58d510b 68 z=-0.1f;
ritarosakai 0:2067d58d510b 69 times=500;
ritarosakai 0:2067d58d510b 70 ix=0;
ritarosakai 0:2067d58d510b 71 iy=0;
ritarosakai 0:2067d58d510b 72 yoi=0;
ritarosakai 0:2067d58d510b 73 kz=0;
ritarosakai 0:2067d58d510b 74 kxin=0;
ritarosakai 0:2067d58d510b 75 kyin=0;
ritarosakai 0:2067d58d510b 76 }
ritarosakai 0:2067d58d510b 77 /*axin=axin*0.1f + axin_old*0.9f;
ritarosakai 0:2067d58d510b 78 ayin=ayin*0.1f + ayin_old*0.9f;
ritarosakai 0:2067d58d510b 79 if(axin>1) {
ritarosakai 0:2067d58d510b 80 axin=1;
ritarosakai 0:2067d58d510b 81 } else if(axin<-1) {
ritarosakai 0:2067d58d510b 82 axin=-1;
ritarosakai 0:2067d58d510b 83 }
ritarosakai 0:2067d58d510b 84 if(ayin>1) {
ritarosakai 0:2067d58d510b 85 ayin=1;
ritarosakai 0:2067d58d510b 86 } else if(ayin<-1) {
ritarosakai 0:2067d58d510b 87 ayin=-1;
ritarosakai 0:2067d58d510b 88 }*/
ritarosakai 0:2067d58d510b 89 gx=-gyin+gy_offset;
ritarosakai 0:2067d58d510b 90 gy=gxin-gx_offset;
ritarosakai 0:2067d58d510b 91 gz=gzin-gz_offset;
ritarosakai 0:2067d58d510b 92 //ax=asin(axin)*57.3f;
ritarosakai 0:2067d58d510b 93 //ay=asin(ayin)*57.3f;
ritarosakai 0:2067d58d510b 94 kxin=(kxin+gx*samp)*0.999f;//ジャイロセンサーの値から角度計算
ritarosakai 0:2067d58d510b 95 kyin=(kyin+gy*samp)*0.999f;
ritarosakai 0:2067d58d510b 96 kz=kz+(gz*samp);//ヨー軸の角度計算
ritarosakai 0:2067d58d510b 97 rxkx=(128-rxdata[0])*0.4f;//x軸の角度の指令値
ritarosakai 0:2067d58d510b 98 rxky=(128-rxdata[1])*0.4f;//y軸の角度の指令値
ritarosakai 0:2067d58d510b 99 kx=kxin+rxkx-gx_offset;//x軸の角度の指令値との差分を計算
ritarosakai 0:2067d58d510b 100 ky=kyin+rxky-gy_offset;//y軸の角度の指令値との差分を計算
ritarosakai 0:2067d58d510b 101 px=kx;//PIDのp
ritarosakai 0:2067d58d510b 102 py=ky;
ritarosakai 0:2067d58d510b 103 ix=ix+(px*samp);//PIDのi
ritarosakai 0:2067d58d510b 104 iy=iy+(py*samp);
ritarosakai 0:2067d58d510b 105 dx=(kx-kx_old)/samp;//PIDのd
ritarosakai 0:2067d58d510b 106 dy=(ky-ky_old)/samp;
ritarosakai 0:2067d58d510b 107 yop=kz;//ヨー軸のPIのp
ritarosakai 0:2067d58d510b 108 yoi=yoigain+kz*samp;//ヨー軸のPIのi
ritarosakai 0:2067d58d510b 109 yo=yop*yopgain + yoigain*yoigain;//ヨー軸のPI計算
ritarosakai 0:2067d58d510b 110 ddx=(dx-dx_old)/samp;//PIDD制御用のDD
ritarosakai 0:2067d58d510b 111 ddy=(dy-dy_old)/samp;
ritarosakai 0:2067d58d510b 112 outx=(px*pxgain) + (ix*ixgain) + (dx*dxgain) + (ddx*ddxgain);//PIDでx軸計算
ritarosakai 0:2067d58d510b 113 outy=(py*pygain) + (iy*iygain) + (dy*dygain) + (ddy*ddygain);//PIDでy軸計算
ritarosakai 0:2067d58d510b 114 out1=outx + z+yo;//出力値計算
ritarosakai 0:2067d58d510b 115 out2=-outx + z+yo;
ritarosakai 0:2067d58d510b 116 out3=outy + z-yo;
ritarosakai 0:2067d58d510b 117 out4=-outy + z-yo;
ritarosakai 0:2067d58d510b 118 if(out1>maxout) {//モーターのPWM値の制限
ritarosakai 0:2067d58d510b 119 out1=maxout;
ritarosakai 0:2067d58d510b 120 } else if(out1<0) {
ritarosakai 0:2067d58d510b 121 out1=0;
ritarosakai 0:2067d58d510b 122 }
ritarosakai 0:2067d58d510b 123 if(out2>maxout) {
ritarosakai 0:2067d58d510b 124 out2=maxout;
ritarosakai 0:2067d58d510b 125 } else if(out2<0) {
ritarosakai 0:2067d58d510b 126 out2=0;
ritarosakai 0:2067d58d510b 127 }
ritarosakai 0:2067d58d510b 128 if(out3>maxout) {
ritarosakai 0:2067d58d510b 129 out3=maxout;
ritarosakai 0:2067d58d510b 130 } else if(out3<0) {
ritarosakai 0:2067d58d510b 131 out3=0;
ritarosakai 0:2067d58d510b 132 }
ritarosakai 0:2067d58d510b 133 if(out4>maxout) {
ritarosakai 0:2067d58d510b 134 out4=maxout;
ritarosakai 0:2067d58d510b 135 } else if(out4<0) {
ritarosakai 0:2067d58d510b 136 out4=0;
ritarosakai 0:2067d58d510b 137 }
ritarosakai 0:2067d58d510b 138 if(times==499){//飛行前に積分値リセット
ritarosakai 0:2067d58d510b 139 ix=0;
ritarosakai 0:2067d58d510b 140 iy=0;
ritarosakai 0:2067d58d510b 141 yoi=0;
ritarosakai 0:2067d58d510b 142 kz=0;
ritarosakai 0:2067d58d510b 143 }
ritarosakai 0:2067d58d510b 144 if(bat<6.2f){//バッテリー低下で飛行を中止
ritarosakai 0:2067d58d510b 145 enable=0;
ritarosakai 0:2067d58d510b 146 }
ritarosakai 0:2067d58d510b 147 if(nosignal>20){//無線通信切断
ritarosakai 0:2067d58d510b 148 enable=0;
ritarosakai 0:2067d58d510b 149 }
ritarosakai 0:2067d58d510b 150 if(z<0){//z<0の時モーターを停止
ritarosakai 0:2067d58d510b 151 out1=0;
ritarosakai 0:2067d58d510b 152 out2=0;
ritarosakai 0:2067d58d510b 153 out3=0;
ritarosakai 0:2067d58d510b 154 out4=0;
ritarosakai 0:2067d58d510b 155 }
ritarosakai 0:2067d58d510b 156 if(enable==1&&times>499) {//モーターを回転
ritarosakai 0:2067d58d510b 157 pwm1=out1;
ritarosakai 0:2067d58d510b 158 pwm2=out2;
ritarosakai 0:2067d58d510b 159 pwm3=out3;
ritarosakai 0:2067d58d510b 160 pwm4=out4;
ritarosakai 0:2067d58d510b 161 }else{
ritarosakai 0:2067d58d510b 162 pwm1=0;
ritarosakai 0:2067d58d510b 163 pwm2=0;
ritarosakai 0:2067d58d510b 164 pwm3=0;
ritarosakai 0:2067d58d510b 165 pwm4=0;
ritarosakai 0:2067d58d510b 166 }
ritarosakai 0:2067d58d510b 167 if(enable==0){
ritarosakai 0:2067d58d510b 168 sampler.detach();
ritarosakai 0:2067d58d510b 169 if(nosignal>20){
ritarosakai 0:2067d58d510b 170 while(1){//0.6秒に0.1秒点灯...無線断絶
ritarosakai 0:2067d58d510b 171 cpu=1;
ritarosakai 0:2067d58d510b 172 wait(0.1);
ritarosakai 0:2067d58d510b 173 cpu=0;
ritarosakai 0:2067d58d510b 174 wait(0.5);
ritarosakai 0:2067d58d510b 175 }
ritarosakai 0:2067d58d510b 176 }else if(bat<6.4f&&bat>3.1f){
ritarosakai 0:2067d58d510b 177 while(1){//2.1秒に0.1秒点灯...バッテリー低下
ritarosakai 0:2067d58d510b 178 cpu=1;
ritarosakai 0:2067d58d510b 179 wait(0.1);
ritarosakai 0:2067d58d510b 180 cpu=0;
ritarosakai 0:2067d58d510b 181 wait(2);
ritarosakai 0:2067d58d510b 182 }
ritarosakai 0:2067d58d510b 183 }else if(bat<3.1f){
ritarosakai 0:2067d58d510b 184 sampler.attach(&sample,samp);//バッテリー未接続...ループ再開
ritarosakai 0:2067d58d510b 185 }
ritarosakai 0:2067d58d510b 186 }
ritarosakai 0:2067d58d510b 187 if(abs(kx)>30){//30度以上傾いたら制御停止
ritarosakai 0:2067d58d510b 188 enable=0;
ritarosakai 0:2067d58d510b 189 }
ritarosakai 0:2067d58d510b 190 if(abs(ky)>30){
ritarosakai 0:2067d58d510b 191 enable=0;
ritarosakai 0:2067d58d510b 192 }
ritarosakai 0:2067d58d510b 193 if(abs(kz)>90){//z軸方向に90度以上回ったら制御停止
ritarosakai 0:2067d58d510b 194 enable=0;
ritarosakai 0:2067d58d510b 195 }
ritarosakai 0:2067d58d510b 196 if(timelimit==1) {
ritarosakai 0:2067d58d510b 197 times++;//タイマーカウント
ritarosakai 0:2067d58d510b 198 }
ritarosakai 0:2067d58d510b 199 //axin_old=axin;
ritarosakai 0:2067d58d510b 200 //ayin_old=ayin;
ritarosakai 0:2067d58d510b 201 gx_old=gx;
ritarosakai 0:2067d58d510b 202 gy_old=gy;
ritarosakai 0:2067d58d510b 203 dx_old=dx;
ritarosakai 0:2067d58d510b 204 dy_old=dy;
ritarosakai 0:2067d58d510b 205 kx_old=kx;
ritarosakai 0:2067d58d510b 206 ky_old=ky;
ritarosakai 0:2067d58d510b 207 pc.printf("%.2f,%.2f\r\n",kx,ky);
ritarosakai 0:2067d58d510b 208 txdata[0]=outx*100+128;//制御データーを送信
ritarosakai 0:2067d58d510b 209 txdata[1]=outy*100+128;
ritarosakai 0:2067d58d510b 210 txdata[2]=bat*20;
ritarosakai 0:2067d58d510b 211 txdata[3]=1;
ritarosakai 0:2067d58d510b 212 nrf.write( NRF24L01P_PIPE_P0, txdata, 4 );
ritarosakai 0:2067d58d510b 213 cpu=0;
ritarosakai 0:2067d58d510b 214 }
ritarosakai 0:2067d58d510b 215
ritarosakai 0:2067d58d510b 216 int main()
ritarosakai 0:2067d58d510b 217 {
ritarosakai 0:2067d58d510b 218 cpu=1;
ritarosakai 0:2067d58d510b 219 pc.baud(230400);
ritarosakai 0:2067d58d510b 220 pwm1.period_us(25);//PWM周波数...40kHz
ritarosakai 0:2067d58d510b 221 pwm2.period_us(25);
ritarosakai 0:2067d58d510b 222 pwm3.period_us(25);
ritarosakai 0:2067d58d510b 223 pwm4.period_us(25);
ritarosakai 0:2067d58d510b 224 if(mpu.getID()==0x68) {
ritarosakai 0:2067d58d510b 225 pc.printf("MPU6050 OK Address=0x%x\r\n",mpu.getID());
ritarosakai 0:2067d58d510b 226 } else {
ritarosakai 0:2067d58d510b 227 pc.printf("MPU6050 error ID=0x%x\r\n",mpu.getID());
ritarosakai 0:2067d58d510b 228 while(1) {
ritarosakai 0:2067d58d510b 229 cpu=1;
ritarosakai 0:2067d58d510b 230 wait(0.5);//1秒に0.5秒点灯...加速度センサーエラー
ritarosakai 0:2067d58d510b 231 cpu=0;
ritarosakai 0:2067d58d510b 232 wait(0.5);
ritarosakai 0:2067d58d510b 233 }
ritarosakai 0:2067d58d510b 234 }
ritarosakai 0:2067d58d510b 235 wait(0.1);
ritarosakai 0:2067d58d510b 236 nrf.powerUp();
ritarosakai 0:2067d58d510b 237 nrf.setRxAddress(0xe7e7e7e5a4);
ritarosakai 0:2067d58d510b 238 nrf.setTxAddress(0xe7e7e7e7e6);
ritarosakai 0:2067d58d510b 239 nrf.setAirDataRate(250);
ritarosakai 0:2067d58d510b 240 pc.printf( "%dMHz %ddbm %dKbps ", nrf.getRfFrequency(),nrf.getRfOutputPower(),nrf.getAirDataRate());
ritarosakai 0:2067d58d510b 241 pc.printf("Tx:0x%010llX ",nrf.getTxAddress());
ritarosakai 0:2067d58d510b 242 pc.printf("Rx:0x%010llX \r\n",nrf.getRxAddress());
ritarosakai 0:2067d58d510b 243 nrf.setTransferSize(4);
ritarosakai 0:2067d58d510b 244 nrf.setReceiveMode();
ritarosakai 0:2067d58d510b 245 nrf.enable();
ritarosakai 0:2067d58d510b 246 bat=batin.read();
ritarosakai 0:2067d58d510b 247 bat=bat*12.65f + 0.2f;
ritarosakai 0:2067d58d510b 248 txdata[0]=kx+128;
ritarosakai 0:2067d58d510b 249 txdata[1]=ky+128;
ritarosakai 0:2067d58d510b 250 txdata[2]=bat*20;
ritarosakai 0:2067d58d510b 251 txdata[3]=1;
ritarosakai 0:2067d58d510b 252 nrf.write( NRF24L01P_PIPE_P0, txdata, 4 );//初期値送信
ritarosakai 0:2067d58d510b 253 cpu=0;
ritarosakai 0:2067d58d510b 254 wait(0.2);
ritarosakai 0:2067d58d510b 255 mpu.start();
ritarosakai 0:2067d58d510b 256 bat=batin.read();
ritarosakai 0:2067d58d510b 257 bat=bat*12.65f + 0.2f;
ritarosakai 0:2067d58d510b 258 pc.printf("Battery %4.2fV\r\n",bat);
ritarosakai 0:2067d58d510b 259 if(bat<6.4f) {
ritarosakai 0:2067d58d510b 260 enable=0;
ritarosakai 0:2067d58d510b 261 pc.printf("Battery is low!\r\n");
ritarosakai 0:2067d58d510b 262 }
ritarosakai 0:2067d58d510b 263 txdata[0]=kx+128;
ritarosakai 0:2067d58d510b 264 txdata[1]=ky+128;
ritarosakai 0:2067d58d510b 265 txdata[2]=bat*20;
ritarosakai 0:2067d58d510b 266 txdata[3]=1;
ritarosakai 0:2067d58d510b 267 nrf.write( NRF24L01P_PIPE_P0, txdata, 4 );
ritarosakai 0:2067d58d510b 268 wait(0.5);
ritarosakai 0:2067d58d510b 269 cpu=1;
ritarosakai 0:2067d58d510b 270 while(i<100) {//ジャイロセンサーのオフセット測定
ritarosakai 0:2067d58d510b 271 mpu.read(&gxin,&gyin,&gzin,&axin,&ayin,&azin);
ritarosakai 0:2067d58d510b 272 gx_offset=gx_offset+gxin;
ritarosakai 0:2067d58d510b 273 gy_offset=gy_offset+gyin;
ritarosakai 0:2067d58d510b 274 gz_offset=gz_offset+gzin;
ritarosakai 0:2067d58d510b 275 times++;
ritarosakai 0:2067d58d510b 276 if(times==10) {
ritarosakai 0:2067d58d510b 277 cpu=!cpu;
ritarosakai 0:2067d58d510b 278 times=0;
ritarosakai 0:2067d58d510b 279 }
ritarosakai 0:2067d58d510b 280 i++;
ritarosakai 0:2067d58d510b 281 wait(0.01);
ritarosakai 0:2067d58d510b 282 }
ritarosakai 0:2067d58d510b 283 i=0;
ritarosakai 0:2067d58d510b 284 gx_offset=gx_offset/100;//ジャイロセンサーのオフセット計算
ritarosakai 0:2067d58d510b 285 gy_offset=gy_offset/100;
ritarosakai 0:2067d58d510b 286 gz_offset=gz_offset/100;
ritarosakai 0:2067d58d510b 287 cpu=0;
ritarosakai 0:2067d58d510b 288 //gx_offset=-2.63;
ritarosakai 0:2067d58d510b 289 //gy_offset=-0.71;
ritarosakai 0:2067d58d510b 290 pc.printf("gyro offset %.2f %.2f %.2f\r\n",gx_offset,gy_offset,gz_offset);
ritarosakai 0:2067d58d510b 291 wait(2);
ritarosakai 0:2067d58d510b 292 sampler.attach_us(&sample,samp*1000000);//ループ開始
ritarosakai 0:2067d58d510b 293 while(1) {
ritarosakai 0:2067d58d510b 294 wait(0.01);
ritarosakai 0:2067d58d510b 295 }
ritarosakai 0:2067d58d510b 296 }