API tools faq
paste
Advertisement
Guest User

STM32F4_TIMER_CONFLICT

a guest
Apr 28th, 2015
55
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
ARM 18.45 KB | None | 0 0
raw download clone embed print report
  1. /*----------------------------------------------------------------------------
  2.  * Name:    Blinky.c
  3.  * Purpose: LED Flasher
  4.  * Note(s):
  5.  *----------------------------------------------------------------------------
  6.  * This file is part of the uVision/ARM development tools.
  7.  * This software may only be used under the terms of a valid, current,
  8.  * end user licence from KEIL for a compatible version of KEIL software
  9.  * development tools. Nothing else gives you the right to use this software.
  10.  *
  11.  * This software is supplied "AS IS" without warranties of any kind.
  12.  *
  13.  * Copyright (c) 2011 Keil - An ARM Company. All rights reserved.
  14.  *----------------------------------------------------------------------------*/
  15.  
  16. #include <stdio.h>
  17. #include <math.h>
  18. #include "stm32f4xx.h"
  19. #include "tm_stm32f4_usart.h"
  20. #include "tm_stm32f4_delay.h"
  21. #include "tm_stm32f4_pwm.h"
  22. #include "tm_stm32f4_disco.h"
  23. #include "defines.h"
  24. #include "stm32f4xx_rcc.h"
  25. #include "stm32f4xx_tim.h"
  26. #include "misc.h"
  27. #include "tm_stm32f4_pwm.h"
  28. #include "stm32f4_servo_onur.h"
  29. #include "tm_stm32f4_adc.h"
  30. #include "tm_stm32f4_mpu6050.h"
  31.  
  32. #define HMC5883L_Write 0x3C
  33. #define HMC5883L_Read 0x3D
  34.  
  35. #define ConfigurationRegisterA 0x00
  36. #define ConfigurationRegisterB 0x01
  37. #define ModeRegister 0x02
  38. #define DataRegisterBegin 0x03
  39.  
  40. #define Measurement_Continuous 0x00
  41. #define Measurement_SingleShot 0x01
  42. #define Measurement_Idle 0x03
  43.  
  44. #define ErrorCode_1 "Entered scale was not valid, valid gauss values are: 0.88, 1.3, 1.9, 2.5, 4.0, 4.7, 5.6, 8.1"
  45. #define ErrorCode_1_Num 1
  46.  
  47. #define BUFFER_SIZE 20
  48.  
  49. TM_PWM_TIM_t TIM4_Data;
  50. uint8_t gelenVeri[2] = {0,0},sayac=0,k=0;
  51. uint16_t denetim=0;
  52. uint16_t yeniFrekans=0,yeniDoluluk=0,veri=0;
  53. uint16_t hangiPID[3] = {65001,65002,65003};
  54. uint16_t hangiMotor[7] = {65005,65006,65007,65008,65010,65011,65012};
  55. uint16_t Yuvarlanma_P=0,Yuvarlanma_I=0,Yuvarlanma_D=0,Yunuslama_P=0,Yunuslama_I=0,Yunuslama_D=0,Yonelme_P=0,Yonelme_I=0,Yonelme_D=0;
  56. uint16_t hizMotor_1=0,hizMotor_2=0,hizMotor_3=0,hizMotor_4=0,hizKameraSagSol=0,hizKameraYukariAsagi=0;
  57. double deger=0;
  58. GPIO_InitTypeDef GPIO_Init_Struct;
  59. TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
  60. TM_MPU6050_t MPU6050_Data1;
  61. double accX=0,accY=0,accZ=0,gyroX=0,gyroY=0,gyroZ=0,gy_aciX=0,gy_aciY=0,gy_aciZ=0,C_aciX=0,C_aciY=0,C_aciZ=0;
  62. double yeni_gyroX=0,yeni_gyroY=0,yeni_gyroZ=0,yeni_aciX=0,yeni_aciY=0,yeni_aciZ=0;
  63. double Katsayi=0.95;
  64. double roll=0,pitch=0,yaw=0;
  65. double RAD_TO_DEG = 180/3.1416;
  66. double pi = 3.1416;
  67. double dt = 0.1;
  68. double Q_angleX=0.001,Q_biasX=0.003,R_measureX=0.03,biasX=0,K_aciX=0;
  69. double Px[4]={0,0,0,0};
  70. double Q_angleY=0.001,Q_biasY=0.003,R_measureY=0.03,biasY=0,K_aciY=0;
  71. double Py[4]={0,0,0,0};
  72. double Q_angleZ=0.001,Q_biasZ=0.003,R_measureZ=0.03,biasZ=0,K_aciZ=0;
  73. double Pz[4]={0,0,0,0};
  74. int16_t veriHMC[3];
  75. int16_t sonveriHMC[3];
  76. uint8_t gelenHMC[6];
  77. char hata[30];
  78. double pusulaVeri=0, pusulaVeriLP=0;
  79. double kazanc[3];
  80. double m_Scale;
  81. uint8_t kontrol=0;
  82. double Mx2, My2, Mz2;
  83. double fc=800;
  84. double srate = 10;
  85. double sensorDataCircularSum=0;
  86. int indexBuffer=0;
  87. double circularBuffer[BUFFER_SIZE];
  88. double filteredOutput=0;
  89. double xFiltered=0, yFiltered=0, zFiltered=0, xFilteredOld=0, yFilteredOld=0, zFilteredOld=0;
  90. double alphaAccel = 0;
  91. double xMagnetFiltered=0, yMagnetFiltered=0, zMagnetFiltered=0, xMagnetFilteredOld=0, yMagnetFilteredOld=0, zMagnetFilteredOld=0;
  92. double alphaMagnet=0;
  93. double xMagnetMax=0, yMagnetMax=0, zMagnetMax=0, xMagnetMin=0, yMagnetMin=0, zMagnetMin=0, xMagnetMap=0, yMagnetMap=0, zMagnetMap=0;
  94. double doluluk=0;
  95.  
  96. double smoothSensorReadings(double sensorRawData){
  97.      // We remove the oldest value from the buffer
  98.      sensorDataCircularSum= sensorDataCircularSum - circularBuffer[indexBuffer];
  99.         // The new input from the sensor is placed in the buffer
  100.      circularBuffer[indexBuffer]=sensorRawData;
  101.     // It is also added to the total sum of the last  BUFFER_SIZE readings
  102.     // This method avoids to sum all the elements every time this function is called.
  103.      sensorDataCircularSum+=sensorRawData;
  104.     // We increment the cursor
  105.      indexBuffer++;
  106.      
  107.      if (indexBuffer>=BUFFER_SIZE) indexBuffer=0;// We test if we arrived to the end
  108.     //of the buffer, in which case we start again from index 0
  109.      filteredOutput =(sensorDataCircularSum/BUFFER_SIZE); // The output is the the mean
  110.     //value of the circular buffer.
  111.        
  112.         return filteredOutput;
  113. }
  114.  
  115. void HMC5883L_VeriYaz(uint8_t Kaydedici, uint8_t Veri)
  116. {  
  117.         TM_I2C_Start(I2C1, HMC5883L_Write, I2C_Direction_Transmitter, 0);
  118.         TM_I2C_WriteData(I2C1, Kaydedici);
  119.         TM_I2C_WriteData(I2C1, Veri);
  120.         TM_I2C_Stop(I2C1);
  121. }
  122.  
  123. void HMC5883L_Oku()
  124. {
  125. //      double sonuc=0;
  126.    
  127.         TM_I2C_ReadMulti(I2C1, HMC5883L_Read, DataRegisterBegin, gelenHMC, 6);
  128.         TM_I2C_Stop(I2C1);
  129.    
  130.         veriHMC[0] = (gelenHMC[0] << 8) | gelenHMC[1];
  131.         veriHMC[2] = (gelenHMC[2] << 8) | gelenHMC[3];
  132.         veriHMC[1] = (gelenHMC[4] << 8) | gelenHMC[5];
  133.    
  134. }
  135.  
  136.  
  137. int SetScale(double gauss)
  138. {
  139.     uint8_t regValue = 0x00;
  140.     if(gauss == 0.88)
  141.     {
  142.         regValue = 0x00;
  143.         m_Scale = 0.73;
  144.     }
  145.     else if(gauss == 1.3)
  146.     {
  147.         regValue = 0x01;
  148.         m_Scale = 0.92;
  149.     }
  150.     else if(gauss == 1.9)
  151.     {
  152.         regValue = 0x02;
  153.         m_Scale = 1.22;
  154.     }
  155.     else if(gauss == 2.5)
  156.     {
  157.         regValue = 0x03;
  158.         m_Scale = 1.52;
  159.     }
  160.     else if(gauss == 4.0)
  161.     {
  162.         regValue = 0x04;
  163.         m_Scale = 2.27;
  164.     }
  165.     else if(gauss == 4.7)
  166.     {
  167.         regValue = 0x05;
  168.         m_Scale = 2.56;
  169.     }
  170.     else if(gauss == 5.6)
  171.     {
  172.         regValue = 0x06;
  173.         m_Scale = 3.03;
  174.     }
  175.     else if(gauss == 8.1)
  176.     {
  177.         regValue = 0x07;
  178.         m_Scale = 4.35;
  179.     }
  180.     else
  181.         return ErrorCode_1_Num;
  182.    
  183.     // Setting is in the top 3 bits of the register.
  184.     regValue = regValue << 5;
  185.        
  186.         HMC5883L_VeriYaz(ConfigurationRegisterB,regValue);
  187.    
  188. }
  189.  
  190. void SetMeasurementMode(uint8_t durum)
  191. {
  192.         HMC5883L_VeriYaz(ModeRegister,durum);
  193. }
  194.  
  195. void GetErrorText(int errorCode)
  196. {
  197.     if(ErrorCode_1_Num == 1)
  198.                 sprintf(hata,ErrorCode_1);
  199.     else
  200.                 sprintf(hata,"Error not defined.");
  201. }
  202.  
  203. void setupHMC5883L()
  204. {
  205.       //Setup the HMC5883L, and check for errors
  206.       int error;
  207.       error = SetScale(1.3); //Set the scale of the compass.
  208.         GetErrorText(error);
  209.    
  210.       if(error != 0)
  211.         {
  212. //              if (TM_USB_VCP_GetStatus() == TM_USB_VCP_CONNECTED)
  213. //                      TM_USB_VCP_Puts(hata);
  214.         }
  215.  
  216.       SetMeasurementMode(Measurement_Continuous); // Set the measurement mode to Continuous
  217. }
  218.  
  219. void ReadScaledAxis()
  220. {
  221.      HMC5883L_Oku();
  222.      sonveriHMC[0] = veriHMC[0] * m_Scale;
  223.      sonveriHMC[1] = veriHMC[1] * m_Scale;
  224.      sonveriHMC[2] = veriHMC[2] * m_Scale;
  225. }
  226.  
  227. double getHeading()
  228. {
  229.    //scaled values from compass.
  230.      double heading;
  231.    
  232.      ReadScaledAxis();
  233.      heading = atan2(sonveriHMC[1], sonveriHMC[0]);
  234.    
  235. //   if(heading < 0)
  236. //       heading += 2*pi;
  237. //   else if(heading > 2*pi)
  238. //       heading -= 2*pi;
  239.      
  240.      return heading * RAD_TO_DEG; //radians to degrees
  241. }
  242.  
  243.  
  244. void HMC5883L_Baslat()
  245. {
  246.         TM_I2C_Start(I2C1, HMC5883L_Write, I2C_Direction_Transmitter, 0);
  247.         TM_I2C_WriteData(I2C1, 0x00);
  248.         TM_I2C_WriteData(I2C1, 0x70);
  249.         TM_I2C_Stop(I2C1);  
  250.    
  251.         TM_I2C_Start(I2C1, HMC5883L_Write, I2C_Direction_Transmitter, 0);
  252.         TM_I2C_WriteData(I2C1, 0x01);
  253.         TM_I2C_WriteData(I2C1, 0xA0);
  254.         TM_I2C_Stop(I2C1);
  255.    
  256.         TM_I2C_Start(I2C1, HMC5883L_Write, I2C_Direction_Transmitter, 0);
  257.         TM_I2C_WriteData(I2C1, 0x02);
  258.         TM_I2C_WriteData(I2C1, 0x00);
  259.         TM_I2C_Stop(I2C1);
  260. }
  261.  
  262.  
  263. void aciAlX(double yeniAciX)
  264. {
  265.         K_aciX = yeniAciX;
  266. }
  267.  
  268. void aciAlY(double yeniAciY)
  269. {
  270.         K_aciY = yeniAciY;
  271. }
  272.  
  273. void aciAlZ(double yeniAciZ)
  274. {
  275.         K_aciZ = yeniAciZ;
  276. }
  277.  
  278. double KalmanX_Hesapla(double gelenAciX,double yeniDegisimX)
  279. {
  280.       double Sx=0,Kx[2]={0,0},yx=0;
  281.         double P00_tempX=0,P01_tempX=0;
  282.        
  283.         yeni_gyroX = yeniDegisimX-biasX;
  284.         K_aciX += dt*yeni_gyroX;
  285.    
  286.         Px[0] += dt * (dt*Px[3]-Px[1]-Px[2]+Q_angleX);
  287.         Px[1] -= dt * Px[3];
  288.         Px[2] -= dt * Px[3];
  289.         Px[3] += Q_biasX * dt;
  290.    
  291.         Sx = Px[0]+R_measureX;
  292.         Kx[0] = Px[0] / Sx;
  293.         Kx[1] = Px[2] / Sx;
  294.        
  295.         yx = gelenAciX-K_aciX;
  296.         K_aciX += Kx[0] * yx;
  297.         biasX += Kx[1] * yx;
  298.    
  299.         P00_tempX = Px[0];
  300.         P01_tempX = Px[1];
  301.    
  302.         Px[0] -= Kx[0] * P00_tempX;
  303.         Px[1] -= Kx[0] * P01_tempX;
  304.         Px[2] -= Kx[1] * P00_tempX;
  305.         Px[3] -= Kx[1] * P01_tempX;
  306.        
  307.         return K_aciX;
  308. }
  309.  
  310.  
  311.  
  312. double KalmanY_Hesapla(double gelenAciY,double yeniDegisimY)
  313. {
  314.         double Sy=0,Ky[2]={0,0},yy=0;
  315.         double P00_tempY=0,P01_tempY=0;
  316.        
  317.         yeni_gyroY = yeniDegisimY-biasY;
  318.         K_aciY += dt*yeni_gyroY;
  319.    
  320.         Py[0] += dt * (dt*Py[3]-Py[1]-Py[2]+Q_angleY);
  321.         Py[1] -= dt * Py[3];
  322.         Py[2] -= dt * Py[3];
  323.         Py[3] += Q_biasY * dt;
  324.    
  325.         Sy = Py[0]+R_measureY;
  326.         Ky[0] = Py[0] / Sy;
  327.         Ky[1] = Py[2] / Sy;
  328.        
  329.         yy = gelenAciY-K_aciY;
  330.         K_aciY += Ky[0] * yy;
  331.         biasY += Ky[1] * yy;
  332.    
  333.         P00_tempY = Py[0];
  334.         P01_tempY = Py[1];
  335.    
  336.         Py[0] -= Ky[0] * P00_tempY;
  337.         Py[1] -= Ky[0] * P01_tempY;
  338.         Py[2] -= Ky[1] * P00_tempY;
  339.         Py[3] -= Ky[1] * P01_tempY;
  340.        
  341.         return K_aciY;
  342.    
  343. }
  344.  
  345. double KalmanZ_Hesapla(double gelenAciZ,double yeniDegisimZ)
  346. {
  347.         double Sz=0,Kz[2]={0,0},yz=0;
  348.         double P00_tempZ=0,P01_tempZ=0;
  349.        
  350.         yeni_gyroZ = yeniDegisimZ-biasZ;
  351.         K_aciZ += dt*yeni_gyroZ;
  352.    
  353.         Pz[0] += dt * (dt*Pz[3]-Pz[1]-Pz[2]+Q_angleZ);
  354.         Pz[1] -= dt * Pz[3];
  355.         Pz[2] -= dt * Pz[3];
  356.         Pz[3] += Q_biasZ * dt;
  357.    
  358.         Sz = Pz[0]+R_measureZ;
  359.         Kz[0] = Pz[0] / Sz;
  360.         Kz[1] = Pz[2] / Sz;
  361.        
  362.         yz = gelenAciZ-K_aciZ;
  363.         K_aciZ += Kz[0] * yz;
  364.         biasZ += Kz[1] * yz;
  365.    
  366.         P00_tempZ = Pz[0];
  367.         P01_tempZ = Pz[1];
  368.    
  369.         Pz[0] -= Kz[0] * P00_tempZ;
  370.         Pz[1] -= Kz[0] * P01_tempZ;
  371.         Pz[2] -= Kz[1] * P00_tempZ;
  372.         Pz[3] -= Kz[1] * P01_tempZ;
  373.        
  374.         return K_aciZ;
  375. }
  376.  
  377.  
  378. void veriOkuMPU()
  379. {
  380.         TM_MPU6050_ReadAll(&MPU6050_Data1);
  381.    
  382.         accX =  MPU6050_Data1.Accelerometer_X / 182.0;
  383.         accY =  MPU6050_Data1.Accelerometer_Y / 182.0;
  384.         accZ =  MPU6050_Data1.Accelerometer_Z / 182.0;
  385.                                
  386.         gyroX =  MPU6050_Data1.Gyroscope_X / 131.0;
  387.         gyroY =  MPU6050_Data1.Gyroscope_Y / 131.0;
  388.         gyroZ =  MPU6050_Data1.Gyroscope_Z / 131.0;
  389.    
  390.         roll  = atan2(accY, accZ) * RAD_TO_DEG;
  391.         pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
  392. }
  393.  
  394. void TM_USART1_ReceiveHandler(uint8_t alinanVeri)
  395. {  
  396.         gelenVeri[sayac] = alinanVeri; //Karsidan gelen veriler iki bayt oldugu icin iki adet veri bekliyoruz
  397.         sayac++;
  398.        
  399.         if(sayac==2) //Iki adet bayt gelmisse
  400.         {
  401.                 sayac=0; //Gelecek islemler icin sayac sifirlaniyor
  402.            
  403.                 veri = ( gelenVeri[1] << 8 ) + gelenVeri[0]; //16 bitlik degerimizi aliyoruz
  404.                 if((veri==hangiPID[0]) || (veri==hangiPID[1]) || (veri==hangiPID[2]) || (veri==hangiMotor[0]) || (veri==hangiMotor[1]) || (veri==hangiMotor[2]) || (veri==hangiMotor[3]) || (veri==hangiMotor[4]) || (veri==hangiMotor[5]) || (veri==hangiMotor[6])) //Gelen bilgi hangi islem bilgisi verisi mi kontrol ediliyor
  405.                         denetim = veri;
  406.                 else
  407.                 {
  408.                         if(denetim == hangiPID[0]) //Eger pwm islemine ait veriyse
  409.                         {
  410.                                 if(k==0)
  411.                                 {
  412.                                         k=1;
  413.                                         Yuvarlanma_P = (gelenVeri[1] << 8) + gelenVeri[0];
  414.                                         TM_USART_Putc(USART1,gelenVeri[0]); //Gelen verileri hata denetimi icin tekrar yolla
  415.                                         TM_USART_Putc(USART1,gelenVeri[1]);
  416.                                 }
  417.                                 else if(k==1)
  418.                                 {
  419.                                         k=2;
  420.                                         Yuvarlanma_I = (gelenVeri[1] << 8) + gelenVeri[0];
  421.                                         TM_USART_Putc(USART1,gelenVeri[0]); //Gelen verileri hata denetimi icin tekrar yolla
  422.                                         TM_USART_Putc(USART1,gelenVeri[1]);
  423.                                 }
  424.                                 else if(k==2)
  425.                                 {
  426.                                         k=0;
  427.                                         Yuvarlanma_D = (gelenVeri[1] << 8) + gelenVeri[0];
  428.                                         TM_USART_Putc(USART1,gelenVeri[0]); //Gelen verileri hata denetimi icin tekrar yolla
  429.                                         TM_USART_Putc(USART1,gelenVeri[1]);
  430.                                 }  
  431.                         }
  432.                         else if(denetim == hangiPID[1]) //Eger pwm islemine ait veriyse
  433.                         {
  434.                                 if(k==0)
  435.                                 {
  436.                                         k=1;
  437.                                         Yunuslama_P = (gelenVeri[1] << 8) + gelenVeri[0];
  438.                                         TM_USART_Putc(USART1,gelenVeri[0]); //Gelen verileri hata denetimi icin tekrar yolla
  439.                                         TM_USART_Putc(USART1,gelenVeri[1]);
  440.                                 }
  441.                                 else if(k==1)
  442.                                 {
  443.                                         k=2;
  444.                                         Yunuslama_I = (gelenVeri[1] << 8) + gelenVeri[0];
  445.                                         TM_USART_Putc(USART1,gelenVeri[0]); //Gelen verileri hata denetimi icin tekrar yolla
  446.                                         TM_USART_Putc(USART1,gelenVeri[1]);
  447.                                 }
  448.                                 else if(k==2)
  449.                                 {
  450.                                         k=0;
  451.                                         Yunuslama_D = (gelenVeri[1] << 8) + gelenVeri[0];
  452.                                         TM_USART_Putc(USART1,gelenVeri[0]); //Gelen verileri hata denetimi icin tekrar yolla
  453.                                         TM_USART_Putc(USART1,gelenVeri[1]);
  454.                                 }  
  455.                         }
  456.                         else if(denetim == hangiPID[2]) //Eger pwm islemine ait veriyse
  457.                         {
  458.                                 if(k==0)
  459.                                 {
  460.                                         k=1;
  461.                                         Yonelme_P = (gelenVeri[1] << 8) + gelenVeri[0];
  462.                                         TM_USART_Putc(USART1,gelenVeri[0]); //Gelen verileri hata denetimi icin tekrar yolla
  463.                                         TM_USART_Putc(USART1,gelenVeri[1]);
  464.                                 }
  465.                                 else if(k==1)
  466.                                 {
  467.                                         k=2;
  468.                                         Yonelme_I = (gelenVeri[1] << 8) + gelenVeri[0];
  469.                                         TM_USART_Putc(USART1,gelenVeri[0]); //Gelen verileri hata denetimi icin tekrar yolla
  470.                                         TM_USART_Putc(USART1,gelenVeri[1]);
  471.                                 }
  472.                                 else if(k==2)
  473.                                 {
  474.                                         k=0;
  475.                                         Yonelme_D = (gelenVeri[1] << 8) + gelenVeri[0];
  476.                                         TM_USART_Putc(USART1,gelenVeri[0]); //Gelen verileri hata denetimi icin tekrar yolla
  477.                                         TM_USART_Putc(USART1,gelenVeri[1]);
  478.                                 }  
  479.                         }
  480.                         else if(denetim==hangiMotor[4])
  481.                         {
  482.                                 hizMotor_1 = (gelenVeri[1] << 8) + gelenVeri[0];
  483.                                 hizMotor_2 = hizMotor_1;
  484.                                 hizMotor_3 = hizMotor_1;
  485.                                 hizMotor_4 = hizMotor_1;
  486.                                 TM_USART_Putc(USART1,gelenVeri[0]); //Gelen verileri hata denetimi icin tekrar yolla
  487.                                 TM_USART_Putc(USART1,gelenVeri[1]);
  488.                         }
  489.                         else if(denetim==hangiMotor[0])
  490.                         {
  491.                                 hizMotor_1 = (gelenVeri[1] << 8) + gelenVeri[0];
  492.                                 TM_USART_Putc(USART1,gelenVeri[0]); //Gelen verileri hata denetimi icin tekrar yolla
  493.                                 TM_USART_Putc(USART1,gelenVeri[1]);
  494.                         }
  495.                         else if(denetim==hangiMotor[1])
  496.                         {
  497.                                 hizMotor_2 = (gelenVeri[1] << 8) + gelenVeri[0];
  498.                                 TM_USART_Putc(USART1,gelenVeri[0]); //Gelen verileri hata denetimi icin tekrar yolla
  499.                                 TM_USART_Putc(USART1,gelenVeri[1]);
  500.                         }
  501.                         else if(denetim==hangiMotor[2])
  502.                         {
  503.                                 hizMotor_3 = (gelenVeri[1] << 8) + gelenVeri[0];
  504.                                 TM_USART_Putc(USART1,gelenVeri[0]); //Gelen verileri hata denetimi icin tekrar yolla
  505.                                 TM_USART_Putc(USART1,gelenVeri[1]);
  506.                         }
  507.                         else if(denetim==hangiMotor[3])
  508.                         {
  509.                                 hizMotor_4 = (gelenVeri[1] << 8) + gelenVeri[0];
  510.                                 TM_USART_Putc(USART1,gelenVeri[0]); //Gelen verileri hata denetimi icin tekrar yolla
  511.                                 TM_USART_Putc(USART1,gelenVeri[1]);
  512.                         }
  513.                         else if(denetim==hangiMotor[5])
  514.                         {
  515.                                 hizKameraSagSol = (gelenVeri[1] << 8) + gelenVeri[0];
  516.                                 TM_USART_Putc(USART1,gelenVeri[0]); //Gelen verileri hata denetimi icin tekrar yolla
  517.                                 TM_USART_Putc(USART1,gelenVeri[1]);
  518.                         }
  519.                         else if(denetim==hangiMotor[6])
  520.                         {
  521.                                 hizKameraYukariAsagi = (gelenVeri[1] << 8) + gelenVeri[0];
  522.                                 TM_USART_Putc(USART1,gelenVeri[0]); //Gelen verileri hata denetimi icin tekrar yolla
  523.                                 TM_USART_Putc(USART1,gelenVeri[1]);
  524.                         }
  525.                 }
  526.         }
  527. }
  528.  
  529. void TIM7_IRQHandler(void)
  530. {
  531.         if (TIM_GetITStatus(TIM7, TIM_IT_Update) != RESET)
  532.         {
  533.                 veriOkuMPU();
  534.                 pusulaVeri = getHeading();
  535.                 pusulaVeriLP = smoothSensorReadings(pusulaVeri);
  536.                
  537.                 if ((roll < -90 && K_aciX > 90) || (roll > 90 && K_aciX < -90))
  538.                 {
  539.                         aciAlX(roll);
  540.                         C_aciX = roll;
  541.                         K_aciX = roll;
  542.                         gy_aciX = roll;
  543.                 }
  544.                 else
  545.                         K_aciX = KalmanX_Hesapla(roll,gyroX); // Calculate the angle using a Kalman filter
  546.  
  547.                 if (K_aciX > 90 || K_aciX < -90)
  548.                         gyroY = -gyroY; // Invert rate, so it fits the restriced accelerometer reading
  549.                                                
  550.                 K_aciY = KalmanY_Hesapla(pitch,gyroY);
  551.                 K_aciZ = KalmanZ_Hesapla(pusulaVeri,gyroZ);
  552.                                                
  553.                 gy_aciX = roll + gyroX * dt; // Calculate gyro angle without any filter
  554.                 gy_aciY = pitch + gyroY * dt;
  555.                 gy_aciZ = pusulaVeri + gyroZ * dt;
  556.                                                
  557.                 C_aciX = Katsayi*(C_aciX+gyroX*dt)+(1-Katsayi)*roll;
  558.                 C_aciY = Katsayi*(C_aciY+gyroY*dt)+(1-Katsayi)*pitch;
  559.                 C_aciZ = Katsayi*(C_aciZ+gyroZ*dt)+(1-Katsayi)*pusulaVeri;
  560.         }
  561.        
  562. }
  563.  
  564. void Zamanlayici_Baslat(void) {
  565.     TIM_TimeBaseInitTypeDef TIM_BaseStruct;
  566.         NVIC_InitTypeDef NVIC_InitStructure;
  567.    
  568.         NVIC_InitStructure.NVIC_IRQChannel = TIM7_IRQn;
  569.         NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 3;
  570.         NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
  571.         NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
  572.         NVIC_Init(&NVIC_InitStructure);
  573.    
  574.     RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM7, ENABLE);
  575.     TIM_BaseStruct.TIM_Prescaler = 8399;
  576.     TIM_BaseStruct.TIM_CounterMode = TIM_CounterMode_Up;
  577.  
  578.     TIM_BaseStruct.TIM_Period = 999; /* 100 ms */
  579.     TIM_BaseStruct.TIM_ClockDivision = TIM_CKD_DIV1;
  580.     TIM_BaseStruct.TIM_RepetitionCounter = 0;
  581.         TIM_TimeBaseInit(TIM7, &TIM_BaseStruct);
  582.  
  583.         TIM_ITConfig(TIM7, TIM_IT_Update, ENABLE);
  584.        
  585.     TIM_Cmd(TIM7, ENABLE);
  586. }
  587.  
  588. int main (void) {
  589.        
  590. //      TM_PWM_TIM_t TIM3_Data;
  591. //      double dolulukOrani = 0;
  592. //      char str[15];
  593.    
  594.        
  595.            
  596.         /* Servo structs */
  597.     TM_SERVO_t Servo1;
  598.    
  599.     /* Initialize system */
  600.     SystemInit();
  601.    
  602.         /* Initialize USART1, 9600baud, TX: PB6 */
  603.     TM_USART_Init(USART1, TM_USART_PinsPack_2, 9600);
  604.    
  605.      /* Initialize ADC0 on channel 1, this is pin PA1 */
  606.     TM_ADC_Init(ADC1, ADC_Channel_1);
  607.    
  608.     /* Initialize delay */
  609.     TM_DELAY_Init();
  610.    
  611.         /* Initialize servo 1, TIM3, Channel 1, Pinspack 2 = PB4 */
  612.     TM_SERVO_Init(&Servo1, TIM3, TM_PWM_Channel_1, TM_PWM_PinsPack_2);
  613.        
  614.     /* Set PWM to 1Hz frequency on timer TIM3 */
  615.     TM_PWM_InitTimer(TIM4, &TIM4_Data, 1);
  616.    
  617.     /* Initialize PWM on TIM4, Channel 1 and PinsPack 2 = PD12 */
  618.     TM_PWM_InitChannel(&TIM4_Data, TM_PWM_Channel_1, TM_PWM_PinsPack_2);
  619.        
  620.         //      TM_USB_VCP_Init();
  621. //      - TM_I2C_Pinspack_2:
  622. //      - I2C1: SCL: PB8, SDA: PB9
  623.         TM_I2C_Init(I2C1, TM_I2C_PinsPack_2, 400000);
  624.        
  625.         TM_MPU6050_Init(&MPU6050_Data1, TM_MPU6050_Device_1, TM_MPU6050_Accelerometer_8G, TM_MPU6050_Gyroscope_250s);
  626.    
  627.         Zamanlayici_Baslat();
  628.        
  629. //      TM_SERVO_SetDegrees(&Servo1, 200);
  630. //      Delayms(2000);
  631. //      TM_SERVO_SetDegrees(&Servo1, 0);
  632. //      Delayms(2000);
  633.        
  634.         NVIC_PriorityGroupConfig(NVIC_PriorityGroup_1);
  635.        
  636.     while (1)
  637.         {
  638.                
  639.                
  640.            
  641.     }
  642. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!