|
小弟自己業(yè)余愛(ài)好,閑時(shí)做了一個(gè)舵機(jī)控制裝置擺弄一下,利用三角函數(shù)還算是可以用搖桿隨意控制方向。有待改進(jìn),請(qǐng)多包涵,應(yīng)大家需求我也可以貼出代碼!謝謝!
//設(shè)置接收dma接口的模擬數(shù)據(jù)(自動(dòng)接收)
__IO uint16_t ADC_ConvertedValue[2];
首先設(shè)置pwm三個(gè)舵機(jī)接口
stm32源碼:
- void TIM3_PWM_Init(u16 arr,u16 psc)
- {
- GPIO_InitTypeDef GPIO_InitStructure;
- TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
- TIM_OCInitTypeDef TIM_OCInitStructure;
-
- RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);//
- RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA , ENABLE);
-
- GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1|GPIO_Pin_2|GPIO_Pin_3; //TIM_CH1
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; //復(fù)用推挽輸出
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
- GPIO_Init(GPIOA, &GPIO_InitStructure);
-
- TIM_TimeBaseStructure.TIM_Period = arr; //設(shè)置在下一個(gè)更新事件裝入活動(dòng)的自動(dòng)重裝載寄存器周期的值 80K
- TIM_TimeBaseStructure.TIM_Prescaler = psc; //設(shè)置用來(lái)作為TIMx時(shí)鐘頻率除數(shù)的預(yù)分頻值 不分頻
- TIM_TimeBaseStructure.TIM_ClockDivision = 0; //設(shè)置時(shí)鐘分割:TDTS = Tck_tim
- TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; //TIM向上計(jì)數(shù)模式
- TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure); //根據(jù)TIM_TimeBaseInitStruct中指定的參數(shù)初始化TIMx的時(shí)間基數(shù)單位
- TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; //選擇定時(shí)器模式:TIM脈沖寬度調(diào)制模式2
- TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; //比較輸出使能
- TIM_OCInitStructure.TIM_Pulse = 0; //設(shè)置待裝入捕獲比較寄存器的脈沖值
- TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; //輸出極性:TIM輸出比較極性高
- TIM_OC2Init(TIM2, &TIM_OCInitStructure);
- TIM_OC3Init(TIM2, &TIM_OCInitStructure);
- TIM_OC4Init(TIM2, &TIM_OCInitStructure); //根據(jù)TIM_OCInitStruct中指定的參數(shù)初始化外設(shè)TIMx
- //TIM_CtrlPWMOutputs(TIM1,ENABLE); //MOE 主輸出使能
- TIM_OC2PreloadConfig(TIM2, TIM_OCPreload_Enable); //CH3預(yù)裝載使能
- TIM_OC3PreloadConfig(TIM2, TIM_OCPreload_Enable); //CH3預(yù)裝載使能
- TIM_OC4PreloadConfig(TIM2, TIM_OCPreload_Enable); //CH3預(yù)裝載使能
-
- TIM_ARRPreloadConfig(TIM2, ENABLE); //使能TIMx在ARR上的預(yù)裝載寄存器
-
- TIM_Cmd(TIM2, ENABLE); //使能TIM1
- }
- float Right_Angle_Hypotenuse(float H,float X)//求直角斜邊的距離
- {
- return sqrt(pow(H,2)+pow(X,2));
- }
- float Right_Angle(float H,float X)//求直角三角形的角度
- {
- return atan(X/H)*180/PI;
- }
- /*三角形ABC角度分別為A,B,C,邊分別為AB=c,BC=a,CA=b;
- 如果你想知道A的度數(shù),參數(shù)依次為bca,
- 如果你想知道B的度數(shù),參數(shù)依次為acb,
- 如果你想知道C的度數(shù),參數(shù)依次為abc,
- */
- float Angle(float a,float b,float c)//求任意三角形的每個(gè)角的角度
- {
- float angle;
- angle=acos((pow(a,2)+pow(b,2)-pow(c,2))/(2*a*b));
- angle=angle*180/PI;
- if(angle<0)angle=180-fabs(angle);
- return angle;
- }
- void YaoGan_XYD_init(void)
- {
- GPIO_InitTypeDef HS_Gpio;
- ADC_InitTypeDef HS_ADC;
- DMA_InitTypeDef HS_DMA;
-
- RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);//開(kāi)啟IO口A時(shí)鐘
- RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE);
- RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
-
- DMA_DeInit(DMA1_Channel1);
- HS_DMA.DMA_PeripheralBaseAddr = (u32)&ADC1->DR;//((u32)0x40012400+0x4c);
- HS_DMA.DMA_MemoryBaseAddr=(u32)&ADC_ConvertedValue;
- HS_DMA.DMA_DIR=DMA_DIR_PeripheralSRC;
- HS_DMA.DMA_BufferSize=2;
- HS_DMA.DMA_PeripheralInc=DMA_PeripheralInc_Disable;
- HS_DMA.DMA_MemoryInc=DMA_MemoryInc_Enable;
- HS_DMA.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord;
- HS_DMA.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord;
- HS_DMA.DMA_Mode = DMA_Mode_Circular;
- HS_DMA.DMA_Priority = DMA_Priority_High;
- HS_DMA.DMA_M2M = DMA_M2M_Disable;
- DMA_Init(DMA1_Channel1, &HS_DMA);
- DMA_Cmd(DMA1_Channel1, ENABLE);
-
-
- HS_Gpio.GPIO_Pin=GPIO_Pin_10; //開(kāi)10號(hào)Io口
- HS_Gpio.GPIO_Mode=GPIO_Mode_IPU; //上拉輸入
- HS_Gpio.GPIO_Speed=GPIO_Speed_50MHz;
- GPIO_Init(GPIOB,&HS_Gpio); //初始化IO口
-
- HS_Gpio.GPIO_Pin=GPIO_Pin_0|GPIO_Pin_1; //開(kāi)啟0,1號(hào)Io口
- HS_Gpio.GPIO_Mode=GPIO_Mode_AIN; //
- HS_Gpio.GPIO_Speed=GPIO_Speed_50MHz;
- GPIO_Init(GPIOB,&HS_Gpio); //初始化IO口
-
- HS_ADC.ADC_Mode = ADC_Mode_Independent;
- HS_ADC.ADC_ScanConvMode = ENABLE;//多通道循環(huán)掃描
- HS_ADC.ADC_ContinuousConvMode = ENABLE;
- HS_ADC.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None;
- HS_ADC.ADC_DataAlign = ADC_DataAlign_Right;
- HS_ADC.ADC_NbrOfChannel = 2;
- ADC_Init(ADC1,&HS_ADC);
- // RCC_ADCCLKConfig(RCC_PCLK2_Div8);//這句意思是把系統(tǒng)時(shí)間分為八分之一
- ADC_RegularChannelConfig(ADC1,ADC_Channel_8,1,ADC_SampleTime_1Cycles5);
- ADC_RegularChannelConfig(ADC1,ADC_Channel_9,2,ADC_SampleTime_1Cycles5);
- ADC_DMACmd(ADC1, ENABLE);
- ADC_Cmd(ADC1,ENABLE);//ADC1使能
- ADC_ResetCalibration(ADC1);//復(fù)位
- while(ADC_GetResetCalibrationStatus(ADC1));//判斷復(fù)位是否完成
- ADC_StartCalibration(ADC1);
- while(ADC_GetCalibrationStatus(ADC1));
- ADC_SoftwareStartConvCmd(ADC1,ENABLE);//執(zhí)行轉(zhuǎn)換功能
- }
- u8 Key(void)//返回按鍵值
- {
- return GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_10);
- }
- u16 ADC1_DMA1_2(u8 x)//
- {
- return ADC_ConvertedValue[x];
- }
- u16 AngleMinMax(u16 angle)
- {
- if(angle<ANGLEMIN){angle=ANGLEMIN;}
- else if(angle>ANGLEMAX){angle=ANGLEMAX;}
- return angle;
- }
- //主程序
- #include "stm32f10x.h"
- #include "init.h"
- u16 LD_L1Angle;//臀部舵機(jī)的角度 左邊
- u16 LD_L2Angle;//膝蓋舵機(jī)的角度 左邊
- u16 LD_L3Angle;//腳踝舵機(jī)的角度 左邊
- float LD_H;//機(jī)器人從臀部舵機(jī)到腳踝舵機(jī)的垂直高度
- float LD_L1H;//機(jī)器人從臀部舵機(jī)到膝蓋舵機(jī)的距離 左邊
- float LD_L2H;//機(jī)器人從膝蓋舵機(jī)到腳踝舵機(jī)的距離 左邊
- float LD_LH;//機(jī)器人臀部到腳踝的距離 左邊
- float LD_LX;//機(jī)器人前后移動(dòng)的距離,變量
- u16 YaoGanX;
- u16 YaoGanY;
- #define YGMIN 1750
- #define YGMAX 2250
- void init()
- {
- LD_H=80;//mm
- LD_L1H=77;
- LD_L2H=80;
- LD_LX=0.0;
-
- }
- int main(void)
- {
- init();
- TIM3_PWM_Init(200,3600);//不分頻。PWM頻率=72000/(899+1)=80Khz
- time_ms(10);
- YaoGan_XYD_init();
- time_ms(10);
- while (1)
- {
- YaoGanX=ADC1_DMA1_2(1);
- YaoGanY=ADC1_DMA1_2(0);
- if(Key()==0)
- {
- time_ms(10);
- if(Key()==0)
- {
- LD_H=80;//mm
- LD_LX=0.0;
- }
- }
- if(YaoGanX<YGMIN)
- {
- LD_LX+=fabs(YaoGanX-YGMIN)/2000.0;
- }else if(YaoGanX>YGMAX)
- {
- LD_LX-=fabs(YaoGanX-YGMAX)/2000.0;
- }
- if(YaoGanY<YGMIN)
- {
- LD_H-=fabs(YaoGanY-YGMIN)/2000.0;
- }else if(YaoGanY>YGMAX)
- {
- LD_H+=fabs(YaoGanY-YGMAX)/2000.0;
- }
- if(LD_H<25){LD_H=25;}if(LD_H>120){LD_H=120;}
- if(LD_LX<-90){LD_LX=-90;}if(LD_LX>90){LD_LX=90;}
- Left_leg_Movement();
-
- TIM_SetCompare2(TIM2,LD_L1Angle);
- TIM_SetCompare3(TIM2,LD_L2Angle);
- TIM_SetCompare4(TIM2,LD_L3Angle);
- time_ms(10);
- }
-
- }
- float Left_leg_Movement(void)//左腿運(yùn)動(dòng)
- {
- float CAB,ABC,BCA,CAD,ACD;//設(shè)置角度參數(shù)
- //下面開(kāi)始計(jì)算所有要用的角度
- LD_LH=Right_Angle_Hypotenuse(LD_H,fabs(LD_LX));//計(jì)算斜邊
- CAB=Angle(LD_L1H,LD_LH,LD_L2H);
- ABC=Angle(LD_L1H,LD_L2H,LD_LH);
- BCA=Angle(LD_LH,LD_L2H,LD_L1H);
- CAD=Right_Angle(LD_H,fabs(LD_LX));
- ACD=Right_Angle(fabs(LD_LX),LD_H);
-
- if(LD_LX<0)
- {
- LD_L1Angle=(u16)((90-CAD-CAB)+80)/4;
- LD_L3Angle=(u16)((BCA+ACD)-30)/4;
- }
- if(LD_LX==0)
- {
- LD_L1Angle=(u16)((90-CAB)+80)/4;
- LD_L3Angle=(u16)((BCA+90)-30)/4;
- }
- if(LD_LX>0)
- {
- LD_L1Angle=(u16)((90-(CAB-CAD)+80))/4;
- LD_L3Angle=(u16)((180-ACD+BCA)-30)/4;
- }
- LD_L2Angle=(u16)((180-ABC)+45)/4;
-
-
- LD_L1Angle=AngleMinMax(LD_L1Angle);
- LD_L2Angle=AngleMinMax(LD_L2Angle);
- LD_L3Angle=AngleMinMax(LD_L3Angle);
- return 1;
- }
- 舵機(jī)1是最上面的接PA1;
- 舵機(jī)2是最上面的接PA2;
- 舵機(jī)3是最上面的接PA3;
- 搖桿的X方向?yàn)镻B0;
- 搖桿的Y方向?yàn)镻B1;
復(fù)制代碼
51單片機(jī)源碼:
//機(jī)器人琳達(dá)的行動(dòng)代碼編寫
- #include <reg52.h>
- #include <intrins.h>
- #include <math.h>
- #include <float.h>
- #define FOSC 11059200L //11.0592M Hz
- #define TIME_US 100 //設(shè)定定時(shí)時(shí)間
- #define uchar unsigned char
- #define uint unsigned int
- P0M0=0xff;
- P0M1=0x00;//強(qiáng)推
- uchar serval[2];
- uint pwm[]={1382,1382,1382};
- uchar pwm_flag=0;
- uint ms0_5con=461;//0.5
- uint ms2_5con=2304;//2.5
- sbit D1=P0^7;//舵機(jī)1
- sbit D2=P0^0;//舵機(jī)2
- sbit D3=P0^1;//舵機(jī)3
- sbit led=P1^0;
- sbit led1=P1^1;
- //機(jī)器人叫 琳達(dá) 簡(jiǎn)稱LD
- uint count;//舵機(jī)變量
- #define PI 3.141593 // 設(shè)定圓周率
- float LD_H;//機(jī)器人從臀部舵機(jī)到腳踝舵機(jī)的垂直高度
- float LD_LX;//機(jī)器人前后移動(dòng)的距離,變量
- float LD_L1H;//機(jī)器人從臀部舵機(jī)到膝蓋舵機(jī)的距離 左邊
- float LD_L2H;//機(jī)器人從膝蓋舵機(jī)到腳踝舵機(jī)的距離 左邊
- uint LD_L1Angle;//臀部舵機(jī)的角度 左邊
- uint LD_L2Angle;//膝蓋舵機(jī)的角度 左邊
- uint LD_L3Angle;//腳踝舵機(jī)的角度 左邊
- float LD_LH;//機(jī)器人臀部到腳踝的距離 左邊
- float in;
- uint y=0;
- void init()
- {
- /*
- TMOD=0x01; //設(shè)定定時(shí)器0的工作方式為1
- TH0=(65536-(FOSC/12*TIME_US)/1000000)/256;
- TL0=(65536-(FOSC/12*TIME_US)/1000000)%256;
- ET0=1; //開(kāi)啟定時(shí)器0中斷
- TR0=1; //開(kāi)啟定時(shí)器
- EA=1; //打開(kāi)總中斷
- */
- TMOD|=0x20;
- TH1=0xfd;
- TL1=0xfd;
- TR1=1;
- REN=1;
- SM0=0;
- SM1=1;
- EA=1;
- ES=1;
-
- TMOD|=0x01;
- TH0=-ms2_5con>>8;
- TL0=-ms2_5con;
- EA=1;
- ET0=1;
- TR0=1;
-
-
- LD_H=120;//mm
- LD_L1H=77;
- LD_L2H=80;
- in=-1.0;
- LD_LX=0.0;
- }
- void time(unsigned int ms)
- {
- uint i,j;
- for(i=0;i<ms;i++)
- #if FOSC == 11059200L
- for(j=0;j<114;j++);
- #elif FOSC == 12000000L
- for(j=0;j<123;j++);
- #elif FOSC == 24000000L
- for(j=0;j<249;j++);
- #else
- for(j=0;j<114;j++);
- #endif
- }
- void main()
- {
- uint x=0;
- init();
- while(1)
- {
- switch(x)
- {
- case 0:
- LD_LX+=in;
- if(LD_LX<-80.0){x=1;}
- if(LD_LX>50.0){x=1;}
- led=0;
- led1=1;
- break;
- case 1:
- LD_H+=in;
- if(LD_H<90.0){in=1.0;x=0;}
- if(LD_H>120.0){in=-1.0;x=0;}
- led=1;
- led1=0;
- break;
- }
- Left_Leg_Movement();
-
- }
- }
- float Right_Angle_Hypotenuse(float H,float X)//求直角斜邊的距離
- {
- return sqrt(pow(H,2)+pow(X,2));
- }
- float Right_Angle(float H,float X)//求直角三角形的角度
- {
- return atan(X/H)*180/PI;
- }
- /*三角形ABC角度分別為A,B,C,邊分別為AB=c,BC=a,CA=b;
- 如果你想知道A的度數(shù),參數(shù)依次為bca,
- 如果你想知道B的度數(shù),參數(shù)依次為acb,
- 如果你想知道C的度數(shù),參數(shù)依次為abc,
- */
- float Angle(float a,float b,float c)//求任意三角形的每個(gè)角的角度
- {
- float angle;
- angle=acos((pow(a,2)+pow(b,2)-pow(c,2))/(2*a*b));
- angle=angle*180/PI;
- if(angle<0)angle=180-abs(angle);
- return angle;
- }
- void Left_leg_Movement()//左腿運(yùn)動(dòng)
- {
- float CAB,ABC,BCA,CAD,ACD;//設(shè)置角度參數(shù)
- //下面開(kāi)始計(jì)算所有要用的角度
- LD_LH=Right_Angle_Hypotenuse(LD_H,abs(LD_LX));//計(jì)算斜邊
- CAB=Angle(LD_L1H,LD_LH,LD_L2H);
- ABC=Angle(LD_L1H,LD_L2H,LD_LH);
- BCA=Angle(LD_LH,LD_L2H,LD_L1H);
- CAD=Right_Angle(LD_H,abs(LD_LX));
- ACD=Right_Angle(abs(LD_LX),LD_H);
-
- if(LD_LX<=0)
- {
- LD_L1Angle=(int)(180-CAD-CAB)/7;
- LD_L3Angle=(int)(BCA+ACD)/7-6;
- }
- if(LD_LX>0)
- {
- LD_L1Angle=(int)(180+CAD-CAB)/7;
- LD_L3Angle=(int)(180-ACD+BCA)/7-6;
- }
- LD_L2Angle=(int)(180-ABC)/7;
-
-
- }
- void Timer0Int() interrupt 1
- {
- /*
- TH0=(65536-(FOSC/12*TIME_US)/1000000)/256;
- TL0=(65536-(FOSC/12*TIME_US)/1000000)%256;
- */
-
- count++;
- if(count > 200)
- {
- count=0;
- }
- if(LD_L1Angle<7)LD_L1Angle=7;if(LD_L1Angle>20)LD_L1Angle=20;
- if(LD_L2Angle<7)LD_L2Angle=7;if(LD_L2Angle>20)LD_L2Angle=20;
- if(LD_L3Angle<7)LD_L3Angle=7;if(LD_L3Angle>20)LD_L3Angle=20;
-
- /*
-
- if(count<LD_L1Angle){D1=1;}else{D1=0;}
- if(count<LD_L2Angle){D2=1;}else{D2=0;}
- if(count<LD_L3Angle){D3=1;}else{D3=0;}
-
- */
- switch(pwm_flag)
- {
- case 1:
- D1=1;
- TH0=-pwm[0]>>8;
- TL0=-pwm[0];
- break;
- case 2:
- D1=0;
- TH0=-(ms2_5con-pwm[0])>>8;
- TL0=-(ms2_5con-pwm[0]);
- pwm_flag=0;
- break;
- case 3:
- TH0=0xff;
- TL0=0x80;
- pwm_flag=0;
- break;
- }
- pwm_flag++;
-
-
- switch(y)
- {
- case 0:
- pwm[0]+=10;
- if(pwm[0]>2000)
- {
- y=1;
- }
- break;
- case 1:
- pwm[0]-=10;
- if(pwm[0]<500)
- {
- y=0;
- }
- break;
- }
- }
- void ser() interrupt 4
- {
-
-
-
- }
復(fù)制代碼
|
|