標(biāo)題: 三軸云臺EVVGC源代碼(PID控制方式 6050傳感器驅(qū)動) [打印本頁]
作者: hcz76315    時(shí)間: 2018-10-30 23:28
標(biāo)題: 三軸云臺EVVGC源代碼(PID控制方式 6050傳感器驅(qū)動)
簡單三軸云臺PID控制方式 6050傳感器驅(qū)動的云臺控制方案。

單片機(jī)源程序如下:
  1. #include "stm32f10x.h"
  2. #include "stm32f10x_rcc.h"
  3. #include "stm32f10x_gpio.h"
  4. #include "stm32f10x_usart.h"
  5. #include "stm32f10x_tim.h"
  6. #include "stm32f10x_i2c.h"
  7. #include "stm32f10x_adc.h"
  8. #include <stdio.h>
  9. #include <math.h>

  11. #define MPU6050_ADDRESS     0xD0   // Gyro address
  12. #define EEPROM_ADDRESS      0xAE   //EEPROM address
  13. #define MPU6050_I2C I2C2 //mpu6050 bus
  14. #define LEDon  GPIO_WriteBit(GPIOB, GPIO_Pin_12,   Bit_SET)
  15. #define LEDoff GPIO_WriteBit(GPIOB, GPIO_Pin_12, Bit_RESET)



  19.        GPIO_InitTypeDef        GPIO_InitStructure;
  20.             TIM_OCInitTypeDef       TIM_OCInitStructure;
  21. TIM_TimeBaseInitTypeDef     TIM_TimeBaseStructure;
  22. TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
  23.     TIM_BDTRInitTypeDef                 TIM_BDTRInitStructure;
  24. USART_ClockInitTypeDef  USART_ClockInitStructure;
  25.                                 ADC_InitTypeDef         ADC_InitStructure;
  26.                                 I2C_InitTypeDef         I2C_InitStructure;



  30. void Periph_clock_enable(void);
  31. void GPIO_Config(void);
  32. void Usart4Init(void);
  33. void USART_PutChar(uint8_t ch);
  34. void USART_PutString(uint8_t * str);
  35. void I2C_Config(void);
  36. void ADC_Config(void);
  37. void Timer1_Config(void);
  38. void Timer8_Config(void);
  39. void Timer2_Config(void);
  40. void Timer5_Config(void);
  41. void Timer4_Config(void);
  42. void MPU6050_Init(void);
  43. void Delay_ms(uint32_t ms);
  44. void MPU6050_I2C_BufferRead(u8 slaveAddr, u8* pBuffer, u8 readAddr, u16 NumByteToRead);
  45. void MPU6050_Gyro_get(void);
  46. void MPU6050_ACC_get(void);
  47. void pitch_PID(void);
  48. void roll_PID(void);
  49. void yaw_PID(void);
  50. u16 readADC1(u8 channel);
  51. void WriteToEEPROM(int addressToWrite, int DataToWrite);
  52. void ReadFromEEPROM(u8 readAddr);
  53. void NVIC_Configuration(void);
  54. void UART4_IRQHandler();
  55. void saveData(void);


  58. int ConfigMode,  w, enable_writing;
  59. int stop=0, i, YawPh1, YawPh2, YawPh3, EEPROM_REGISTER, EEPROM_DATA, EepromData, UART4_DATA;
  60. float roll_amplitude, pitch_amplitude, yaw_amplitude, dt=0.002, ADC1Ch1_vid, sinus, cosinus, ROLL;
  61. uint8_t XMSB,XLSB,YMSB,YLSB,ZMSB,ZLSB;
  62. uint8_t out[] = {0,0,0,0,0,0};
  63. short int gyroADC_PITCH, gyroADC_ROLL, gyroADC_YAW, accADC_ROLL, accADC_PITCH, accADC_YAW, printcounter;
  64. float pitch, Gyro_Pitch_angle, pitch_output, pitch_setpoint=0, pitch_Error_current, pitch_Error_last, pitch_P, pitch_D, pitch_Kp, pitch_Kd, pitch_angle=0, pitch_angle_true, pitch_angle_correction;
  65. float roll,   Gyro_Roll_angle,  roll_output,  roll_setpoint=0,  roll_Error_current,  roll_Error_last,  roll_P,  roll_D,  roll_Kp,  roll_Kd,  roll_angle=0,  roll_angle_correction;
  66. float yaw,     Gyro_Yaw_angle,   yaw_output,   yaw_setpoint=0,   yaw_Error_current,   yaw_Error_last,   yaw_P,   yaw_D,   yaw_Kp,   yaw_Kd,   yaw_angle=0,   yaw_angle_correction;
  67. float acc_pitch_angle, acc_roll_angle, accADC_x, accADC_y, accADC_z, gyroADC_x, gyroADC_y, gyroADC_z, fs, fssmall, acc_pitch_angle_vid, acc_roll_angle_vid;
  68. uint8_t ACCread[6], GYROread[6];
  69. int RXString[3];
  70. char configData[9]={'1','1','1','1','1','1','1','1','1'};
  71. float sinusas[91]={
  72. 0.000, 0.017, 0.035, 0.052, 0.070, 0.087, 0.105, 0.122, 0.139, 0.156,
  73. 0.174, 0.191, 0.208, 0.225, 0.242, 0.259, 0.276, 0.292, 0.309, 0.326,
  74. 0.342, 0.358, 0.375, 0.391, 0.407, 0.423, 0.438, 0.454, 0.469, 0.485,
  75. 0.500, 0.515, 0.530, 0.545, 0.559, 0.574, 0.588, 0.602, 0.616, 0.629,
  76. 0.643, 0.656, 0.669, 0.682, 0.695, 0.707, 0.719, 0.731, 0.743, 0.755,
  77. 0.766, 0.777, 0.788, 0.799, 0.809, 0.819, 0.829, 0.839, 0.848, 0.857,
  78. 0.866, 0.875, 0.883, 0.891, 0.899, 0.906, 0.914, 0.920, 0.927, 0.934,
  79. 0.940, 0.945, 0.951, 0.956, 0.961, 0.966, 0.970, 0.974, 0.978, 0.982,
  80. 0.985, 0.988, 0.990, 0.993, 0.995, 0.996, 0.998, 0.999, 0.999, 1.000,
  81. 1.000};

  83. char buff[10];
  84. int main(void)
  85. {
  86.        
  87.        
  88.         Periph_clock_enable();
  89.         GPIO_Config();       
  90.         Usart4Init();
  91.         I2C_Config();
  92.         ADC_Config();
  93.         Delay_ms(100);
  94.         MPU6050_Init();
  95.         Timer1_Config();
  96.         Timer8_Config();
  97.         Timer2_Config();
  98.         Timer5_Config();
  99.         Timer4_Config();
  100.         NVIC_Configuration();
  101.        
  102.         TIM_Cmd(TIM5, ENABLE);
  103.         TIM_CtrlPWMOutputs(TIM5, ENABLE);
  104.         for (i=1 ; i<1 ; i++) ;
  105.         TIM_Cmd(TIM4, ENABLE);
  106.         TIM_CtrlPWMOutputs(TIM4, ENABLE);
  107.        
  108.         LEDoff;
  109.   
  110.        
  111.        

  113.         Delay_ms(100);
  114.        
  115.                         for(i=0; i<9;i++){
  116.                                                         ReadFromEEPROM(i);
  117.                                                         configData[i]=EepromData;                                                       
  118.                                                         Delay_ms(5);                                                       
  119.                                                         }
  120.                                                          /* Enable Acknowledgement to be ready for another reception */
  121.                                                         I2C_AcknowledgeConfig(I2C2, ENABLE);
  122.         Delay_ms(100);
  123.        
  124. pitch_Kp = 0.160;  //spring
  125. pitch_Kd = 0.80;//damping

  127. roll_Kp = 0.160;  //spring
  128. roll_Kd = 0.80;//damping

  130. yaw_Kp = 0.160;  //spring
  131. yaw_Kd = 0.80;//damping


  134. //pitch_amplitude=250; //500=100%
  135. //roll_amplitude=450; //500=100%
  136.   //yaw_amplitude=450; //500=100%


  139. while(1)
  140.         {       
  141.                 LEDon;
  142.                
  143.                 while(ConfigMode==1){
  144.                 LEDon;
  145.                 Delay_ms(100);
  146.                 LEDoff;
  147.                 Delay_ms(100);
  148.                 TIM4->CCR1=0;
  149.                 TIM4->CCR2=0;
  150.                 TIM4->CCR3=0;
  151.                 TIM5->CCR1=0;
  152.                 TIM5->CCR2=0;
  153.                 TIM5->CCR3=0;
  154.                 TIM1->CCR1=0;
  155.                 TIM1->CCR2=0;
  156.                 TIM1->CCR3=0;
  157.                 TIM8->CCR1=0;
  158.                 TIM8->CCR2=0;
  159.                 TIM8->CCR3=0;       
  160.                 }
  161.                
  162.                 MPU6050_ACC_get();//Getting Accelerometer data
  163.                
  164.                 acc_pitch_angle = -(atan2(accADC_x, accADC_z))-0.10;   //Calculating pitch ACC angle+callibration
  165.                 acc_roll_angle  = +(atan2(accADC_y, accADC_z));   //Calculating roll ACC angle               
  166.                
  167.                 MPU6050_Gyro_get();//Getting Gyroscope data               
  168.        
  169.                 acc_pitch_angle_vid=  ((acc_pitch_angle_vid*99.00)+acc_pitch_angle)/100.00;        //Averaging pitch ACC values
  170.     acc_roll_angle_vid=   ((acc_roll_angle_vid* 99.00)+acc_roll_angle)/ 100.00; //Averaging roll ACC values               
  171.                
  172.                 sinus   = sinusas[(int)(ADC1Ch1_vid*57.3)];      //Calculating sinus               
  173.     cosinus = sinusas[90-(int)(ADC1Ch1_vid*57.3)];   //Calculating cosinus
  174.                
  175.                 ROLL=-gyroADC_z*sinus+gyroADC_y*cosinus;
  176.                 roll_angle =(roll_angle + ROLL*dt)    + 0.0002*(acc_pitch_angle_vid-roll_angle); //Roll Horizon
  177.                
  178.                
  179.                 //ROLL=-gyroADC_z*sinus+gyroADC_y*cosinus;
  180.                 yaw_angle =(yaw_angle + gyroADC_z*dt); //Yaw
  181.                
  182.                
  183.                
  184.                 pitch_angle_true=((pitch_angle_true  + gyroADC_x*dt) + 0.0002*(acc_roll_angle_vid-pitch_angle_true)); //Pitch Horizon
  185.                
  186.                 ADC1Ch1_vid=  ((ADC1Ch1_vid*99.00)+(readADC1(1)/4000.00))/100.00;        //Averaging ADC values
  187.                 ADC1Ch1_vid=0.00;
  188.                
  189.                 pitch_angle=pitch_angle_true-ADC1Ch1_vid;//Adding angle

  191.                 pitch_angle_correction=pitch_angle*150.0;
  192.                 if(pitch_angle_correction> 2.0){pitch_angle_correction= 2.0;}
  193.                 if(pitch_angle_correction<-2.0){pitch_angle_correction=-2.0;}
  194.                 pitch_setpoint=pitch_setpoint+pitch_angle_correction;//Pitch return to zero after collision
  195.                
  196.                 roll_angle_correction=roll_angle*200.0;
  197.                 if(roll_angle_correction> 2.0){roll_angle_correction= 2.0;}
  198.                 if(roll_angle_correction<-2.0){roll_angle_correction=-2.0;}
  199.                 roll_setpoint=roll_setpoint+roll_angle_correction;//Roll return to zero after collision
  200.                
  201.                 /*yaw_angle_correction=yaw_angle*150.0;
  202.                 if(yaw_angle_correction> 1.0){yaw_angle_correction= 1.0;}
  203.                 if(yaw_angle_correction<-1.0){yaw_angle_correction=-1.0;}
  204.                 yaw_setpoint=yaw_setpoint+yaw_angle_correction;//Roll return to zero after collision*/
  205.                
  206.     pitch_PID();//Pitch axis pid
  207.                 roll_PID(); //Roll axis pid
  208.                 yaw_PID(); //Yaw axis pid
  209.                
  210.                
  211.                 printcounter++; //Print data to UART
  212.                 if (printcounter>=100)
  213.                         {
  214.                                 //sprintf (buff, " %d %d %c Labas\n\r", ACCread[0], ACCread[1], ACCread[2]);
  215.                                 //sprintf (buff, " %x %x %x %x %x %x Labas\n\r", ACCread[0], ACCread[1], ACCread[2], ACCread[3], ACCread[4], ACCread[5]);
  216.                                 //sprintf (buff, "Labas %d %d\n\r", ACCread[0], ACCread[1]);
  217.                                 //sprintf (buff, "%3.1f %f\n\r", ADC1Ch1_vid*57.3, sinus);
  218.                                 //sprintf (buff, "Labas %f %f %f \n\r", accADC_x, accADC_y, accADC_z);
  219.                                 //sprintf (buff, "%3.1f %3.1f \n\r", acc_pitch_angle_vid*57.3,  acc_roll_angle_vid *57.3);
  220.                                 //sprintf (buff, "%3.1f %3.1f \n\r", pitch_angle*57.3,  roll_angle*57.3);
  221.                                 //sprintf (buff, "%3.3f\n\r", yaw_angle*57.3);
  222.                                 //USART_PutString(buff);
  223.                                 printcounter=0;
  224.                         }
  225.                
  226.     stop=0;
  227.                 LEDoff;       
  228.                 while(stop==0) {}//Closed loop waits for interrupt

  230.                
  231.         }
  232. }

  234. void Periph_clock_enable(void)
  235. {
  236.         RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB  |
  237.                          RCC_APB2Periph_GPIOC | RCC_APB2Periph_GPIOD  |
  238.                          RCC_APB2Periph_GPIOE |
  239.                                                                                                  RCC_APB2Periph_AFIO  | RCC_APB2Periph_ADC1        | RCC_APB2Periph_TIM1 | RCC_APB2Periph_TIM8, ENABLE);
  240.         RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5  | RCC_APB1Periph_TIM2 | RCC_APB1Periph_UART4   
  241.                                                                                                  | RCC_APB1Periph_TIM4, ENABLE);
  242.   RCC_AHBPeriphClockCmd (RCC_AHBPeriph_DMA1,  ENABLE);
  243. }

  245. void GPIO_Config(void)
  246. {
  247.                         GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;
  248.                         GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
  249.                         GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  250.                         GPIO_Init(GPIOB, &GPIO_InitStructure);
  251.        
  252.                         //CS_G
  253.                         GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5;
  254.                         GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
  255.                         GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  256.                         GPIO_Init(GPIOC, &GPIO_InitStructure);
  257.        
  258.                         //CS_G
  259.                         GPIO_InitStructure.GPIO_Pin = GPIO_Pin_7;
  260.                         GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
  261.                         GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  262.                         GPIO_Init(GPIOA, &GPIO_InitStructure);
  263. }



  267. void Usart4Init(void)
  268. {
  269.                         GPIO_InitTypeDef GPIO_InitStructure;
  270.                         USART_InitTypeDef USART_InitStructure;
  271.                         USART_ClockInitTypeDef USART_ClockInitStructure;

  273.                         //Set USART2 Tx (PA.02) as AF push-pull
  274.                         GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10;
  275.                         GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
  276.                         GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  277.                         GPIO_Init(GPIOC, &GPIO_InitStructure);
  278.                         //Set USART2 Rx (PA.03) as input floating
  279.                         GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11;
  280.                         GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
  281.                         GPIO_Init(GPIOC, &GPIO_InitStructure);
  282.        
  283.        
  284.                         USART_ClockStructInit(&USART_ClockInitStructure);
  285.                         USART_ClockInit(UART4, &USART_ClockInitStructure);
  286.                         USART_InitStructure.USART_BaudRate = 9600;
  287.                         USART_InitStructure.USART_WordLength = USART_WordLength_8b;
  288.                         USART_InitStructure.USART_StopBits = USART_StopBits_1;
  289.                         USART_InitStructure.USART_Parity = USART_Parity_No ;
  290.                         USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
  291.                         USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
  292.                         //Write USART2 parameters
  293.                         USART_Init(UART4, &USART_InitStructure);
  294.                         //Enable UART4 Receive interrupt
  295.                         USART_ITConfig(UART4, USART_IT_RXNE, ENABLE);
  296.                         //Enable USART2
  297.                         USART_Cmd(UART4, ENABLE);
  298.                         }
  299.                        
  300.                        
  301.                        
  302. void USART_PutChar(uint8_t ch)
  303. {
  304.   while(!(UART4->SR & USART_SR_TXE));
  305.   UART4->DR = ch;
  306. }

  308. void USART_PutString(uint8_t * str)
  309. {
  310. while(*str != 0)
  311.   {
  312.   USART_PutChar(*str);
  313.   str++;
  314. }
  315. }

  317. void UART4_IRQHandler()//UART4 Interrupt handler implementation
  318. {
  319.     int eeRreg;
  320.           ConfigMode=1;
  321.                 while ( USART_GetFlagStatus(UART4, USART_FLAG_RXNE) == RESET);
  322.     UART4_DATA=USART_ReceiveData(UART4);
  323. LEDon;
  324.                 if(UART4_DATA==103){ //if "g"
  325.                                                        
  326.                                                         Delay_ms(100);
  327.                                                         sprintf (buff, "x");
  328.                                                         USART_PutString(buff);
  329.                                                         for(eeRreg=0; eeRreg<9;eeRreg++){
  330.                                                         ReadFromEEPROM(eeRreg);
  331.                                                         Delay_ms(5);
  332.                                                         sprintf (buff, "%c",EepromData);
  333.                                                         USART_PutString(buff);
  334.                                                         }
  335.                                                          /* Enable Acknowledgement to be ready for another reception */
  336.                                                         I2C_AcknowledgeConfig(I2C2, ENABLE);
  337.                                                         }
  338.                                                        
  339.                                                        

  341.                                                        
  342.                 if(enable_writing==1){                                                       
  343.                                                         configData[w]=(int)UART4_DATA;
  344.                                                         w++;
  345.                                                         if(w>8){w=0; enable_writing=0; saveData();}
  346.                                                         }
  347.                                                        
  348.                                                        
  349.                 if(UART4_DATA==104){ // if h (write to eeprom)
  350.                                                        
  351.                                                         enable_writing=1;
  352.                                                         }
  353.                
  354.                                                        
  355.                 if(UART4_DATA==105){
  356.                                                        
  357.                                                         ConfigMode=1;
  358.                                                         }       
  359.                                                        
  360.                 if(UART4_DATA==106){
  361.                                                        
  362.                                                         ConfigMode=0;
  363.                                                         }                                                               
  364.                        
  365.                
  366.                
  367.        
  368. }

  370. void NVIC_Configuration(void)
  371. {
  372.   NVIC_InitTypeDef NVIC_InitStructure;

  374.   /* Configure the NVIC Preemption Priority Bits */  
  375.   NVIC_PriorityGroupConfig(NVIC_PriorityGroup_0);
  376.   
  377.   /* Enable the USARTy Interrupt */
  378.   NVIC_InitStructure.NVIC_IRQChannel = UART4_IRQn;
  379.   NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
  380.   NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
  381.   NVIC_Init(&NVIC_InitStructure);

  383. }
  384. void I2C_Config(void)
  385. {
  386. //Clock enable
  387.                 RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C2, ENABLE);       
  388. //GPIO Setup
  389.     GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10 | GPIO_Pin_11;
  390.     GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;  // Open Drain, I2C bus pulled high externally
  391.     GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  392.     GPIO_Init(GPIOB, &GPIO_InitStructure);

  394. //I2C Setup
  395.           I2C_Cmd(I2C2,ENABLE);               
  396.           I2C_InitStructure.I2C_Mode = I2C_Mode_I2C;
  397.     I2C_InitStructure.I2C_ClockSpeed = 400000;
  398.     I2C_InitStructure.I2C_DutyCycle = I2C_DutyCycle_2;
  399.     I2C_InitStructure.I2C_OwnAddress1 = 0xD0;
  400.     I2C_InitStructure.I2C_Ack = I2C_Ack_Enable;
  401.     I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
  402.     I2C_Init(I2C2,&I2C_InitStructure);
  403.    
  404.           I2C_AcknowledgeConfig(I2C2, ENABLE);

  406. }


  409. void MPU6050_Init(void)
  410. {
  411.         Delay_ms(5);
  412.   I2C_GenerateSTART(I2C2, ENABLE); // Send I2C1 START condition
  413.   while(!I2C_GetFlagStatus(I2C2, I2C_FLAG_SB));
  414.   I2C_Send7bitAddress(I2C2, MPU6050_ADDRESS, I2C_Direction_Transmitter);// Send EEPROM slave Address for write  
  415.   while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED));// Test on I2C1 EV6 and clear it
  416.   I2C_SendData(I2C2, 0x19);// Send I2C1 EEPROM internal address //MPU6050_RA_SMPLRT_DIV
  417.   while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_BYTE_TRANSMITTED)); // Test on I2C1 EV8 and clear it   
  418.   I2C_SendData(I2C2, 0x07);// Send I2C1 EEPROM data   
  419.   while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_BYTE_TRANSMITTED));// Test on I2C1 EV8 and clear it  
  420.   I2C_GenerateSTOP(I2C2, ENABLE);// Send I2C1 STOP Condition
  421.         while(I2C_GetFlagStatus(I2C2, I2C_FLAG_STOPF));
  422.        
  423.         I2C_GenerateSTART(I2C2, ENABLE); // Send I2C1 START condition
  424.   while(!I2C_GetFlagStatus(I2C2, I2C_FLAG_SB));   
  425.   I2C_Send7bitAddress(I2C2, MPU6050_ADDRESS, I2C_Direction_Transmitter);// Send EEPROM slave Address for write   
  426.   while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED));// Test on I2C1 EV6 and clear it  
  427.   I2C_SendData(I2C2, 0x1A);// Send I2C1 EEPROM internal address
  428.   while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_BYTE_TRANSMITTED)); //Test on I2C1 EV8 and clear it
  429.   I2C_SendData(I2C2, 0x02);// Send I2C1 EEPROM data LOW PASS..............................................................................
  430.   while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_BYTE_TRANSMITTED));// Test on I2C1 EV8 and clear it
  431.   I2C_GenerateSTOP(I2C2, ENABLE);// Send I2C1 STOP Condition
  432.         while(I2C_GetFlagStatus(I2C2, I2C_FLAG_STOPF));
  433.        
  434.         I2C_GenerateSTART(I2C2, ENABLE); // Send I2C1 START condition
  435.   while(!I2C_GetFlagStatus(I2C2, I2C_FLAG_SB));   
  436.   I2C_Send7bitAddress(I2C2, MPU6050_ADDRESS, I2C_Direction_Transmitter);// Send EEPROM slave Address for write   
  437.   while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED));// Test on I2C1 EV6 and clear it  
  438.   I2C_SendData(I2C2, 0x1B);// Send I2C1 EEPROM internal address
  439.   while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_BYTE_TRANSMITTED)); //Test on I2C1 EV8 and clear it
  440.   I2C_SendData(I2C2, 0x08);// Send I2C1 EEPROM data
  441.   while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_BYTE_TRANSMITTED));// Test on I2C1 EV8 and clear it
  442.   I2C_GenerateSTOP(I2C2, ENABLE);// Send I2C1 STOP Condition
  443.         while(I2C_GetFlagStatus(I2C2, I2C_FLAG_STOPF));
  444.        
  445.   I2C_GenerateSTART(I2C2, ENABLE); // Send I2C1 START condition
  446.   while(!I2C_GetFlagStatus(I2C2, I2C_FLAG_SB));   
  447.   I2C_Send7bitAddress(I2C2, MPU6050_ADDRESS, I2C_Direction_Transmitter);// Send EEPROM slave Address for write   
  448.   while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED));// Test on I2C1 EV6 and clear it  
  449.   I2C_SendData(I2C2, 0x6B);// Send I2C1 EEPROM internal address
  450.   while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_BYTE_TRANSMITTED)); //Test on I2C1 EV8 and clear it
  451.   I2C_SendData(I2C2, 0x00);// Send I2C1 EEPROM data
  452.   while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_BYTE_TRANSMITTED));// Test on I2C1 EV8 and clear it
  453.   I2C_GenerateSTOP(I2C2, ENABLE);// Send I2C1 STOP Condition
  454.         while(I2C_GetFlagStatus(I2C2, I2C_FLAG_STOPF));
  455.        
  456.         Delay_ms(5);

  458. }

  460. void ADC_Config(void)
  461. {
  462. // Configure ADC on ADC0_IN0    pin PA0
  463.   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1;
  464.   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN;
  465.   GPIO_Init(GPIOA, &GPIO_InitStructure);
  466.        

  468.   /* PCLK2 is the APB2 clock */
  469.   /* ADCCLK = PCLK2/6 = 72/6 = 12MHz*/
  470.   RCC_ADCCLKConfig(RCC_PCLK2_Div6);

  472.   /* Enable ADC1 clock so that we can talk to it */
  473.   RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE);
  474.   /* Put everything back to power-on defaults */
  475.   ADC_DeInit(ADC1);

  477.   /* ADC1 Configuration ------------------------------------------------------*/
  478.   /* ADC1 and ADC2 operate independently */
  479.   ADC_InitStructure.ADC_Mode = ADC_Mode_Independent;
  480.   /* Disable the scan conversion so we do one at a time */
  481.   ADC_InitStructure.ADC_ScanConvMode = DISABLE;
  482.   /* Don't do contimuous conversions - do them on demand */
  483.   ADC_InitStructure.ADC_ContinuousConvMode = DISABLE;
  484.   /* Start conversin by software, not an external trigger */
  485.   ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None;
  486.   /* Conversions are 12 bit - put them in the lower 12 bits of the result */
  487.   ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right;
  488.   /* Say how many channels would be used by the sequencer */
  489.   ADC_InitStructure.ADC_NbrOfChannel = 1;

  491.   /* Now do the setup */
  492.   ADC_Init(ADC1, &ADC_InitStructure);
  493.   /* Enable ADC1 */
  494.   ADC_Cmd(ADC1, ENABLE);

  496.   /* Enable ADC1 reset calibaration register */
  497.   ADC_ResetCalibration(ADC1);
  498.   /* Check the end of ADC1 reset calibration register */
  499.   while(ADC_GetResetCalibrationStatus(ADC1));
  500.   /* Start ADC1 calibaration */
  501.   ADC_StartCalibration(ADC1);
  502.   /* Check the end of ADC1 calibration */
  503.   while(ADC_GetCalibrationStatus(ADC1));


  506. }

  508. u16 readADC1(u8 channel)
  509. {
  510.   ADC_RegularChannelConfig(ADC1, channel, 1, ADC_SampleTime_28Cycles5);
  511.   // Start the conversion
  512.   ADC_SoftwareStartConvCmd(ADC1, ENABLE);
  513.   // Wait until conversion completion
  514.   while(ADC_GetFlagStatus(ADC1, ADC_FLAG_EOC) == RESET);
  515.   // Get the conversion value
  516.   return ADC_GetConversionValue(ADC1);
  517. }

  519.         void Delay_ms(uint32_t ms)
  520. {
  521.         volatile uint32_t nCount;
  522.         RCC_ClocksTypeDef RCC_Clocks;
  523.     RCC_GetClocksFreq (&RCC_Clocks);

  525.         nCount=(RCC_Clocks.HCLK_Frequency/10000)*ms;
  526.         for (; nCount!=0; nCount--);
  527. }

  529. void MPU6050_I2C_BufferRead(u8 slaveAddr, u8* pBuffer, u8 readAddr, u16 NumByteToRead)
  530. {


  533. /* While the bus is busy */
  534. while(I2C_GetFlagStatus(MPU6050_I2C, I2C_FLAG_BUSY));

  536. /* Send START condition */
  537. I2C_GenerateSTART(MPU6050_I2C, ENABLE);

  539. /* Test on EV5 and clear it */
  540. while(!I2C_CheckEvent(MPU6050_I2C, I2C_EVENT_MASTER_MODE_SELECT));

  542. /* Send MPU6050 address for write */
  543. I2C_Send7bitAddress(MPU6050_I2C, slaveAddr, I2C_Direction_Transmitter);

  545. /* Test on EV6 and clear it */
  546. while(!I2C_CheckEvent(MPU6050_I2C, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED));

  548. /* Clear EV6 by setting again the PE bit */
  549. I2C_Cmd(MPU6050_I2C, ENABLE);

  551. /* Send the MPU6050's internal address to write to */
  552. I2C_SendData(MPU6050_I2C, readAddr);

  554. /* Test on EV8 and clear it */
  555. while(!I2C_CheckEvent(MPU6050_I2C, I2C_EVENT_MASTER_BYTE_TRANSMITTED));

  557. /* Send STRAT condition a second time */
  558. I2C_GenerateSTART(MPU6050_I2C, ENABLE);

  560. /* Test on EV5 and clear it */
  561. while(!I2C_CheckEvent(MPU6050_I2C, I2C_EVENT_MASTER_MODE_SELECT));

  563. /* Send MPU6050 address for read */
  564. I2C_Send7bitAddress(MPU6050_I2C, slaveAddr, I2C_Direction_Receiver);

  566. /* Test on EV6 and clear it */
  567. while(!I2C_CheckEvent(MPU6050_I2C, I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED));

  569. /* While there is data to be read */
  570. while(NumByteToRead)
  571. {
  572. if(NumByteToRead == 1)
  573. {
  574. /* Disable Acknowledgement */
  575. I2C_AcknowledgeConfig(MPU6050_I2C, DISABLE);

  577. /* Send STOP Condition */
  578. I2C_GenerateSTOP(MPU6050_I2C, ENABLE);
  579. }

  581. /* Test on EV7 and clear it */
  582. if(I2C_CheckEvent(MPU6050_I2C, I2C_EVENT_MASTER_BYTE_RECEIVED))
  583. {
  584. /* Read a byte from the MPU6050 */
  585. *pBuffer = I2C_ReceiveData(MPU6050_I2C);

  587. /* Point to the next location where the byte read will be saved */
  588. pBuffer++;

  590. /* Decrement the read bytes counter */
  591. NumByteToRead--;
  592. }
  593. }

  595. /* Enable Acknowledgement to be ready for another reception */
  596. I2C_AcknowledgeConfig(MPU6050_I2C, ENABLE);


  599. }


  602. void MPU6050_ACC_get(void)
  603. {
  604.         MPU6050_I2C_BufferRead(MPU6050_ADDRESS, ACCread, 0X3B, 6);
  605.                 accADC_ROLL  =  (((ACCread[0]<<8) | ACCread[1]));
  606.                 accADC_x  =(accADC_ROLL);
  607.                 accADC_PITCH =  (((ACCread[2]<<8) | ACCread[3]));
  608.                 accADC_y=(accADC_PITCH);
  609.                 accADC_YAW   =  (((ACCread[4]<<8) | ACCread[5]));
  610.                 accADC_z    =(accADC_YAW);
  611. }       

  613. void MPU6050_Gyro_get(void)
  614. {
  615.                 MPU6050_I2C_BufferRead(MPU6050_ADDRESS, GYROread, 0X43, 6);
  616.                 gyroADC_ROLL  =  (((GYROread[0]<<8) | GYROread[1]));
  617.                 gyroADC_x=((float)gyroADC_ROLL+300)/8000.00;
  618.                 gyroADC_PITCH =  (((GYROread[2]<<8) | GYROread[3]));
  619.                 gyroADC_y=((float)gyroADC_PITCH+320)/8000.00;
  620.                 gyroADC_YAW   =  (((GYROread[4]<<8) | GYROread[5]));
  621.                 gyroADC_z=((float)gyroADC_YAW+350)/8000.00;
  622. }
  623. void WriteToEEPROM(int addressToWrite, int DataToWrite)//Write data to external EEPROM
  624. {
  625.         Delay_ms(5);
  626.   I2C_GenerateSTART(I2C2, ENABLE); // Send I2C2 START condition
  627.   while(!I2C_GetFlagStatus(I2C2, I2C_FLAG_SB));
  628.   I2C_Send7bitAddress(I2C2, EEPROM_ADDRESS, I2C_Direction_Transmitter);// Send EEPROM slave Address for write  
  629.   while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED));// Test on I2C2 EV6 and clear it
  630.   I2C_SendData(I2C2, addressToWrite);// Send I2C2 EEPROM internal address
  631.   while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_BYTE_TRANSMITTED)); // Test on I2C2 EV8 and clear it   
  632.   I2C_SendData(I2C2, DataToWrite);// Send I2C2 EEPROM data   
  633.   while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_BYTE_TRANSMITTED));// Test on I2C2 EV8 and clear it  
  634.   I2C_GenerateSTOP(I2C2, ENABLE);// Send I2C2 STOP Condition
  635.         while(I2C_GetFlagStatus(I2C2, I2C_FLAG_STOPF));
  636.         Delay_ms(5);
  637. }

  639. void ReadFromEEPROM(u8 readAddr)
  640. {


  643. /* While the bus is busy */
  644. while(I2C_GetFlagStatus(I2C2, I2C_FLAG_BUSY));

  646. /* Send START condition */
  647. I2C_GenerateSTART(I2C2, ENABLE);

  649. /* Test on EV5 and clear it */
  650. while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_MODE_SELECT));

  652. /* Send MPU6050 address for write */
  653. I2C_Send7bitAddress(I2C2, EEPROM_ADDRESS, I2C_Direction_Transmitter);

  655. /* Test on EV6 and clear it */
  656. while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED));

  658. /* Clear EV6 by setting again the PE bit */
  659. I2C_Cmd(I2C2, ENABLE);

  661. /* Send the MPU6050's internal address to write to */
  662. I2C_SendData(I2C2, readAddr);

  664. /* Test on EV8 and clear it */
  665. while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_BYTE_TRANSMITTED));

  667. /* Send STRAT condition a second time */
  668. I2C_GenerateSTART(I2C2, ENABLE);

  670. /* Test on EV5 and clear it */
  671. while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_MODE_SELECT));

  673. /* Send eeprom address for read */
  674. I2C_Send7bitAddress(I2C2, EEPROM_ADDRESS, I2C_Direction_Receiver);


  677. /* Test on EV6 and clear it */
  678. while(!I2C_CheckEvent(I2C2 ,I2C_EVENT_MASTER_BYTE_RECEIVED));

  680. /* Read a byte from the 24C02 */
  681. EepromData = I2C_ReceiveData(I2C2);

  683. /* Disable Acknowledgement */
  684. I2C_AcknowledgeConfig(I2C2, DISABLE);

  686. /* Send STOP Condition */
  687. I2C_GenerateSTOP(I2C2, ENABLE);



  691. }

  693. void saveData(void){
  694.         int eeRegCount;       
  695.                
  696.        

  698.         for(eeRegCount=0; eeRegCount<9;eeRegCount++){
  699.                                                         LEDon;
  700.                                                         WriteToEEPROM(eeRegCount, configData[eeRegCount]);
  701.                                                         Delay_ms(5);                                                       
  702.                                                         }
  703.                                                        
  704.                                                         LEDoff;
  705.        
  706. }
  707. void Timer1_Config(void)//ROLL Timer configuration
  708. {       

  710.         //Timer1 Config
  711.        
  712.         //PWM pin config
  713.   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8 | GPIO_Pin_9 | GPIO_Pin_10;
  714.   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
  715.         GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
  716.   GPIO_Init(GPIOA, &GPIO_InitStructure);
  717.        
  718.         //PWM pin config
  719.   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15;
  720.   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
  721.         GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
  722.   GPIO_Init(GPIOB, &GPIO_InitStructure);

  724.         //Time Base configuration
  725.         TIM_TimeBaseInitStructure.TIM_Prescaler = 5; // Period*Prescaler=24'000'000Hz  //2400 1s
  726.   TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up;
  727.   TIM_TimeBaseInitStructure.TIM_Period = 1000; //20000(presc=24)=50hz(servo signal)
  728.   TIM_TimeBaseStructure.TIM_ClockDivision = 0;
  729.   TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
  730.   TIM_TimeBaseInit(TIM1, &TIM_TimeBaseInitStructure);               

  732.   //Configuration in PWM mode
  733.         TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1 ;
  734.         TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
  735.         TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable;
  736.         TIM_OCInitStructure.TIM_Pulse = 0;      
  737.         TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
  738.         TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_High;
  739.         TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
  740.         TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCIdleState_Reset;       
  741.   TIM_OC1Init(TIM1, &TIM_OCInitStructure);       
  742.         TIM_OC2Init(TIM1, &TIM_OCInitStructure);       
  743.         TIM_OC3Init(TIM1, &TIM_OCInitStructure);       


  746.   //Automatic Output enable, Break, dead time and lock configuration
  747.   TIM_BDTRInitStructure.TIM_OSSRState = TIM_OSSRState_Enable;
  748.   TIM_BDTRInitStructure.TIM_OSSIState = TIM_OSSIState_Enable;
  749.   TIM_BDTRInitStructure.TIM_LOCKLevel = TIM_LOCKLevel_1;
  750.   TIM_BDTRInitStructure.TIM_DeadTime = 80;
  751.   TIM_BDTRInitStructure.TIM_Break = TIM_Break_Disable;
  752.   TIM_BDTRInitStructure.TIM_BreakPolarity = TIM_BreakPolarity_High;
  753.   TIM_BDTRInitStructure.TIM_AutomaticOutput = TIM_AutomaticOutput_Enable;
  754.   TIM_BDTRConfig(TIM1, &TIM_BDTRInitStructure);
  755.        
  756.   //TIM1 counter enable
  757.   TIM_Cmd(TIM1, ENABLE);

  759.   //Main Output Enable
  760.   TIM_CtrlPWMOutputs(TIM1, ENABLE);

  762. }       

  764. void Timer8_Config(void)//Pitch Timer configuration
  765. {       

  767.         //Timer8 Config
  768.        
  769.         //PWM pin config
  770.   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7 | GPIO_Pin_8;
  771.   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
  772.         GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
  773.   GPIO_Init(GPIOC, &GPIO_InitStructure);
  774.        
  775.         //PWM pin config
  776.   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_7;
  777.   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
  778.         GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
  779.   GPIO_Init(GPIOA, &GPIO_InitStructure);
  780.        
  781.         //PWM pin config
  782.   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1;
  783.   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
  784.         GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
  785.   GPIO_Init(GPIOB, &GPIO_InitStructure);

  787.         //Time Base configuration
  788.         TIM_TimeBaseInitStructure.TIM_Prescaler = 5; // Period*Prescaler=24'000'000Hz  //2400 1s
  789.   TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up;
  790.   TIM_TimeBaseInitStructure.TIM_Period = 1000; //20000(presc=24)=50hz(servo signal)
  791.   TIM_TimeBaseStructure.TIM_ClockDivision = 0;
  792.   TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
  793.   TIM_TimeBaseInit(TIM8, &TIM_TimeBaseInitStructure);               

  795.   //Configuration in PWM mode
  796.         TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1 ;
  797.         TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
  798.         TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable;
  799.         TIM_OCInitStructure.TIM_Pulse = 0;      
  800.         TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
  801.         TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_High;
  802.         TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
  803.         TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCIdleState_Reset;       
  804.   TIM_OC1Init(TIM8, &TIM_OCInitStructure);       
  805.         TIM_OC2Init(TIM8, &TIM_OCInitStructure);       
  806.         TIM_OC3Init(TIM8, &TIM_OCInitStructure);       


  809.   //Automatic Output enable, Break, dead time and lock configuration
  810.   TIM_BDTRInitStructure.TIM_OSSRState = TIM_OSSRState_Enable;
  811.   TIM_BDTRInitStructure.TIM_OSSIState = TIM_OSSIState_Enable;
  812.   TIM_BDTRInitStructure.TIM_LOCKLevel = TIM_LOCKLevel_1;
  813.   TIM_BDTRInitStructure.TIM_DeadTime = 80;
  814.   TIM_BDTRInitStructure.TIM_Break = TIM_Break_Disable;
  815.   TIM_BDTRInitStructure.TIM_BreakPolarity = TIM_BreakPolarity_High;
  816.   TIM_BDTRInitStructure.TIM_AutomaticOutput = TIM_AutomaticOutput_Enable;
  817.   TIM_BDTRConfig(TIM8, &TIM_BDTRInitStructure);
  818.        
  819.   //TIM8 counter enable
  820.   TIM_Cmd(TIM8, ENABLE);

  822.   //Main Output Enable
  823.   TIM_CtrlPWMOutputs(TIM8, ENABLE);

  825. }       
  826. void Timer2_Config(void)//Main(loop) Timer configuration
  827. {
  828.         //Timer2 Config
  829.         TIM2->PSC = 71;         // Set prescaler (PSC + 1)
  830.         TIM2->ARR = 2000;           // Auto reload value 2000
  831.         TIM2->DIER = TIM_DIER_UIE; // Enable update interrupt (timer level)
  832.         TIM2->CR1 = TIM_CR1_CEN;   // Enable timer
  833.         NVIC_EnableIRQ(TIM2_IRQn); // Enable interrupt from TIM2 (NVIC level)
  834. }

  836. void TIM2_IRQHandler(void)
  837. {
  838. if(TIM2->SR & TIM_SR_UIF) // if UIF flag is set
  839.   {
  840.   TIM2->SR &= ~TIM_SR_UIF; // clear UIF flag  
  841.         stop=1;
  842.   }
  843. }

  845. void Timer5_Config(void)//Pitch Timer configuration
  846. {       
  847.         //PWM pin config
  848.   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_2;
  849.   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
  850.         GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
  851.   GPIO_Init(GPIOA, &GPIO_InitStructure);
  852.        
  853.         //Timer5 Config
  854.                 TIM_TimeBaseInitStructure.TIM_Prescaler = 5; // Period*Prescaler=24'000'000Hz  //2400 1s
  855.   TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up;
  856.   TIM_TimeBaseInitStructure.TIM_Period = 1000; //20000(presc=24)=50hz(servo signal)
  857.   TIM_TimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV4;
  858.   TIM_TimeBaseInit(TIM5, &TIM_TimeBaseInitStructure);       
  859.        
  860.         //PWM Config on Timer5 CH3
  861.         TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1 ;
  862.         TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
  863.         TIM_OCInitStructure.TIM_Pulse = 0;      
  864.         TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low;
  865.   TIM_OC3Init(TIM5, &TIM_OCInitStructure);
  866.         //PWM Config on Timer5 CH2
  867.         TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1 ;
  868.         TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
  869.         TIM_OCInitStructure.TIM_Pulse = 0;      
  870.         TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low;
  871.   TIM_OC2Init(TIM5, &TIM_OCInitStructure);       
  872.         //PWM Config on Timer5 CH1
  873.         TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1 ;
  874.         TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
  875.         TIM_OCInitStructure.TIM_Pulse = 0;      
  876.         TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low;
  877.   TIM_OC1Init(TIM5, &TIM_OCInitStructure);               
  878.        

  880. }       

  882. void Timer4_Config(void)//Pitch Timer configuration
  883. {       
  884.         //PWM pin config
  885.   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8 | GPIO_Pin_7 | GPIO_Pin_6;
  886.   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
  887.         GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
  888.   GPIO_Init(GPIOB, &GPIO_InitStructure);
  889.         //Timer4 Config

  891.         TIM_TimeBaseInitStructure.TIM_Prescaler = 5; // Period*Prescaler=24'000'000Hz  //2400 1s
  892.   TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up;
  893.   TIM_TimeBaseInitStructure.TIM_Period = 1000; //20000(presc=24)=50hz(servo signal)
  894.   TIM_TimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV4;
  895. ……………………

  897. …………限于本文篇幅 余下代碼請從51黑下載附件…………
復(fù)制代碼

所有資料51hei提供下載:
EVVGC _程序.rar (202.4 KB, 下載次數(shù): 88)



作者: bemc    時(shí)間: 2018-10-31 07:56
不錯的程序
作者: Hellow-world    時(shí)間: 2020-7-10 18:01
開源項(xiàng)目的代碼
作者: Highnose    時(shí)間: 2021-2-10 12:09
被控對象是啥呀？ 舵機(jī)？ 直流電機(jī)？還是無刷電機(jī)？  解釋一下就好



歡迎光臨 (http://www.torrancerestoration.com/bbs/) Powered by Discuz! X3.1