本帖最后由 顺子 于 2015-11-6 21:50 编辑
xiancun010 发表于 2015-10-30 20:10 
顶一个。贴出代码吧。让我们也学习学习。
//设置接收dma接口的模拟数据(自动接收)
__IO uint16_t ADC_ConvertedValue[2];
首先设置pwm三个舵机接口
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；
|
发表于 2015-10-28 15:50:05
楼主