#include "../Include/myLcdText.h" #include "../Include/myDefinition.h" #include "../Include/myGlobal_Vars.h" #include "../Include/MPU6050.h" #include "../Include/myDelay.h" #include "../Include/myADC.h" #include "board-st_discovery.h" #include "../Include/myIMUDMP.h" #include "../Include/effirof4_init.h" //#include "../Include/myKalman.h" //-------------------------Rotary Encoder------------------------- unsigned short Get_Gticks(void) {return(Gticks);} //speeds count volatile int16_t leftCount; volatile int16_t rightCount; volatile int16_t fwdCount; volatile int16_t rotCount; volatile int16_t rpmKaCount; volatile int16_t rpmKiCount; //distances - jarak yg ditempuh volatile int32_t rpmKaTotal; volatile int32_t rpmKiTotal; volatile int32_t rpmTotal; volatile int32_t leftTotal; volatile int32_t rightTotal; volatile int32_t fwdTotal; //Jauhnya maju volatile int32_t rotTotal; //heading volatile int32_t rotkaAwal; volatile int32_t rotkiAwal; volatile int32_t fwdAwal; volatile int32_t rotAwal; // local variables volatile int16_t oldLeftEncoder; volatile int16_t oldRightEncoder; volatile int16_t leftEncoder; volatile int16_t rightEncoder; // unsigned int duty_1,duty_2; char sstring[20]; void encodersRead (void) { oldLeftEncoder = leftEncoder; leftEncoder = TIM_GetCounter (ENCL_TIMER) ; oldRightEncoder = rightEncoder; rightEncoder = TIM_GetCounter (ENCR_TIMER) ; leftCount = leftEncoder - oldLeftEncoder; rightCount = rightEncoder - oldRightEncoder; fwdCount = leftCount + rightCount; rotCount = - (leftCount - rightCount); fwdTotal += fwdCount; rotTotal += rotCount; leftTotal += leftCount; rightTotal += rightCount; rpmKaCount +=rightCount; rpmKiCount +=leftCount; } void encodersReset (void) { __disable_irq(); leftEncoder = 0; rightEncoder = 0; oldLeftEncoder = 0; oldRightEncoder = 0; leftTotal = 0; rightTotal = 0; fwdTotal = 0; rotTotal = 0; rpmTotal=0; rpmKaTotal=0; rpmKiTotal=0; TIM_SetCounter (ENCL_TIMER, 0); TIM_SetCounter (ENCR_TIMER, 0); encodersRead(); __enable_irq(); } //every 1ms void TIM4_IRQHandler(void) { if (TIM_GetFlagStatus(TIM4, TIM_FLAG_Update)) { TIM_ClearITPendingBit(TIM4, TIM_FLAG_Update); ///* //pulsa=(float)frekuensi*60/20;//formula perhitungan kecepatan //frekuensi=0; //rpmKaTotal=rpmKaCount*120/1100; //rpmKiTotal=rpmKiCount*120/1100; //ini 120 = 1 ms //dari 2hz ke 42hz berarti dikali 21 dari nilai awal di atas ///* gDelta+=(float)0.001; TimingDelay_Decrement(); TimeStamp_Increment(); encodersRead (); rpmKaTotal=(float)(rpmKaCount*60*1000)/(560); rpmKiTotal=(float)(rpmKiCount*60*1000)/(560); rpmTotal=(float)(rpmKaTotal+rpmKiTotal)/2; rpmKaCount=0;rpmKiCount=0; //*/ // PulsaMic=FrekMic*21; // FrekMic=0; //*/ } } void TIM5_IRQHandler(void) { if (TIM_GetFlagStatus(TIM5, TIM_FLAG_Update)) { TIM_ClearITPendingBit(TIM5, TIM_FLAG_Update); if(PROGRAM_STATE==1){ /* //loopDMP(); rapid16fi(sstring,0,Pitch); Monitor6String(sstring); Monitor6(' '); rapid16fi(sstring,0,Roll); Monitor6String(sstring); Monitor6('\r'); Monitor6('\n'); if(fTxFree==1){ fTxFree=0; USART_DMA_Config(Roll); } if(fTxFreeX==1){ fTxFreeX=0; USART_DMA_ConfigX(Pitch); }*/ } } } void KirimByte(unsigned char x) { UART4->DR = x; while(!(UART4->SR & USART_FLAG_TXE)); UART4->SR &= ~USART_FLAG_TXE; //USART_SendData(USART2, x); } void KirimIMU(unsigned int Nilai) { KirimByte((Nilai%100)/10 + 0x30); KirimByte(Nilai%10 + 0x30); } void KirimRate(unsigned int Nilai) { // KirimByte((Nilai%10000)/1000 + 0x30); KirimByte((Nilai%1000)/100 + 0x30); KirimByte((Nilai%100)/10 + 0x30); KirimByte(Nilai%10 + 0x30); } void Brake(void) { ocrA_Ki = 0; ocrA_Ka = 0; ocrB_Ki = 0; ocrB_Ka = 0; } void Maju(unsigned short int Ki, unsigned short int Ka) { if(Ki > 2999) Ki = 2999; if(Ka > 2999) Ka = 2999; ocrA_Ki = 0; ocrA_Ka = Ka; ocrB_Ki = Ki; ocrB_Ka = 0; } void Mundur(unsigned short int Ki, unsigned short int Ka) { ocrA_Ki = Ki; ocrA_Ka = 0; ocrB_Ki = 0; ocrB_Ka = Ka; } void PutarKa(unsigned short int Ki, unsigned short int Ka) { ocrA_Ki = 0; ocrA_Ka = 0; ocrB_Ki = Ki; ocrB_Ka = Ka; } void PutarKi(unsigned short int Ki, unsigned short int Ka) { ocrA_Ki = Ki; ocrA_Ka = Ka; ocrB_Ki = 0; ocrB_Ka = 0; }