#include "sys.h"
void WFI_SET(void)
{
	__ASM volatile("wfi");		  
}
//关闭所有中断
void INTX_DISABLE(void)
{		  
	__ASM volatile("cpsid i");
}
//开启所有中断
void INTX_ENABLE(void)
{
	__ASM volatile("cpsie i");		  
}
//设置栈顶地址
//addr:栈顶地址
__asm void MSR_MSP(u32 addr) 
{
    MSR MSP, r0 			//set Main Stack value
    BX r14
}
#ifndef __SYS_H
#define __SYS_H	
#include "stm32f10x.h"
//0,不支持ucos
//1,支持ucos
#define SYSTEM_SUPPORT_OS		0		//定义系统文件夹是否支持UCOS
																	    
	 
//位带操作,实现51类似的GPIO控制功能
//具体实现思想,参考<<CM3权威指南>>第五章(87页~92页).
//IO口操作宏定义
#define BITBAND(addr, bitnum) ((addr & 0xF0000000)+0x2000000+((addr &0xFFFFF)<<5)+(bitnum<<2)) 
#define MEM_ADDR(addr)  *((volatile unsigned long  *)(addr)) 
#define BIT_ADDR(addr, bitnum)   MEM_ADDR(BITBAND(addr, bitnum)) 
//IO口地址映射
#define GPIOA_ODR_Addr    (GPIOA_BASE+12) //0x4001080C 
#define GPIOB_ODR_Addr    (GPIOB_BASE+12) //0x40010C0C 
#define GPIOC_ODR_Addr    (GPIOC_BASE+12) //0x4001100C 
#define GPIOD_ODR_Addr    (GPIOD_BASE+12) //0x4001140C 
#define GPIOE_ODR_Addr    (GPIOE_BASE+12) //0x4001180C 
#define GPIOF_ODR_Addr    (GPIOF_BASE+12) //0x40011A0C    
#define GPIOG_ODR_Addr    (GPIOG_BASE+12) //0x40011E0C    
#define GPIOA_IDR_Addr    (GPIOA_BASE+8) //0x40010808 
#define GPIOB_IDR_Addr    (GPIOB_BASE+8) //0x40010C08 
#define GPIOC_IDR_Addr    (GPIOC_BASE+8) //0x40011008 
#define GPIOD_IDR_Addr    (GPIOD_BASE+8) //0x40011408 
#define GPIOE_IDR_Addr    (GPIOE_BASE+8) //0x40011808 
#define GPIOF_IDR_Addr    (GPIOF_BASE+8) //0x40011A08 
#define GPIOG_IDR_Addr    (GPIOG_BASE+8) //0x40011E08 
 
//IO口操作,只对单一的IO口!
//确保n的值小于16!
#define PAout(n)   BIT_ADDR(GPIOA_ODR_Addr,n)  //输出 
#define PAin(n)    BIT_ADDR(GPIOA_IDR_Addr,n)  //输入 
#define PBout(n)   BIT_ADDR(GPIOB_ODR_Addr,n)  //输出 
#define PBin(n)    BIT_ADDR(GPIOB_IDR_Addr,n)  //输入 
#define PCout(n)   BIT_ADDR(GPIOC_ODR_Addr,n)  //输出 
#define PCin(n)    BIT_ADDR(GPIOC_IDR_Addr,n)  //输入 
#define PDout(n)   BIT_ADDR(GPIOD_ODR_Addr,n)  //输出 
#define PDin(n)    BIT_ADDR(GPIOD_IDR_Addr,n)  //输入 
#define PEout(n)   BIT_ADDR(GPIOE_ODR_Addr,n)  //输出 
#define PEin(n)    BIT_ADDR(GPIOE_IDR_Addr,n)  //输入
#define PFout(n)   BIT_ADDR(GPIOF_ODR_Addr,n)  //输出 
#define PFin(n)    BIT_ADDR(GPIOF_IDR_Addr,n)  //输入
#define PGout(n)   BIT_ADDR(GPIOG_ODR_Addr,n)  //输出 
#define PGin(n)    BIT_ADDR(GPIOG_IDR_Addr,n)  //输入
//以下为汇编函数
void WFI_SET(void);		//执行WFI指令
void INTX_DISABLE(void);//关闭所有中断
void INTX_ENABLE(void);	//开启所有中断
void MSR_MSP(u32 addr);	//设置堆栈地址
#endif
#include "stm32f10x.h"
#include "Delay.h"
#include "ps2.h"
#include "moto.h"
#include "sys.h"
#include "encoder.h"
#include "pwm.h"
#include "gpio.h"
#include <math.h>
#include "Servo.h"
#define PS2_MAX_SPEED 7000
int PS2_KEY;
uint32_t system_time = 0;
float Target_Left, Actual_Left, Out_Left;
float Target_Right, Actual_Right, Out_Right;
float Kp = 0.5, Ki = 0.5, Kd = 0.2;
float Error0_Left, Error1_Left, Error2_Left;
float Error0_Right, Error1_Right, Error2_Right;
static float dout_last_Left, dout_last_Right;
float dout_Left, dout_Right;
int32_t Encoder_Offset[4] = {0};
int main(void)
{ 
    NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
    
    delay_Init();
    Servo_Init();
    
    PS2_Init();
    PS2_SetInit();
    Gpio_Init();
    PWM_Init(7200, 0);
    Encoder_Init_Tim2();
    Encoder_Init_Tim3();
    Encoder_Init_Tim4();
    Encoder_Init_Tim8();
    
    uint8_t count = 0;
    do {																			
        PS2_SetInit();
        delay_ms(100);
        count++;
    } while(PS2_RedLight() != 0 && count < 3);
    
    while(1)
    {   
        PS2_KEY = PS2_DataKey();
        
        // 电机控制
        switch(PS2_KEY)
        {
            case 5:
                Target_Left = PS2_MAX_SPEED * 2.0f;
                Target_Right = PS2_MAX_SPEED * 2.0f;
                break;
            case 7:
                Target_Left = -PS2_MAX_SPEED * 2.0f;
                Target_Right = -PS2_MAX_SPEED * 2.0f;
                break;
            case 6:
                Target_Left = -PS2_MAX_SPEED * 2.0f;
                Target_Right = PS2_MAX_SPEED * 2.0f;
                break;
            case 8:
                Target_Left = PS2_MAX_SPEED * 2.0f;
                Target_Right = -PS2_MAX_SPEED * 2.0f;
                break;
            case 2:
                    Servo_Start_UP();
                break;
            case 3:
                    Servo_Start_DOWN();
                break;
            default:
                Target_Left = 0;
                Target_Right = 0;
                break;
        }
        
        system_time++;
        
        // 编码器读取和PID控制
        Encoder_Offset[0] = Read_Encoder(2);
        Encoder_Offset[1] = Read_Encoder(3);  
        Encoder_Offset[2] = Read_Encoder(4);
        Encoder_Offset[3] = Read_Encoder(8);
        
        Actual_Left = (Encoder_Offset[0] + Encoder_Offset[1]) / 2.0f;
        Actual_Right = (Encoder_Offset[2] + Encoder_Offset[3]) / 2.0f;
        
        // PID计算
        Error2_Left = Error1_Left;
        Error1_Left = Error0_Left;
        Error0_Left = Target_Left - Actual_Left;
        
        // 积分分离
        float c_Left;
        if (fabs(Error0_Left) < 100) {
            c_Left = 1;
        } else {
            c_Left = 0;
        }
        
        // 不完全微分
        float alpha = 0.8f;
        float dout_standard_Left = Kd * (Error0_Left + Error2_Left - 2 * Error1_Left);
        dout_last_Left = dout_Left;
        dout_Left = (1 - alpha) * dout_standard_Left + alpha * dout_last_Left;
        
        // 设置死区
        if (fabs(Error0_Left) < 10) {
            Out_Left = 0;
        } else {
            Out_Left += Kp * (Error0_Left - Error1_Left) + Ki * Error0_Left * c_Left + dout_Left;
        }
        
        if (Out_Left > PS2_MAX_SPEED * 2.0f) {
            Out_Left = PS2_MAX_SPEED * 2.0f;
        }
        if (Out_Left < -PS2_MAX_SPEED * 2.0f) {
            Out_Left = -PS2_MAX_SPEED * 2.0f;
        }
        
        Error2_Right = Error1_Right;
        Error1_Right = Error0_Right;
        Error0_Right = Target_Right - Actual_Right;
        
        float c_Right;
        if (fabs(Error0_Right) < 100) {
            c_Right = 1;
        } else {
            c_Right = 0;
        }
        
        float dout_standard_Right = Kd * (Error0_Right + Error2_Right - 2 * Error1_Right);
        dout_last_Right = dout_Right;
        dout_Right = (1 - alpha) * dout_standard_Right + alpha * dout_last_Right;
        
        if (fabs(Error0_Right) < 10) {
            Out_Right = 0;
        } else {
            Out_Right += Kp * (Error0_Right - Error1_Right) + Ki * Error0_Right * c_Right + dout_Right;
        }
        
        if (Out_Right > PS2_MAX_SPEED * 2.0f) {
            Out_Right = PS2_MAX_SPEED * 2.0f;
        }
        if (Out_Right < -PS2_MAX_SPEED * 2.0f) {
            Out_Right = -PS2_MAX_SPEED * 2.0f;
        }
        
        // 电机输出
        motor_SetSpeed1((int16_t)Out_Left);
        motor_SetSpeed2((int16_t)Out_Left);
        motor_SetSpeed3((int16_t)Out_Right);
        motor_SetSpeed4((int16_t)Out_Right);
        
        delay_ms(10);
    }
}
#include "stm32f10x.h"               
#include "Servo.h"
#include "delay.h"
SERVO_PARAM Servo = {1500, 500, 1500, 2000, 5}; 
void Servo_Init(void)
{
    
    Servo_Gpio_Init();
    delay_ms(10);
    Servo_Time_Init();
    delay_ms(10);
    
    Servo_Pwm_Set(Servo.middle);
    delay_ms(200);
}
void Servo_Start_UP(void)
{
    if (Servo.pwm < Servo.right_limit) {
        uint16_t new_pwm = Servo.pwm + Servo.speed;
        if(new_pwm > Servo.right_limit) {
            new_pwm = Servo.right_limit;
        }
        Servo_Pwm_Set(new_pwm);
    }
}
void Servo_Start_DOWN(void)
{
    if (Servo.pwm > Servo.left_limit) {
        uint16_t new_pwm = Servo.pwm - Servo.speed;
        if(new_pwm < Servo.left_limit) {
            new_pwm = Servo.left_limit;
        }
        Servo_Pwm_Set(new_pwm);
    }
}
void Servo_STOP(void)
{
	
}
#define SERVO_PWM_PIN GPIO_Pin_8
#define SERVO_PWM_PORT GPIOA
void Servo_Gpio_Init(void)
{
    // 开启GPIOA和AFIO时钟
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_AFIO, ENABLE);
    
    GPIO_InitTypeDef GPIO_InitStructure;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; 
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8;  
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(GPIOA, &GPIO_InitStructure);
    
}
void Servo_Time_Init(void)
{
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);
    
    TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
    TIM_TimeBaseInitStructure.TIM_Period = 20000-1;
    TIM_TimeBaseInitStructure.TIM_Prescaler = 72-1;
    TIM_TimeBaseInitStructure.TIM_ClockDivision = 0;
    TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up;
    TIM_TimeBaseInitStructure.TIM_RepetitionCounter = 0;
    TIM_TimeBaseInit(TIM1, &TIM_TimeBaseInitStructure);
    
    TIM_OCInitTypeDef TIM_OCInitStructure;
    TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
    TIM_OCInitStructure.TIM_OutputNState = TIM_OutputState_Disable; 
    TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
    TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCPolarity_High;
    TIM_OCInitStructure.TIM_Pulse = Servo.middle;
    
    TIM_OC1Init(TIM1, &TIM_OCInitStructure); 
    TIM_OC1PreloadConfig(TIM1, TIM_OCPreload_Enable);
    
    TIM_ARRPreloadConfig(TIM1, ENABLE);
    TIM_CtrlPWMOutputs(TIM1, ENABLE);
    TIM_Cmd(TIM1, ENABLE);
}
void Servo_Pwm_Set(uint16_t value)
{
    if (value < Servo.left_limit) {
        value = Servo.left_limit;
    } else if (value > Servo.right_limit) {
        value = Servo.right_limit;																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																																			
    }
    
    TIM_SetCompare1(TIM1, value); 
    Servo.pwm = value;
}
#ifndef _SERVO_H_
#define _SERVO_H_
#include "stm32f10x.h"          
#define SERVO_PWM_PIN GPIO_Pin_8
#define SERVO_PWM_PORT GPIOA
typedef struct {
    uint16_t pwm;
    uint16_t left_limit;  
    uint16_t middle;       
    uint16_t right_limit;  
    uint16_t speed;
} SERVO_PARAM;
extern SERVO_PARAM Servo;
void Servo_Init(void);
void Servo_Gpio_Init(void);
void Servo_Pwm_Set(uint16_t value);
void Servo_Start_UP(void);   
void Servo_Start_DOWN(void);   
void Servo_STOP(void);
void Servo_Time_Init(void);
void Servo_Update_PWM(void);
#endif
#include "encoder.h"
#include "stm32f10x_gpio.h"
/*******************************************************
Function:Initialize TIM1 to encoder interface mode
Input   ;none
Output  :none
函数    :把TIM1初始化为编码器接口模式
入口参数:无
返回值  :无
********************************************************/
void Encoder_Init_Tim8(void)
{
	TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;  
  TIM_ICInitTypeDef TIM_ICInitStructure;  
  GPIO_InitTypeDef GPIO_InitStructure;
  RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8, ENABLE);//使能定时器4的时钟
  RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE);//使能PB端口时钟
	
	
  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7;	//端口配置
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; //浮空输入
  GPIO_Init(GPIOC, &GPIO_InitStructure);					      //根据设定参数初始化GPIOA
  
  TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
  TIM_TimeBaseStructure.TIM_Prescaler = 0x0; // 预分频器 
  TIM_TimeBaseStructure.TIM_Period = 65535; //设定计数器自动重装值
  TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;//选择时钟分频:不分频
  TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;////TIM向上计数  
  TIM_TimeBaseInit(TIM8, &TIM_TimeBaseStructure);
  TIM_EncoderInterfaceConfig(TIM8, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);//使用编码器模式3
  TIM_ICStructInit(&TIM_ICInitStructure);
  TIM_ICInitStructure.TIM_ICFilter = 10;	//滤波10
  TIM_ICInit(TIM8, &TIM_ICInitStructure);
  TIM_ClearFlag(TIM8, TIM_FLAG_Update);//清除TIM的更新标志位
  TIM_ITConfig(TIM8, TIM_IT_Update, ENABLE);
  //Reset counter
  TIM_SetCounter(TIM8,0);
  TIM_Cmd(TIM8, ENABLE); 
}
/*******************************************************
Function:Initialize TIM2 to encoder interface mode
Input   ;none
Output  :none
函数    :把TIM2初始化为编码器接口模式
入口参数:无
返回值  :无
********************************************************/
void Encoder_Init_Tim2(void)
{
	TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;  
  TIM_ICInitTypeDef TIM_ICInitStructure;  
  GPIO_InitTypeDef GPIO_InitStructure;
  RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);//使能定时器4的时钟
  RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA|RCC_APB2Periph_AFIO, ENABLE);//使能PB端口时钟
	
  GPIO_PinRemapConfig(GPIO_FullRemap_TIM2,ENABLE);
//  GPIO_PinRemapConfig(GPIO_Remap_SWJ_Disable,ENABLE);
	
  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_15;	//端口配置
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; //浮空输入
  GPIO_Init(GPIOA, &GPIO_InitStructure);					      //根据设定参数初始化GPIOB
	
  GPIO_InitStructure.GPIO_Pin =GPIO_Pin_3;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
  GPIO_Init(GPIOB,&GPIO_InitStructure);
  
  TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
  TIM_TimeBaseStructure.TIM_Prescaler = 0x0; // 预分频器 
  TIM_TimeBaseStructure.TIM_Period = 65535; //设定计数器自动重装值
  TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;//选择时钟分频:不分频
  TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;////TIM向上计数  
  TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);
  TIM_EncoderInterfaceConfig(TIM2, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);//使用编码器模式3
  TIM_ICStructInit(&TIM_ICInitStructure);
  TIM_ICInitStructure.TIM_ICFilter = 10;	//滤波10
  TIM_ICInit(TIM2, &TIM_ICInitStructure);
  TIM_ClearFlag(TIM2, TIM_FLAG_Update);//清除TIM的更新标志位
  TIM_ITConfig(TIM2, TIM_IT_Update, ENABLE);
  //Reset counter
  TIM_SetCounter(TIM2,0);
  TIM_Cmd(TIM2, ENABLE); 
}
/*******************************************************
Function:Initialize TIM2 to encoder interface mode
Input   ;none
Output  :none
函数    :把TIM2初始化为编码器接口模式
入口参数:无
返回值  :无
********************************************************/
void Encoder_Init_Tim3(void)
{
	TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;  
  TIM_ICInitTypeDef TIM_ICInitStructure;  
  GPIO_InitTypeDef GPIO_InitStructure;
  RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);//使能定时器3的时钟
  RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);//使能PB端口时钟
	
  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7;	//端口配置
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; //浮空输入
  GPIO_Init(GPIOA, &GPIO_InitStructure);					      //根据设定参数初始化GPIOB	
  
  TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
  TIM_TimeBaseStructure.TIM_Prescaler = 0x0; // 预分频器 
  TIM_TimeBaseStructure.TIM_Period = 65535; //设定计数器自动重装值
  TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;//选择时钟分频:不分频
  TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;////TIM向上计数  
  TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);
  TIM_EncoderInterfaceConfig(TIM3, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);//使用编码器模式3
  TIM_ICStructInit(&TIM_ICInitStructure);
  TIM_ICInitStructure.TIM_ICFilter = 10;	//滤波10
  TIM_ICInit(TIM3, &TIM_ICInitStructure);
  TIM_ClearFlag(TIM3, TIM_FLAG_Update);//清除TIM的更新标志位
  TIM_ITConfig(TIM3, TIM_IT_Update, ENABLE);
  //Reset counter
  TIM_SetCounter(TIM3,0);
  TIM_Cmd(TIM3, ENABLE); 
}
/**************************************************************************
Function: Initialize TIM4 to encoder interface mode
Input   : none
Output  : none
函数功能:把TIM4初始化为编码器接口模式
入口参数:无
返回  值:无
**************************************************************************/
void Encoder_Init_Tim4(void)
{
	TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;  
  TIM_ICInitTypeDef TIM_ICInitStructure;  
  GPIO_InitTypeDef GPIO_InitStructure;
  RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);//使能定时器4的时钟
  RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);//使能PB端口时钟
	
  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7;	//端口配置
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; //浮空输入
  GPIO_Init(GPIOB, &GPIO_InitStructure);					      //根据设定参数初始化GPIOB
  
  TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
  TIM_TimeBaseStructure.TIM_Prescaler = 0x0; // 预分频器 
  TIM_TimeBaseStructure.TIM_Period = 65535; //设定计数器自动重装值
  TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;//选择时钟分频:不分频
  TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;////TIM向上计数  
  TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);
  TIM_EncoderInterfaceConfig(TIM4, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);//使用编码器模式3
  TIM_ICStructInit(&TIM_ICInitStructure);
  TIM_ICInitStructure.TIM_ICFilter = 10;
  TIM_ICInit(TIM4, &TIM_ICInitStructure);
  TIM_ClearFlag(TIM4, TIM_FLAG_Update);//清除TIM的更新标志位
  TIM_ITConfig(TIM4, TIM_IT_Update, ENABLE);
  //Reset counter
  TIM_SetCounter(TIM4,0);
  TIM_Cmd(TIM4, ENABLE); 
}
/**************************************************************************
Function: Read encoder count per unit time
Input   : TIMX:Timer
Output  : none
函数功能:单位时间读取编码器计数
入口参数:TIMX:定时器
返回  值:速度值
**************************************************************************/
int Read_Encoder(u8 TIMX)
{
   int Encoder_TIM;    
   switch(TIMX)
	 {
	   case 8:  Encoder_TIM= (short)TIM8 -> CNT;  TIM8 -> CNT=0;break;
	   case 2:  Encoder_TIM= (short)TIM2 -> CNT;  TIM2 -> CNT=0;break;	
	   case 3:  Encoder_TIM= (short)TIM3 -> CNT;  TIM3 -> CNT=0;break;
	   case 4:  Encoder_TIM= (short)TIM4 -> CNT;  TIM4 -> CNT=0;break;	
	   default: Encoder_TIM=0;
	 }
	 //if(Encoder_TIM<0)  Encoder_TIM=-Encoder_TIM;
		return Encoder_TIM;
}
/**************************************************************************
Function: TIM4 interrupt service function
Input   : none
Output  : none
函数功能:TIM4中断服务函数
入口参数:无
返回  值:无
**************************************************************************/
void TIM4_IRQHandler(void)
{ 		    		  			    
	if(TIM4->SR&0X0001)//溢出中断
	{    				   				     	    	
	}				   
	TIM4->SR&=~(1<<0);//清除中断标志位 	    
}
/**************************************************************************
Function: TIM1 interrupt service function
Input   : none
Output  : none
函数功能:TIM1中断服务函数
入口参数:无
返回  值:无
**************************************************************************/
void TIM8_IRQHandler(void)
{ 		    		  			    
	if(TIM8->SR&0X0001)//溢出中断
	{    				   				     	    	
	}				   
	TIM8->SR&=~(1<<0);//清除中断标志位 	    
}
/**************************************************************************
Function: TIM3 interrupt service function
Input   : none
Output  : none
函数功能:TIM3中断服务函数
入口参数:无
返回  值:无
**************************************************************************/
void TIM3_IRQHandler(void)
{ 		    		  			    
	if(TIM3->SR&0X0001)//溢出中断
	{    				   				     	    	
	}				   
	TIM3->SR&=~(1<<0);//清除中断标志位 	    
}
/**************************************************************************
Function: TIM2 interrupt service function
Input   : none
Output  : none
函数功能:TIM2中断服务函数
入口参数:无
返回  值:无
**************************************************************************/
void TIM2_IRQHandler(void)
{ 		    		  			    
	if(TIM2->SR&0X0001)//溢出中断
	{    				   				     	    	
	}				   
	TIM2->SR&=~(1<<0);//清除中断标志位 	    
}
#ifndef _ENCODER_H
#define _ENCODER_H
#include "stm32f10x.h"
void Encoder_Init_Tim2(void);
void Encoder_Init_Tim4(void);
void Encoder_Init_Tim3(void);
void Encoder_Init_Tim8(void);
int Read_Encoder(u8 TIMX);
// 中断服务函数声明
void TIM4_IRQHandler(void);
void TIM8_IRQHandler(void);
void TIM3_IRQHandler(void);
void TIM2_IRQHandler(void);
#endif
#include "gpio.h"
void Gpio_Init(void)
{
    GPIO_InitTypeDef GPIO_InitStructure;
    
    // 使能时钟
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB | RCC_APB2Periph_GPIOC, ENABLE);
    
    // 配置电机控制引脚为推挽输出
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    
    // 电机A控制引脚
    GPIO_InitStructure.GPIO_Pin = MOTOR_AIN1_PIN | MOTOR_AIN2_PIN;
    GPIO_Init(MOTOR_AIN1_PORT, &GPIO_InitStructure);
    
    // 电机B控制引脚  
    GPIO_InitStructure.GPIO_Pin = MOTOR_BIN1_PIN | MOTOR_BIN2_PIN;
    GPIO_Init(MOTOR_BIN1_PORT, &GPIO_InitStructure);
    
    // 电机C控制引脚
    GPIO_InitStructure.GPIO_Pin = MOTOR_CIN1_PIN | MOTOR_CIN2_PIN;
    GPIO_Init(MOTOR_CIN1_PORT, &GPIO_InitStructure);
    
    // 电机D控制引脚
    GPIO_InitStructure.GPIO_Pin = MOTOR_DIN1_PIN | MOTOR_DIN2_PIN;
    GPIO_Init(MOTOR_DIN1_PORT, &GPIO_InitStructure);
    
    // STBY引脚
    GPIO_InitStructure.GPIO_Pin = MOTOR_STBY_PIN;
    GPIO_Init(MOTOR_STBY_PORT, &GPIO_InitStructure);
    
    // 默认使能电机驱动
    Motor_Enable();
}
void Motor_Enable(void)
{
    GPIO_SetBits(MOTOR_STBY_PORT, MOTOR_STBY_PIN);  // STBY = 1
}
void Motor_Disable(void)
{
    GPIO_ResetBits(MOTOR_STBY_PORT, MOTOR_STBY_PIN); // STBY = 0
}
#ifndef __GPIO_H
#define __GPIO_H
#include "stm32f10x.h"
#define MOTOR_AIN1_PIN    GPIO_Pin_13
#define MOTOR_AIN1_PORT   GPIOC
#define MOTOR_AIN2_PIN    GPIO_Pin_14  
#define MOTOR_AIN2_PORT   GPIOC
#define MOTOR_BIN1_PIN    GPIO_Pin_12
#define MOTOR_BIN1_PORT   GPIOB
#define MOTOR_BIN2_PIN    GPIO_Pin_13
#define MOTOR_BIN2_PORT   GPIOB
#define MOTOR_CIN1_PIN    GPIO_Pin_0
#define MOTOR_CIN1_PORT   GPIOB
#define MOTOR_CIN2_PIN    GPIO_Pin_1
#define MOTOR_CIN2_PORT   GPIOB
#define MOTOR_DIN1_PIN    GPIO_Pin_2
#define MOTOR_DIN1_PORT   GPIOC
#define MOTOR_DIN2_PIN    GPIO_Pin_1
#define MOTOR_DIN2_PORT   GPIOC
// STBY引脚
#define MOTOR_STBY_PIN    GPIO_Pin_15
#define MOTOR_STBY_PORT   GPIOB
void Gpio_Init(void);
void Motor_Enable(void);
void Motor_Disable(void);
#endif
#include "moto.h"
#include "pwm.h"
#include <math.h>
#include <stdlib.h>  // 添加这个头文件用于abs函数
void moto(int mode)
{
	if(mode==1)    //顺时针转动
	{
	 GPIO_SetBits(GPIOC, GPIO_Pin_14);	 // 高电平      PC14 --- AIN2      1   
	 GPIO_ResetBits(GPIOC, GPIO_Pin_13);	 // 低电平}   PC13 --- AIN1      0
	
	 GPIO_SetBits(GPIOB, GPIO_Pin_13);     //高电平   PB13 --- BIN2       1
	 GPIO_ResetBits(GPIOB, GPIO_Pin_12);  // 低电平   PB12 --- BIN1       0
		
	 GPIO_SetBits(GPIOB, GPIO_Pin_1);     //高电平   PB1 --- CIN2       1
	 GPIO_ResetBits(GPIOB, GPIO_Pin_0);  // 低电平   PB0 --- CIN1       0
		
	 GPIO_SetBits(GPIOC, GPIO_Pin_1);     //高电平   PC2 --- DIN2       1
	 GPIO_ResetBits(GPIOC, GPIO_Pin_2);  // 低电平   PC1 --- DIN1       0
	
	}
	 if(mode==0)   //逆时针转动
	{
	 GPIO_SetBits(GPIOC, GPIO_Pin_13);	 // 高电平       PC13 --- AIN1     1
	 GPIO_ResetBits(GPIOC, GPIO_Pin_14);	 // 低电平}    PC14 --- AIN2     0
	
	 GPIO_SetBits(GPIOB, GPIO_Pin_12);     //高电平   PB12 --- BIN1     1
	 GPIO_ResetBits(GPIOB, GPIO_Pin_13);  // 低电平   PB13 --- BIN2        0
		
     GPIO_SetBits(GPIOB, GPIO_Pin_0);     //高电平   PB0 --- CIN1       1
	 GPIO_ResetBits(GPIOB, GPIO_Pin_1);  // 低电平   PB1 --- CIN2       0
		
	 GPIO_SetBits(GPIOC, GPIO_Pin_2);     //高电平   PC1 --- DIN1       1
	 GPIO_ResetBits(GPIOC, GPIO_Pin_1);  // 低电平   PC2 --- DIN2       0
	 }
 
}
void motor_StopAll(void)
{
    motor_SetSpeed1(0);
    motor_SetSpeed2(0);
    motor_SetSpeed3(0);
    motor_SetSpeed4(0);
}
void motor_SetSpeed1(int16_t speed)
{
    // 根据TB6612真值表设置方向
    if(speed > 0) {
        // 正转
        GPIO_SetBits(GPIOC, GPIO_Pin_13);   // AIN1 = 1
        GPIO_ResetBits(GPIOC, GPIO_Pin_14); // AIN2 = 0
    } else if(speed < 0) {
        // 反转
        GPIO_ResetBits(GPIOC, GPIO_Pin_13); // AIN1 = 0
        GPIO_SetBits(GPIOC, GPIO_Pin_14);   // AIN2 = 1
    } else {
        // 停止
        GPIO_ResetBits(GPIOC, GPIO_Pin_13); // AIN1 = 0
        GPIO_ResetBits(GPIOC, GPIO_Pin_14); // AIN2 = 0
    }
    TIM_SetCompare1(TIM5, abs(speed));
}
// 其他三个电机的SetSpeed函数做类似修改,将参数改为int16_t
void motor_SetSpeed2(int16_t speed)
{
    if(speed > 0) {
        GPIO_SetBits(GPIOB, GPIO_Pin_12);   // BIN1 = 1
        GPIO_ResetBits(GPIOB, GPIO_Pin_13); // BIN2 = 0
    } else if(speed < 0) {
        GPIO_ResetBits(GPIOB, GPIO_Pin_12); // BIN1 = 0
        GPIO_SetBits(GPIOB, GPIO_Pin_13);   // BIN2 = 1
    } else {
        GPIO_ResetBits(GPIOB, GPIO_Pin_12); // BIN1 = 0
        GPIO_ResetBits(GPIOB, GPIO_Pin_13); // BIN2 = 0
    }
    TIM_SetCompare2(TIM5, abs(speed));
}
void motor_SetSpeed3(int16_t speed)
{
    if(speed > 0) {
        GPIO_SetBits(GPIOB, GPIO_Pin_0);    // CIN1 = 1
        GPIO_ResetBits(GPIOB, GPIO_Pin_1);  // CIN2 = 0
    } else if(speed < 0) {
        GPIO_ResetBits(GPIOB, GPIO_Pin_0);  // CIN1 = 0
        GPIO_SetBits(GPIOB, GPIO_Pin_1);    // CIN2 = 1
    } else {
        GPIO_ResetBits(GPIOB, GPIO_Pin_0);  // CIN1 = 0
        GPIO_ResetBits(GPIOB, GPIO_Pin_1);  // CIN2 = 0
    }
    TIM_SetCompare3(TIM5, abs(speed));
}
void motor_SetSpeed4(int16_t speed)
{
    if(speed > 0) {
        GPIO_SetBits(GPIOC, GPIO_Pin_2);    // DIN1 = 1
        GPIO_ResetBits(GPIOC, GPIO_Pin_1);  // DIN2 = 0
    } else if(speed < 0) {
        GPIO_ResetBits(GPIOC, GPIO_Pin_2);  // DIN1 = 0
        GPIO_SetBits(GPIOC, GPIO_Pin_1);    // DIN2 = 1
    } else {
        GPIO_ResetBits(GPIOC, GPIO_Pin_2);  // DIN1 = 0
        GPIO_ResetBits(GPIOC, GPIO_Pin_1);  // DIN2 = 0
    }
    TIM_SetCompare4(TIM5, abs(speed));
}#ifndef __MOTO_H
#define __MOTO_H
#include "stm32f10x.h"
void moto_Init(void);
void moto(int mode);
void motor_SetSpeed1(int16_t speed);  // 改为int16_t
void motor_SetSpeed2(int16_t speed);
void motor_SetSpeed3(int16_t speed);
void motor_SetSpeed4(int16_t speed);
void motor_StopAll(void);
int Velocity_A(int TargetVelocity, int CurrentVelocity);
int Velocity_B(int TargetVelocity, int CurrentVelocity);
int Velocity_C(int TargetVelocity, int CurrentVelocity);
int Velocity_D(int TargetVelocity, int CurrentVelocity);
#endif
#include "ps2.h"
 
#define DELAY_TIME  delay_us(5); 
u16 Handkey;	// 按键值读取,零时存储。
u8 Comd[2]={0x01,0x42};	//开始命令。请求数据
u8 Data[9]={0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}; //数据存储数组
u16 MASK[]={
    PSB_SELECT,
    PSB_L1,
    PSB_R1,
    PSB_START,
    PSB_PAD_UP,
    PSB_PAD_RIGHT,
    PSB_PAD_DOWN,
    PSB_PAD_LEFT,
    PSB_L2,
    PSB_R2,
    PSB_L3,
    PSB_R3,
    PSB_GREEN,
    PSB_RED,
    PSB_BLUE,
    PSB_PINK
	};	
 /**************************************************************************
函数功能:以下是PS2接收器模块的初始化代码
入口参数:无
返回  值:无
按键值与按键明
手柄接口初始化    输入  DI->PA11
                 输出  DO->PA1    CS->PA2 CLK->PA0
**************************************************************************/
void PS2_Init(void)
{
	GPIO_InitTypeDef GPIO_InitStructure;                           //定义GPIO_InitStructure结构体
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);           //开启GPIOA时钟
	GPIO_InitStructure.GPIO_Mode=GPIO_Mode_Out_PP;                 //复用推挽输出模式
	GPIO_InitStructure.GPIO_Pin=GPIO_Pin_8|GPIO_Pin_9|GPIO_Pin_10;  //P1.P2.P0口 
	GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;             	 //50MHZ
	GPIO_Init(GPIOB,&GPIO_InitStructure);                          //GPIOA初始化  PA1 PA2  PA0		
	GPIO_InitStructure.GPIO_Mode=GPIO_Mode_IPD;                     //下拉输入模式
	GPIO_InitStructure.GPIO_Pin=GPIO_Pin_11;													//P3口
	GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;                //50MHZ  
	GPIO_Init(GPIOB,&GPIO_InitStructure);                          //GPIOA初始化 PA0 
}
/**************************************************************************
函数功能:向手柄发送命令
入口参数:CMD指令
返回  值:无
**************************************************************************/
void PS2_Cmd(u8 CMD)
{
	volatile u16 ref=0x01;
	Data[1] = 0;
	for(ref=0x01;ref<0x0100;ref<<=1)
	{
		if(ref&CMD)
		{
			DO_H;                   //拉高输出位,输出一位控制位
		}
		else DO_L;								
		CLK_H;                        //时钟拉高
		DELAY_TIME;
		CLK_L;
		DELAY_TIME;
		CLK_H;
		if(DI)
			Data[1] = ref|Data[1];
	}
	delay_us(16);
}
/**************************************************************************
函数功能:判断是否为红灯模式,0x41=模拟绿灯,0x73=模拟红灯
入口参数:CMD指令
返回  值:0,红灯模式  其他,其他模式
**************************************************************************/
u8 PS2_RedLight(void)
{
	CS_L;
	PS2_Cmd(Comd[0]);  //开始命令
	PS2_Cmd(Comd[1]);  //请求数据
	CS_H;
	if( Data[1] == 0X41)   return 1 ;
	else return 0;
}
/**************************************************************************
函数功能:读取手柄数据
入口参数:无
返回  值:无
**************************************************************************/
void PS2_ReadData(void)
{
	volatile u8 byte=0;
	volatile u16 ref=0x01;
	CS_L;
	PS2_Cmd(Comd[0]);  //开始命令
	PS2_Cmd(Comd[1]);  //请求数据
	for(byte=2;byte<9;byte++)          //开始接受数据
	{
		for(ref=0x01;ref<0x100;ref<<=1)
		{
			CLK_H;
			DELAY_TIME;
			CLK_L;
			DELAY_TIME;
			CLK_H;
		      if(DI)
		      Data[byte] = ref|Data[byte];
		}
        delay_us(16);
	}
	CS_H;
}
/**************************************************************************
函数功能:对读出来的PS2的数据进行处理,只处理按键部分
入口参数:CMD指令
返回  值:无  
//只有一个按键按下时按下为0, 未按下为1
**************************************************************************/
u8 PS2_DataKey()
{
	u8 index;
	PS2_ClearData();
	PS2_ReadData();
	Handkey=(Data[4]<<8)|Data[3];     //这是16个按键  按下为0, 未按下为1
	for(index=0;index<16;index++)
	{	    
		if((Handkey&(1<<(MASK[index]-1)))==0)
		return index+1;
	}
	return 0;          //有按键按下回调0传输给while循环的函数里。
}
/**************************************************************************
函数功能:向手柄发送命令
入口参数:得到一个摇杆的模拟量	 范围0~256
返回  值:无
**************************************************************************/
u8 PS2_AnologData(u8 button)
{
	return Data[button];
}
//清除数据缓冲区
void PS2_ClearData()
{
	u8 a;
	for(a=0;a<9;a++)
		Data[a]=0x00;
}
/******************************************************
函数功能: 手柄震动函数,
Calls:		 void PS2_Cmd(u8 CMD);
入口参数: motor1:右侧小震动电机 0x00关,其他开
	        motor2:左侧大震动电机 0x40~0xFF 电机开,值越大 震动越大
返回  值:无
******************************************************/
void PS2_Vibration(u8 motor1, u8 motor2)
{
	CS_L;
	delay_us(16);
    PS2_Cmd(0x01);  //开始命令
	PS2_Cmd(0x42);    //请求数据
	PS2_Cmd(0X00);
	PS2_Cmd(motor1);
	PS2_Cmd(motor2);
	PS2_Cmd(0X00);
	PS2_Cmd(0X00);
	PS2_Cmd(0X00);
	PS2_Cmd(0X00);
	CS_H;
	delay_us(16);  
}
/**************************************************************************
函数功能:short poll
入口参数:无
返回  值:无
**************************************************************************/
void PS2_ShortPoll(void)
{
	CS_L;
	delay_us(16);
	PS2_Cmd(0x01);  
	PS2_Cmd(0x42);  
	PS2_Cmd(0X00);
	PS2_Cmd(0x00);
	PS2_Cmd(0x00);
	CS_H;
	delay_us(16);	
}
/**************************************************************************
函数功能:进入配置
入口参数:无
返回  值:无
**************************************************************************/
void PS2_EnterConfing(void)
{
    CS_L;
	delay_us(16);
	PS2_Cmd(0x01);  
	PS2_Cmd(0x43);  
	PS2_Cmd(0X00);
	PS2_Cmd(0x01);
	PS2_Cmd(0x00);
	PS2_Cmd(0X00);
	PS2_Cmd(0X00);
	PS2_Cmd(0X00);
	PS2_Cmd(0X00);
	CS_H;
	delay_us(16);
}
/**************************************************************************
函数功能:发送模式设置
入口参数:无
返回  值:无
**************************************************************************/
void PS2_TurnOnAnalogMode(void)
{
	CS_L;
	PS2_Cmd(0x01);  
	PS2_Cmd(0x44);  
	PS2_Cmd(0X00);
	PS2_Cmd(0x01); //analog=0x01;digital=0x00  软件设置发送模式
	PS2_Cmd(0xEE); //Ox03锁存设置,即不可通过按键“MODE”设置模式。固定为绿灯模式。
				   //0xEE不锁存软件设置,可通过按键“MODE”设置模式。
	PS2_Cmd(0X00);
	PS2_Cmd(0X00);
	PS2_Cmd(0X00);
	PS2_Cmd(0X00);
	CS_H;
	delay_us(16);
}
/**************************************************************************
函数功能:振动设置
入口参数:无
返回  值:无
**************************************************************************/
void PS2_VibrationMode(void)
{
	CS_L;
	delay_us(16);
	PS2_Cmd(0x01);  
	PS2_Cmd(0x4D);  
	PS2_Cmd(0X00);
	PS2_Cmd(0x00);
	PS2_Cmd(0X01);
	CS_H;
	delay_us(16);	
}
/**************************************************************************
函数功能:完成并保存配置
入口参数:无
返回  值:无
**************************************************************************/
void PS2_ExitConfing(void)
{
    CS_L;
	delay_us(16);
	PS2_Cmd(0x01);  
	PS2_Cmd(0x43);  
	PS2_Cmd(0X00);
	PS2_Cmd(0x00);
	PS2_Cmd(0x5A);
	PS2_Cmd(0x5A);
	PS2_Cmd(0x5A);
	PS2_Cmd(0x5A);
	PS2_Cmd(0x5A);
	CS_H;
	delay_us(16);
}
/**************************************************************************
函数功能:手柄配置初始化
入口参数:无
返回  值:无
**************************************************************************/
void PS2_SetInit(void)
{
	PS2_ShortPoll();
	PS2_ShortPoll();
	PS2_ShortPoll();
	PS2_EnterConfing();		//进入配置模式
	PS2_TurnOnAnalogMode();	//“红绿灯”配置模式,并选择是否保存
	//PS2_VibrationMode();	//开启震动模式
	PS2_ExitConfing();		//完成并保存配置
}
#ifndef __PSTWO_H
#define __PSTWO_H
#include "delay.h"
#include "sys.h"
/*********************************************************
      
**********************************************************/	 
#define DI   PBin(11)           //PA0  输入
#define DO_H PBout(10)=1        //命令位高
#define DO_L PBout(10)=0        //命令位低
#define CS_H PBout(9)=1       //CS拉高
#define CS_L PBout(9)=0       //CS拉低
#define CLK_H PBout(8)=1      //时钟拉高
#define CLK_L PBout(8)=0      //时钟拉低
#define PSB_SELECT      1
#define PSB_L1          2
#define PSB_R1          3
#define PSB_START       4
#define PSB_PAD_UP      5
#define PSB_PAD_RIGHT   6
#define PSB_PAD_DOWN    7
#define PSB_PAD_LEFT    8
#define PSB_L2          9 /**/
#define PSB_R2          10/**/
#define PSB_L3          11
#define PSB_R3          12
#define PSB_GREEN       13
#define PSB_RED         14
#define PSB_BLUE        15
#define PSB_PINK        16
#define PSB_TRIANGLE    13
#define PSB_CIRCLE      14
#define PSB_CROSS       15
#define PSB_SQUARE      16
//#define WHAMMY_BAR		8
//These are stick values
#define PSS_RX 5                //右摇杆X轴数据
#define PSS_RY 6
#define PSS_LX 7
#define PSS_LY 8
extern u8 Data[9];
extern u16 MASK[16];
extern u16 Handkey;
void PS2_Init(void);
u8 PS2_RedLight(void);   //判断是否为红灯模式
void PS2_ReadData(void); //读手柄数据
void PS2_Cmd(u8 CMD);		  //向手柄发送命令
u8 PS2_DataKey(void);		  //按键值读取
u8 PS2_AnologData(u8 button); //得到一个摇杆的模拟量
void PS2_ClearData(void);	  //清除数据缓冲区
void PS2_Vibration(u8 motor1, u8 motor2);//振动设置motor1  0xFF开,其他关,motor2  0x40~0xFF
void PS2_EnterConfing(void);	 //进入配置
void PS2_TurnOnAnalogMode(void); //发送模拟量
void PS2_VibrationMode(void);    //振动设置
void PS2_ExitConfing(void);	     //完成配置
void PS2_SetInit(void);		     //配置初始化
#endif
#include "pwm.h"
#include "math.h"
#include <stdlib.h>
/**************************************************************************
函数功能:pwm初始化
入口参数:arr:设为一个时钟频率的最大值  psc: 预分频值
返回  值:无
**************************************************************************/
void PWM_Init(u16 arr,u16 psc)
{
	GPIO_InitTypeDef GPIO_InitStructure;                //定义结构体GPIO_InitStructure
	TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;      //定义结构体TIM_TimeBaseStructure   
	TIM_OCInitTypeDef TIM_OCInitStructure;              //定义结构体TIM_OCInitStructure
	
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);//使能PA端口时钟
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5,ENABLE);//使能定时器3
	
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
	GPIO_InitStructure.GPIO_Pin  = GPIO_Pin_0|GPIO_Pin_1|GPIO_Pin_2|GPIO_Pin_3;
	GPIO_InitStructure.GPIO_Speed= GPIO_Speed_50MHz;
	GPIO_Init(GPIOA,&GPIO_InitStructure);
	
	TIM_TimeBaseStructure.TIM_Period = arr;                //设置下一个更新活动的自动重装载寄存器的值
	TIM_TimeBaseStructure.TIM_Prescaler = psc;             //预分配值
	TIM_TimeBaseStructure.TIM_ClockDivision = 0;           //时钟分割
	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; //向上计数
	TIM_TimeBaseInit(TIM5,&TIM_TimeBaseStructure);
	
	TIM_OCInitStructure.TIM_OCMode= TIM_OCMode_PWM1;             //PWM脉冲宽度调制1
	TIM_OCInitStructure.TIM_Pulse = 0;                           //设置待装入捕获比较寄存器的脉冲值
	TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;    //设置TIM输出极性为高
	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;//比较输出使能
	TIM_OC1Init(TIM5,&TIM_OCInitStructure);
	TIM_OC2Init(TIM5,&TIM_OCInitStructure);
	TIM_OC3Init(TIM5,&TIM_OCInitStructure);
	TIM_OC4Init(TIM5,&TIM_OCInitStructure);
	
	
	TIM_OC1PreloadConfig(TIM5,TIM_OCPreload_Enable);
	TIM_OC2PreloadConfig(TIM5,TIM_OCPreload_Enable);
	TIM_OC3PreloadConfig(TIM5,TIM_OCPreload_Enable);
	TIM_OC4PreloadConfig(TIM5,TIM_OCPreload_Enable);//使能预装载寄存器
	
	TIM_ARRPreloadConfig(TIM5,ENABLE);              //使能自动装载允许位
	TIM_Cmd(TIM5,ENABLE);//启动定时器5	
}
int myabs(int value){
	if (value<0) value=-value;
	return value;
}
void Set_PWM(int motor1,int motor2,int motor3,int motor4)
{
	if(motor1>0){
		GPIO_SetBits(GPIOC, GPIO_Pin_14);	 // 高电平      PC14 --- AIN2      1   
		GPIO_ResetBits(GPIOC, GPIO_Pin_13);	 // 低电平}   PC13 --- AIN1      0
	
	}else{
		 GPIO_SetBits(GPIOC, GPIO_Pin_13);	 // 高电平       PC13 --- AIN1     1
	 GPIO_ResetBits(GPIOC, GPIO_Pin_14);	 // 低电平}    PC14 --- AIN2     0
	}
	if(motor2>0){
		GPIO_SetBits(GPIOB, GPIO_Pin_13);     //高电平   PB13 --- BIN2       1
	 GPIO_ResetBits(GPIOB, GPIO_Pin_12);  // 低电平   PB12 --- BIN1       0
	}else{
	GPIO_SetBits(GPIOB, GPIO_Pin_12);     //高电平   PB12 --- BIN1     1
	 GPIO_ResetBits(GPIOB, GPIO_Pin_13);  // 低电平   PB13 --- BIN2        0
	}
	if(motor3>0){
		 GPIO_SetBits(GPIOB, GPIO_Pin_1);     //高电平   PB1 --- CIN2       1
	 GPIO_ResetBits(GPIOB, GPIO_Pin_0);  // 低电平   PB0 --- CIN1       0
	}else{
	  GPIO_SetBits(GPIOB, GPIO_Pin_0);     //高电平   PB0 --- CIN1       1
	 GPIO_ResetBits(GPIOB, GPIO_Pin_1);  // 低电平   PB1 --- CIN2       0
	}
	if(motor4>0){
		 GPIO_SetBits(GPIOC, GPIO_Pin_1);     //高电平   PC2 --- DIN2       1
	 GPIO_ResetBits(GPIOC, GPIO_Pin_2);  // 低电平   PC1 --- DIN1       0
	}else{
	GPIO_SetBits(GPIOC, GPIO_Pin_2);     //高电平   PC1 --- DIN1       1
	 GPIO_ResetBits(GPIOC, GPIO_Pin_1);  // 低电平   PC2 --- DIN2       0
	}
	TIM_SetCompare1(TIM5,myabs(motor1));
	TIM_SetCompare2(TIM5,myabs(motor2));
    TIM_SetCompare3(TIM5,myabs(motor3));//设置TIM3通道3的占空比  3000/7200
    TIM_SetCompare4(TIM5,myabs(motor4));
	
}
#ifndef __PWM_H
#define	__PWM_H
#include "stm32f10x.h"
void PWM_Init(u16 arr,u16 psc);
void Set_PWM(int motor1,int motor2,int motot3,int motor4);
#endif
帮我检查代码,为什么在ps2上使用#define PSB_GREEN       13
#define PSB_RED         14
#define PSB_BLUE        15
#define PSB_PINK        16这些按键时候没有反应,没法操控小车实现实时的舵机上升和下降
					最新发布