FPGA麦克纳姆轮控制代码 -

/********************************版权声明**************************************
** 大西瓜团队
**
**麦克纳姆轮小车控制程序已完结
*******************************************************************************/
module ADC_TLC549
(
clk, //系统50MHZ时钟
adc_sclk, //AD TLC549的时钟
data, //AD TLC549的数据口
cs, //AD TLC549的片选择
adc_sclk2, //AD TLC549的时钟
data2, //AD TLC549的数据口
cs2, //AD TLC549的片选择
adc_sclk3, //AD TLC549的时钟
data3, //AD TLC549的数据口
cs3, //AD TLC549的片选择
adc_sclk4, //AD TLC549的时钟
data4, //AD TLC549的数据口
cs4, //AD TLC549的片选择
adc_sclk6, //AD TLC549的时钟
data_, //AD TLC549的数据口
cs6, //AD TLC549的片选择
led,
M,
fashe,
zhuanwan,
chenggan_pwm,
chenggan,
zhuantai_pwm,
zhuantai
);
input clk;
input zhuanwan;
input data;
input data2;
input data3;
input data4;
input data_;
output cs;
output adc_sclk;
output cs2;
output adc_sclk2;
output cs3;
output adc_sclk3;
output cs4;
output adc_sclk4;
output cs6;
output adc_sclk6;
output[3:0] led;
output[7:0] M;
output[1:0] fashe;
output chenggan_pwm;
output zhuantai_pwm;
output[1:0] chenggan;
output[1:0] zhuantai;
reg[7:0] M;
reg[1:0] zhuantai;
reg[1:0] chenggan;
reg [32:0] count1;
reg [32:0] countchenggan;
reg [10:0] pwm_count;
reg flag;
reg[3:0] pwm_flag;
reg pwm_flag4;
reg[16:0] tenvalue1;
reg flag_pwm;
reg cs,adc_sclk,clk1k,clk1ms;
reg[15:0] coun;
reg[15:0] count;
reg[15:0] count5;
reg[15:0] count7;
reg[24:0] count1ms;
reg[3:0] cnt;
reg[2:0] number;
reg[1:0] state;
reg[3:0] wei;
reg ledcs;
reg [7:0] duan;
reg[7:0] dataout;
reg[16:0] tenvalue;
reg[16:0]data5;
reg[16:0]data6;
reg[16:0]data7;
reg[16:0]data8;
reg [32:0] count12;
reg [10:0] pwm_count2;
reg [10:0] pwm_count4;
reg flag2;
reg[1:0] pwm_flag2;
reg[16:0] tenvalue12;
reg cs2,adc_sclk2,clk1k2,clk1ms2;
reg cs3,adc_sclk3,clk1k3,clk1ms3;
reg cs4,adc_sclk4,clk1k4,clk1ms4;
reg cs6,adc_sclk6,clk1k6,clk1ms6;
reg[15:0] count2;
reg[15:0] count3;
reg[15:0] count4;
reg[15:0] count6;
reg[24:0] count1ms2;
reg[24:0] count1ms3;
reg[24:0] count1ms4;
reg[24:0] count1ms6;
reg[3:0] cnt2;
reg[3:0] cnt3;
reg[3:0] cnt4;
reg[3:0] cnt6;
//reg[2:0] number;
reg[1:0] state2;
reg[1:0] state3;
reg[1:0] state4;
reg[1:0] state6;
//reg[3:0] wei;
//reg ledcs;
//reg [7:0] duan;
reg[7:0] dataout2;
reg[16:0] tenvalue2;
reg[7:0] dataout3;
reg[7:0] dataout4;
reg[7:0] dataout6;
reg[16:0] tenvalue3;
reg[16:0] tenvalue4;
reg[16:0] tenvalue5;
reg[16:0] tenvalue6;
reg[16:0] tenvalue7;
parameter sample=2'b00,
display=2'b01;
parameter sample2=2'b00,
display2=2'b01;
parameter sample3=2'b00,
display3=2'b01;
parameter sample4=2'b00,
display4=2'b01;
parameter sample6=2'b00,
display6=2'b01;
/*****************************************************
*****************车底盘控制AD采集***********************
*****************************************************/
/**********产生100k的采集时钟信号*********/
always@(posedge clk)
begin
if(count<=250)
count<=count+1'b1;
else
begin
count<=0;
adc_sclk<=~adc_sclk;
end
end
/*******产生周期为1ms即1kHz的信号*********/
always@(posedge clk)
begin
if(count1ms>25'd25000)
begin
clk1ms<=~clk1ms;
count1ms<=0;
end
else
count1ms<=count1ms+1;
end
/*********AD采样程序**************/
always@(negedge adc_sclk)
begin
case(state)
sample:
begin
cs<=0;
dataout[7:0]<={dataout[6:0],data};
if(cnt>4'd7)
begin
cnt<=0;
state<=display;
end
else
begin
cnt<=cnt+1;
state<=sample;
end
end
display:
begin
cs<=1;//关AD片选
tenvalue<=(tendata((dataout>>4)&8'b0000_1111)*16+ tendata(dataout&8'b0000_1111))*129;//
//得到采集的数据
state<=sample;
end
default: state<=display;
endcase
end
/***********2进制转十进制函数*************/
function[7:0] tendata;//返回一个4位的数字
input[7:0] datain;
begin
case(datain)
4'b00000000: tendata=4'd0;//0
4'b00000001: tendata=4'd1;//1
4'b00000010: tendata=4'd2;//2
4'b00000011: tendata=4'd3;//3
4'b00000100: tendata=4'd4;//4
4'b00000101: tendata=4'd5;//5
4'b00000110: tendata=4'd6;//6
4'b00000111: tendata=4'd7;//7
4'b00001000: tendata=4'd8;//8
4'b00001001: tendata=4'd9;//9
4'b00001010: tendata=4'd10;//
4'b00001011: tendata=4'd11;//
4'b00001100: tendata=4'd12;
4'b00001101: tendata=4'd13;
4'b00001110: tendata=4'd14;
4'b00001111: tendata=4'd15;
default:tendata=4'bzzzz_zzzz;
endcase
end
endfunction
/*********十进制转LED段选函数*********/
/*function[7:0] leddata;//返回一个8位的数字
input[3:0] datain;
begin
case(datain)
4'd0: leddata=8'b11000000;//0
4'd1: leddata=8'b11111001;//1
4'd2: leddata=8'b10100100;//2
4'd3: leddata=8'b10110000;//3
4'd4: leddata=8'b10011001;//4
4'd5: leddata=8'b10010010;//5
4'd6: leddata=8'b10000010;//6
4'd7: leddata=8'b11111000;//7
4'd8: leddata=8'b10000000;//8
4'd9: leddata=8'b10010000;//9
4'd10: leddata=8'b10111111;//-
4'd11: leddata=8'b01111111;//.
default:leddata=8'bzzzz_zzzz;
endcase
end
endfunction
/********数码管扫描函数*************/
/**********产生100k的采集时钟信号*********/
always@(posedge clk)
begin
if(count2<=250)
count2<=count2+1'b1;
else
begin
count2<=0;
adc_sclk2<=~adc_sclk2;
end
end
/*******产生周期为1ms即1kHz的信号*********/
always@(posedge clk)
begin
if(count1ms2>25'd25000)
begin
clk1ms2<=~clk1ms2;
count1ms2<=0;
end
else
count1ms2<=count1ms2+1;
end
/*********AD采样程序**************/
always@(negedge adc_sclk2)
begin
case(state2)
sample2:
begin
cs2<=0;
dataout2[7:0]<={dataout2[6:0],data2};
if(cnt2>4'd7)
begin
cnt2<=0;
state2<=display2;
end
else
begin
cnt2<=cnt2+1;
state2<=sample2;
end
end
display2:
begin
cs2<=1;//关AD片选
tenvalue2<=(tendata2((dataout2>>4)&8'b0000_1111)*16+ tendata2(dataout2&8'b0000_1111))*129;//
//得到采集的数据
state2<=sample2;
end
default: state2<=display2;
endcase
end
/***********2进制转十进制函数*************/
function[7:0] tendata2;//返回一个4位的数字
input[7:0] datain2;
begin
case(datain2)
4'b00000000: tendata2=4'd0;//0
4'b00000001: tendata2=4'd1;//1
4'b00000010: tendata2=4'd2;//2
4'b00000011: tendata2=4'd3;//3
4'b00000100: tendata2=4'd4;//4
4'b00000101: tendata2=4'd5;//5
4'b00000110: tendata2=4'd6;//6
4'b00000111: tendata2=4'd7;//7
4'b00001000: tendata2=4'd8;//8
4'b00001001: tendata2=4'd9;//9
4'b00001010: tendata2=4'd10;//
4'b00001011: tendata2=4'd11;//
4'b00001100: tendata2=4'd12;
4'b00001101: tendata2=4'd13;
4'b00001110: tendata2=4'd14;
4'b00001111: tendata2=4'd15;
default:tendata2=4'bzzzz_zzzz;
endcase
end
endfunction
/*always@(posedge clk1ms)
begin
if(number==5)
number<=0;
else
begin
number<=number+1;
case(number)
4'd0:
begin
duan<=leddata((tenvalue/10)%10);//个位
wei<=4'b1110;
end
4'd1:
begin
duan<=leddata((tenvalue/100)%10);//十位
wei<=4'b1101;
end
4'd2:
begin
duan<=leddata((tenvalue/1000)%10); //百位
wei<=4'b1011;
end
4'd3:
begin
duan<=leddata(tenvalue/10000);//千位
wei<=4'b0111;
end
4'd4:
begin
duan<=leddata(4'd11);//. 显示小数点
wei<=4'b0111;
end
endcase
end
end */
/*****************************************************
*****************发射装置电机AD采集*********************
*****************************************************/
/**********产生100k的采集时钟信号*********/
always@(posedge clk)
begin
if(count3<=250)
count3<=count3+1'b1;
else
begin
count3<=0;
adc_sclk3<=~adc_sclk3;
end
end
/*******产生周期为1ms即1kHz的信号*********/
always@(posedge clk)
begin
if(count1ms3>25'd25000)
begin
clk1ms3<=~clk1ms3;
count1ms3<=0;
end
else
count1ms3<=count1ms3+1;
end
/*********AD采样程序**************/
always@(negedge adc_sclk3)
begin
case(state3)
sample3:
begin
cs3<=0;
dataout3[7:0]<={dataout3[6:0],data3};
if(cnt3>4'd7)
begin
cnt3<=0;
state3<=display3;
end
else
begin
cnt3<=cnt3+1;
state3<=sample3;
end
end
display3:
begin
cs3<=1;//关AD片选
tenvalue3<=(tendata3((dataout3>>4)&8'b0000_1111)*16+ tendata3(dataout3&8'b0000_1111))*129;//
//得到采集的数据
state3<=sample3;
end
default: state3<=display3;
endcase
end
/***********2进制转十进制函数*************/
function[7:0] tendata3;//返回一个4位的数字
input[7:0] datain3;
begin
case(datain3)
4'b00000000: tendata3=4'd0;//0
4'b00000001: tendata3=4'd1;//1
4'b00000010: tendata3=4'd2;//2
4'b00000011: tendata3=4'd3;//3
4'b00000100: tendata3=4'd4;//4
4'b00000101: tendata3=4'd5;//5
4'b00000110: tendata3=4'd6;//6
4'b00000111: tendata3=4'd7;//7
4'b00001000: tendata3=4'd8;//8
4'b00001001: tendata3=4'd9;//9
4'b00001010: tendata3=4'd10;//
4'b00001011: tendata3=4'd11;//
4'b00001100: tendata3=4'd12;
4'b00001101: tendata3=4'd13;
4'b00001110: tendata3=4'd14;
4'b00001111: tendata3=4'd15;
default:tendata3=4'bzzzz_zzzz;
endcase
end
endfunction
/*****************************************************
*****************撑杆控制AD采集************************
*****************************************************/
/**********产生100k的采集时钟信号*********/
always@(posedge clk)
begin
if(count5<=250)
count5<=count5+1'b1;
else
begin
count5<=0;
adc_sclk4<=~adc_sclk4;
end
end
/*******产生周期为1ms即1kHz的信号*********/
always@(posedge clk)
begin
if(count1ms4>25'd25000)
begin
clk1ms4<=~clk1ms4;
count1ms4<=0;
end
else
count1ms4<=count1ms4+1;
end
/*********AD采样程序**************/
always@(negedge adc_sclk4)
begin
case(state4)
sample4:
begin
cs4<=0;
dataout4[7:0]<={dataout4[6:0],data4};
if(cnt4>4'd7)
begin
cnt4<=0;
state4<=display4;
end
else
begin
cnt4<=cnt4+1;
state4<=sample4;
end
end
display4:
begin
cs4<=1;//关AD片选
tenvalue4<=(tendata4((dataout4>>4)&8'b0000_1111)*16+ tendata4(dataout4&8'b0000_1111))*129;//
//得到采集的数据
state4<=sample4;
end
default: state4<=display4;
endcase
end
/***********2进制转十进制函数*************/
function[7:0] tendata4;//返回一个4位的数字
input[7:0] datain4;
begin
case(datain4)
4'b00000000: tendata4=4'd0;//0
4'b00000001: tendata4=4'd1;//1
4'b00000010: tendata4=4'd2;//2
4'b00000011: tendata4=4'd3;//3
4'b00000100: tendata4=4'd4;//4
4'b00000101: tendata4=4'd5;//5
4'b00000110: tendata4=4'd6;//6
4'b00000111: tendata4=4'd7;//7
4'b00001000: tendata4=4'd8;//8
4'b00001001: tendata4=4'd9;//9
4'b00001010: tendata4=4'd10;//
4'b00001011: tendata4=4'd11;//
4'b00001100: tendata4=4'd12;
4'b00001101: tendata4=4'd13;
4'b00001110: tendata4=4'd14;
4'b00001111: tendata4=4'd15;
default:tendata4=4'bzzzz_zzzz;
endcase
end
endfunction
/*****************************************************
*****************转台控制AD采集************************
*****************************************************/
/**********产生100k的采集时钟信号*********/
always@(posedge clk)
begin
if(count7<=250)
count7<=count7+1'b1;
else
begin
count7<=0;
adc_sclk6<=~adc_sclk6;
end
end
/*******产生周期为1ms即1kHz的信号*********/
always@(posedge clk)
begin
if(count1ms6>25'd25000)
begin
clk1ms6<=~clk1ms6;
count1ms6<=0;
end
else
count1ms6<=count1ms6+1;
end
/*********AD采样程序**************/
always@(negedge adc_sclk6)
begin
case(state6)
sample6:
begin
cs6<=0;
dataout6[7:0]<={dataout6[6:0],data_};
if(cnt6>4'd7)
begin
cnt6<=0;
state6<=display6;
end
else
begin
cnt6<=cnt6+1;
state6<=sample6;
end
end
display6:
begin
cs6<=1;//关AD片选
tenvalue6<=(tendata6((dataout6>>4)&8'b0000_1111)*16+ tendata6(dataout6&8'b0000_1111))*129;//
//得到采集的数据
state6<=sample6;
end
default: state6<=display6;
endcase
end
/***********2进制转十进制函数*************/
function[7:0] tendata6;//返回一个4位的数字
input[7:0] datain6;
begin
case(datain6)
4'b00000000: tendata6=4'd0;//0
4'b00000001: tendata6=4'd1;//1
4'b00000010: tendata6=4'd2;//2
4'b00000011: tendata6=4'd3;//3
4'b00000100: tendata6=4'd4;//4
4'b00000101: tendata6=4'd5;//5
4'b00000110: tendata6=4'd6;//6
4'b00000111: tendata6=4'd7;//7
4'b00001000: tendata6=4'd8;//8
4'b00001001: tendata6=4'd9;//9
4'b00001010: tendata6=4'd10;//
4'b00001011: tendata6=4'd11;//
4'b00001100: tendata6=4'd12;
4'b00001101: tendata6=4'd13;
4'b00001110: tendata6=4'd14;
4'b00001111: tendata6=4'd15;
default:tendata6=4'bzzzz_zzzz;
endcase
end
endfunction
///////撑杆计算中值//////
always @(posedge clk)
begin
if(tenvalue4>16750)
begin
tenvalue5=3*(tenvalue4-16750);
chenggan=2'b01;
end
else
begin
tenvalue5=3*(16750-tenvalue4);
chenggan=2'b10;
end
end
///////////////////////////////转台控制/////////////////////////////
always @(posedge clk)
begin
if(tenvalue6>20000)
begin
tenvalue7=(15000*5-(tenvalue6-18000)*5);
zhuantai=2'b01;
end
if(tenvalue6<17000)
begin
tenvalue7=(17900*4-(17000-tenvalue6)*5);
zhuantai=2'b10;
end
if(20000>tenvalue6&&tenvalue6>17000)
begin
tenvalue7<=0;
//chenggan =2'b10;
end
end
always@(posedge clk)
begin
if(tenvalue7==0)
flag_pwm=1'b0;
else
begin
if(coun<=tenvalue7)
coun<=coun+1'b1;
else
begin
coun<=0;
flag_pwm<=~flag_pwm;
end
end
end
///////////////////////////////撑杆///////////////////////////////
always @(posedge clk)
begin
pwm_count4<=tenvalue5 [16:5] ;
countchenggan=countchenggan+1'b1;
if (countchenggan[13:4] < pwm_count4)
pwm_flag4=1'b1;
else
pwm_flag4=1'b0;
end
//////pwm发射电机////////
always @(posedge clk)
begin
pwm_count2<=tenvalue3[16:5] ;
count4=count4+1'b1;
if (count4[13:4] < pwm_count2)
pwm_flag2=2'b11;
else
pwm_flag2=2'b00;
end
//////////计算中值////////
always @(posedge clk)
begin
if(tenvalue>16750)
data5=3*(tenvalue-16750);
else
data6=3*(16750-tenvalue);
if(tenvalue2>14800)
data7=3*(tenvalue2-14800);
else
data8=3*(14800-tenvalue2);
end
//////////轮子控制代码///////
always @(posedge clk)
begin
if(data5>data6&&data5>data7&&data5>data8)
begin
tenvalue1<=data5;
M=8'b01010101;
end
if(data6>data5&&data6>data7&&data6>data8)
begin
tenvalue1<=data6;
M=8'b10101010;
end
if(data7>data5&&data7>data6&&data7>data8)
begin
tenvalue1<=data7;
if(zhuanwan==1)
M=8'b01101001;
else
M=8'b10011001;
end
if(data8>data5&&data8>data6&&data8>data7)
begin
tenvalue1<=data8;
if(zhuanwan==1)
M=8'b10010110;
else
M=8'b01100110;
end
pwm_count<=tenvalue1[16:5] ;
count1=count1+1'b1;
if (count1[13:4] < pwm_count)
pwm_flag=4'b1111;
else
pwm_flag=4'b0000;
end
assign led=pwm_flag;
assign fashe=pwm_flag2;
assign chenggan_pwm=pwm_flag4;
assign zhuantai_pwm=flag_pwm;
endmodule

复制代码