小弟自己業余愛好,閑時做了一個舵機控制裝置擺弄一下,利用三角函數還算是可以用搖桿隨意控制方向。有待改進,請多包涵,應大家需求我也可以貼出代碼!謝謝!
//設置接收dma接口的模擬數據(自動接收)
__IO uint16_t ADC_ConvertedValue[2];
首先設置pwm三個舵機接口
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; //復用推挽輸出
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
- GPIO_Init(GPIOA, &GPIO_InitStructure);
-
- TIM_TimeBaseStructure.TIM_Period = arr; //設置在下一個更新事件裝入活動的自動重裝載寄存器周期的值 80K
- TIM_TimeBaseStructure.TIM_Prescaler = psc; //設置用來作為TIMx時鐘頻率除數的預分頻值 不分頻
- TIM_TimeBaseStructure.TIM_ClockDivision = 0; //設置時鐘分割:TDTS = Tck_tim
- TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; //TIM向上計數模式
- TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure); //根據TIM_TimeBaseInitStruct中指定的參數初始化TIMx的時間基數單位
- TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; //選擇定時器模式:TIM脈沖寬度調制模式2
- TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; //比較輸出使能
- TIM_OCInitStructure.TIM_Pulse = 0; //設置待裝入捕獲比較寄存器的脈沖值
- TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; //輸出極性:TIM輸出比較極性高
- TIM_OC2Init(TIM2, &TIM_OCInitStructure);
- TIM_OC3Init(TIM2, &TIM_OCInitStructure);
- TIM_OC4Init(TIM2, &TIM_OCInitStructure); //根據TIM_OCInitStruct中指定的參數初始化外設TIMx
- //TIM_CtrlPWMOutputs(TIM1,ENABLE); //MOE 主輸出使能
- TIM_OC2PreloadConfig(TIM2, TIM_OCPreload_Enable); //CH3預裝載使能
- TIM_OC3PreloadConfig(TIM2, TIM_OCPreload_Enable); //CH3預裝載使能
- TIM_OC4PreloadConfig(TIM2, TIM_OCPreload_Enable); //CH3預裝載使能
-
- TIM_ARRPreloadConfig(TIM2, ENABLE); //使能TIMx在ARR上的預裝載寄存器
-
- 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的度數,參數依次為bca,
- 如果你想知道B的度數,參數依次為acb,
- 如果你想知道C的度數,參數依次為abc,
- */
- float Angle(float a,float b,float c)//求任意三角形的每個角的角度
- {
- 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);//開啟IO口A時鐘
- 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; //開10號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; //開啟0,1號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;//多通道循環掃描
- 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);//這句意思是把系統時間分為八分之一
- 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);//復位
- while(ADC_GetResetCalibrationStatus(ADC1));//判斷復位是否完成
- ADC_StartCalibration(ADC1);
- while(ADC_GetCalibrationStatus(ADC1));
- ADC_SoftwareStartConvCmd(ADC1,ENABLE);//執行轉換功能
- }
- 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;//臀部舵機的角度 左邊
- u16 LD_L2Angle;//膝蓋舵機的角度 左邊
- u16 LD_L3Angle;//腳踝舵機的角度 左邊
- float LD_H;//機器人從臀部舵機到腳踝舵機的垂直高度
- float LD_L1H;//機器人從臀部舵機到膝蓋舵機的距離 左邊
- float LD_L2H;//機器人從膝蓋舵機到腳踝舵機的距離 左邊
- float LD_LH;//機器人臀部到腳踝的距離 左邊
- float LD_LX;//機器人前后移動的距離,變量
- 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)//左腿運動
- {
- float CAB,ABC,BCA,CAD,ACD;//設置角度參數
- //下面開始計算所有要用的角度
- LD_LH=Right_Angle_Hypotenuse(LD_H,fabs(LD_LX));//計算斜邊
- 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;
- }
- 舵機1是最上面的接PA1;
- 舵機2是最上面的接PA2;
- 舵機3是最上面的接PA3;
- 搖桿的X方向為PB0;
- 搖桿的Y方向為PB1;
復制代碼
51單片機源碼:
//機器人琳達的行動代碼編寫
- #include <reg52.h>
- #include <intrins.h>
- #include <math.h>
- #include <float.h>
- #define FOSC 11059200L //11.0592M Hz
- #define TIME_US 100 //設定定時時間
- #define uchar unsigned char
- #define uint unsigned int
- P0M0=0xff;
- P0M1=0x00;//強推
- 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;//舵機1
- sbit D2=P0^0;//舵機2
- sbit D3=P0^1;//舵機3
- sbit led=P1^0;
- sbit led1=P1^1;
- //機器人叫 琳達 簡稱LD
- uint count;//舵機變量
- #define PI 3.141593 // 設定圓周率
- float LD_H;//機器人從臀部舵機到腳踝舵機的垂直高度
- float LD_LX;//機器人前后移動的距離,變量
- float LD_L1H;//機器人從臀部舵機到膝蓋舵機的距離 左邊
- float LD_L2H;//機器人從膝蓋舵機到腳踝舵機的距離 左邊
- uint LD_L1Angle;//臀部舵機的角度 左邊
- uint LD_L2Angle;//膝蓋舵機的角度 左邊
- uint LD_L3Angle;//腳踝舵機的角度 左邊
- float LD_LH;//機器人臀部到腳踝的距離 左邊
- float in;
- uint y=0;
- void init()
- {
- /*
- TMOD=0x01; //設定定時器0的工作方式為1
- TH0=(65536-(FOSC/12*TIME_US)/1000000)/256;
- TL0=(65536-(FOSC/12*TIME_US)/1000000)%256;
- ET0=1; //開啟定時器0中斷
- TR0=1; //開啟定時器
- EA=1; //打開總中斷
- */
- 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的度數,參數依次為bca,
- 如果你想知道B的度數,參數依次為acb,
- 如果你想知道C的度數,參數依次為abc,
- */
- float Angle(float a,float b,float c)//求任意三角形的每個角的角度
- {
- 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()//左腿運動
- {
- float CAB,ABC,BCA,CAD,ACD;//設置角度參數
- //下面開始計算所有要用的角度
- LD_LH=Right_Angle_Hypotenuse(LD_H,abs(LD_LX));//計算斜邊
- 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
- {
-
-
-
- }
復制代碼
|