KKmoonparkingsensorControlboxLEDLCD示的接口，power接入，蜂鳴器控制接口等。距離物的距離。Sensorpart基于知識背景，設(shè)計了一個基于MPU6050陀螺儀角加速度傳感器的六軸體感機械基本原理：通過三個MPU6050貼在胳膊的不同地方，用來現(xiàn)在胳膊的三點加速度FPGA65MPU6050貼片3FPGAArtex-本系統(tǒng)以Xilinx公司的xc74a35tcpg236-1fpga為主控制器，基于mpu6050 系統(tǒng)特色：①6050濾波采用了卡爾曼濾波，確保角度的穩(wěn)定性②具有可靠 案論方案一利用MicroBlaze軟核ip核自己構(gòu)建一個軟核單片機，然后利用mpu6050比利用c語言算法經(jīng)行濾波解算。方案優(yōu)勢是可以利用c語言豐富的c語言庫方便的解算sin和cos函數(shù)從而方案缺點是MicroBlaze構(gòu)建比較繁瑣，要自己構(gòu)建總線和接口，效率比較低，對fpga利用面積上考慮比較差。利用時序電路來模擬i2c的高低電平，并且用Verilog編寫mpu6050的數(shù)據(jù)的程序。然后返回其他模塊模擬輸入輸出來模擬6050數(shù)據(jù)的過程，然后返回cordic算法計fpga的sin和cos以及arctan函數(shù)。進行四元數(shù)解算和輸出。1.2系統(tǒng)總體框圖1.2系統(tǒng)總體框圖二．四元數(shù)的結(jié)算和姿態(tài)控mpu6050Mpu6050有兩種輸出一種是通過串行通訊協(xié)議直接讀出三軸角加速度和角的驅(qū)動需要特殊程序，而且是基于c語言和c++的用fpga實現(xiàn)起來比較困為,在內(nèi)的投影為由于旋轉(zhuǎn)繞進行，所以Z坐標未變，即有。在解算中，要首先把加速度計到的值(三維向量)轉(zhuǎn)化為單位向量,即向量除以模，傳入?yún)?shù)是陀螺儀x,y,z值和加速度計x,y,z值axyz是測量得到的重力向量，vxyz是陀螺積分后的姿態(tài)來推算出的重力向T這里使用了一階庫塔求解四元數(shù)微分方程：Q_ANGLE.Yaw=atan2(2*q1*q2+2*q0*q3,-2*q2*q2-2*q3*q31)*57.3;// =asin(-2*q1*q3+2*q0*q2)*57.3;//Q_ANGLE.X=atan2(2*q2*q3+2*q0*q1,-2*q1*q1-2*q2*q2+57.3;//6050姿態(tài)的設(shè)中一個6050負責(zé)大臂的姿態(tài)，一個6050小臂的姿態(tài)。在手部因為硬件的限制，采用了鉗式結(jié)構(gòu)抓取東西，程度的時候，即認為已經(jīng)抓取物體成功，自動鎖定角度。三．機械臂機械機構(gòu)的設(shè)機械骨架FPGA通過的信號輸出經(jīng)過升壓電路驅(qū)動，輸送到舵機。不同100hz5v590分之25為向右90度，百分之15為中值點，后期通過波微調(diào)中值即可獲四．系統(tǒng)測試及結(jié)果分測試使4.1序號12213SIGLENTSDS-1102CML141516測試方 測試數(shù)序號41516分析6050采用積分方式解算姿態(tài)，但隨著時間增長，積分獲得的誤差值會隨附錄16050程inputclk,rst_n;outputscl;inoutsda; regalways@(posedgeclkornegedgerst_n)cnt_10ms<=cnt_10ms<=always@(posedgeclkornegedgerst_n)cnt_sum<=cnt_sum<=0;cnt_sum<=always@(posedgeclkornegedgerst_n)cnt<=default:cnt<=3'd5;`defineSCL_POS`defineSCL_HIG`defineSCL_NEG`defineSCL_LOWalways@(posedgeclkornegedgerst_n)elseif(cnt==3'd0)scl_r<=1'b1;elsescl_r<=assignsclscl_r;//scl`defineACC_XH8'h3B//加速度x軸地`defineACC_XL8'h3Cx`defineACC_YH8'h3D//加速度y軸地`defineACC_YL8'h3Ey`defineACC_ZH8'h3F//加速度z軸地`defineACC_ZL8'h40z`defineGYRO_XH8'h43//陀螺儀x軸地`defineGYRO_XL8'h44x`defineGYRO_YH8'h45//陀螺儀y軸地`defineGYRO_YL8'h46y`defineGYRO_ZH8'h47//陀螺儀z軸地`defineGYRO_ZL8'h48z`definePWR_MGMT_1`defineSMPLRT_DIV`defineCONFIG1`defineGYRO_CONFIG`defineACC_CONFIG`definePWR_MGMT_1_VAL`defineSMPLRT_DIV_VAL`defineCONFIG1_VAL`defineGYRO_CONFIG_VAL`defineACC_CONFIG_VALparameterIDLE=4'd0;parameterSTART1=4'd1;parameterADD1=4'd2;parameterACK1=4'd3;parameterADD2=4'd4;parameterACK2=4'd5;parameterSTART2=4'd6;parameterADD3=4'd7;parameterACK3=4'd8;parameterDATA=4'd9;parameterACK4=4'd10;parameterSTOP1=4'd11;parameterSTOP2=4'd12;parameterADD_EXT=4'd13;parameterACK_EXT=regsda_r;//輸出reg[3:0]num;regreg[7:0]ACC_XH_READ;//加速度X軸高八位reg[7:0]ACC_XL_READ;//加速度X軸低八位reg[7:0]ACC_YH_READ;//加速度Y軸高八位reg[7:0]ACC_YL_READ;//加速度Y軸低八位reg[7:0]ACC_ZH_READ;//加速度Z軸高八位reg[7:0]ACC_ZL_READ;//加速度Z軸低八位reg[7:0]GYRO_XH_READ;//陀螺儀X軸高八位reg[7:0]GYRO_XL_READ;//陀螺儀X軸低八位reg[7:0]GYRO_YH_READ;//陀螺儀Y軸高八位reg[7:0]GYRO_YL_READ;//陀螺儀Y軸低八位reg 陀螺儀Z軸低八always@(posedgeclkornegedgerst_n)state<=sda_link<=1'b0;//高阻態(tài)num<=4'd0;ACC_XH_READ<=8'h00;ACC_XL_READ<=8'h00;ACC_YH_READ<=8'h00;ACC_YL_READ<=8'h00;ACC_ZH_READ<=8'h00;ACC_ZL_READ<=8'h00;GYRO_XL_READ8'h00;GYRO_YL_READ8'h00;GYRO_ZL_READ<=times<=times<=sda_link1'b1;//sdasda_r<=1'b1;//拉高sdastate=START1;sda_link1'b1;//sdasda_r1'b0sda，startstate<=ADD1;num<=state<=if(num4'd88sda_r<=1'b1;sda_link1'b0;//sdastate<=ACK1;state<=ADD1;<=state<=state<=5'd1:db_r<=`PWR_MGMT_1;5'd2:db_r<=5'd3:db_r<=5'd4:db_r<=5'd5:db_r<=5'd6:db_r<=5'd7:db_r<=5'd8:db_r<=5'd9:db_r<=5'd10:db_r<=5'd11:db_r<=5'd12:db_r<=5'd13:db_r<=5'd14:db_r<=5'd15:db_r<=5'd16:db_r<=5'd17:db_r<=`GYRO_ZL;default:begintimes<=5'd1;stateACK1ADD2:beginnum<=4'd0;sda_r<=1'b1;state<=ACK2;state<=ADD2;num<=<=址state<=3'd1:db_r<=3'd2:db_r<=3'd3:db_r<=3'd4:db_r<=3'd5:db_r<=3'd6:db_r<=default:db_r<=`DEVICE_READ;if(times>=state<=state<=stateACK2num<=4'd0;sda_link1'b0;//sdastate<=ACK_EXT;sda_link<=1'b1;state<=ADD_EXT;<=state<=state<=STOP1;stateACK_EXTsda_r<=1'b1;//拉高sdastate<=START2;sda_r1'b0sda，startstate<=ADD3;state<=ADD3:beginnum<=sda_r<=1'b1;//拉高sdastate<=ACK3;num<=sda_r<=db_r[4'd7-num];//按位寫入state<=elsestate<=ACK3:beginstate<=sda_link1'b0;//sdastateACK3DATA:beginnum<=5'd6:ACC_XH_READ[4'd7-num]<=5'd7:ACC_XL_READ[4'd7-num]<=5'd8:ACC_YH_READ[4'd7-num]<=5'd9:ACC_YL_READ[4'd7-num]<= 5'd12:GYRO_XH_READ[4'd7-num]5'd13:GYRO_XL_READ[4'd7-num]5'd14:GYRO_YH_READ[4'd7-num]5'd15:GYRO_YL_READ[4'd7-num]5'd16:GYRO_ZH_READ[4'd7-num]5'd17:GYRO_ZL_READ[4'd7-num]elseif((`SCL_LOW)&&(num==4'd8))num<=4'd0;//計數(shù)清零state<=ACK4;state<=ACK4:beginif(times==5'd17)state<=STOP1;stateACK4sda_link1'b1;//sdasda_r<=1'b0;//拉低sdastate<=STOP1;sda_r1'b1sdastopstate<=STOP2;state<=elseif(cnt_10ms20'hffff010msstate<=state<=assignsda=moduleinputCLK;input[7:0]ACC_XH_READ;//加速度X軸高八位input[7:0]ACC_XL_READ;//加速度X軸低八位input[7:0]ACC_YH_READ;//加速度Y軸高八位input[7:0]ACC_YL_READ;//加速度Y軸低八位input[7:0]ACC_ZH_READ;//加速度Z軸高八位input[7:0]ACC_ZL_READ;//加速度Z軸低八位input[7:0]GYRO_XH_READ;//陀螺儀X軸高八位input[7:0]GYRO_XL_READ;//陀螺儀X軸低八位input[7:0]GYRO_YH_READ;//陀螺儀Y軸高八位input[7:0]GYRO_YL_READ;//陀螺儀Y軸低八位input[7:0]GYRO_ZH_READ;//陀螺儀Z軸高八位input[7:0]GYRO_ZL_READ;//陀螺儀Z軸低八位output[31:0]Yaw,Roll;wire[7:0]ACC_XH_READ;//加速度X軸高八位wire[7:0]ACC_XL_READ;//加速度X軸低八位wire[7:0]ACC_YH_READ;//加速度Y軸高八位wire[7:0]ACC_YL_READ;//加速度Y軸低八位wire[7:0]ACC_ZH_READ;//Zwire[7:0]ACC_ZL_READ;//加速度Z軸低八位wire[7:0]GYRO_XH_READ;//陀螺儀X軸高八位wire[7:0]GYRO_XL_READ;//陀螺儀X軸低八位wire[7:0]GYRO_YH_READ;//陀螺儀Y軸高八位wire[7:0]GYRO_YL_READ;//陀螺儀Y軸低八位wire[7:0]GYRO_ZH_READ;//陀螺儀Z軸高八位wire[7:0]GYRO_ZL_READ;//陀螺儀Z軸低八reg[31:0]reg[31:0]reg[31:0]reg[31:0]reg[31:0]reg[31:0]reg[31:0]wire[31:0]reg[31:0]reg[31:0]reg[31:0]reg[31:0]wire[23:0]wirevalid;reg[23:0]reg[31:0]reg[63:0]reg[31:0]reg[31:0]reg[31:0]reg[31:0]reg[63:0]reg[31:0]reg[15:0b=16'h0100;//168reg[31:0]reg[31:0]parameterinitialbegin//assign//a1norm1_0cordic_0mycordic1 //wire //inputwire .m_axis_dout_tvalid(), //outputwire //a2norm2_0cordic_0mycordic2 inputwire //inputwire[31: //output //outputwire[23:0]m_axis_dout_tdata inputwire //inputwire output //outputwire[31:cordic_1mycordic4 inputwire //inputwire[31:.m_axis_dout_tvalid( //output //outputwire[310]always@(posedgeCLK)begin88/