odometriKRAI.cpp

00001 /***************************************************************************
00002  * Title      : Library untuk odometri base
00003  * Name       : odometriKrai.cpp
00004  * Version    : 1.0
00005  * Author     : Oktavianus Irvan Sitanggang EL 18
00006  * Date       : 17 Desember 2019
00007  * Description:
00008  *
00009  * Library ini digunakan untuk melakukan perhitungan posisi robot
00010  * 
00011  * 
00012  ***************************************************************************/
00013 
00014 /******************************* library ***********************************/
00015 
00016 #include "odometriKRAI.h"
00017 #include "mbed.h"
00018 
00019 
00020 
00021 #ifndef PHI
00022 #define PHI  3.14159265359
00023 #endif
00024 
00025 #ifndef D_RODA
00026 #define D_RODA 0.06
00027 #endif
00028 /*************************** inisiasi class *******************************/
00029 odometriKRAI::odometriKRAI(TIM_TypeDef *_TIMEncX, TIM_TypeDef *_TIMEncY, PinName SDA, PinName SCL)
00030 : encX(_TIMEncX), encY(_TIMEncY), kompass(SDA, SCL, 0xC0) {
00031     position.x = 0;      // initiate all Value
00032     position.y = 0;      
00033     position.teta = 0;  
00034     kompass.compassResetOffsetValue();
00035     }
00036 
00037 
00038 /*************************** definisi fungsi ******************************/
00039 
00040 /* update position from base */
00041 void odometriKRAI::updatePosition(void){
00042     float xTemp = encX.getPulses(1);                            /* butuh 1.5us */
00043     float yTemp = encY.getPulses(1);                            /* butuh 1.5us */
00044 
00045     kompass.compassUpdateValue();                               /* ??? */
00046     position.teta = kompass.compassValue();
00047     position.x +=  (xTemp*PHI*D_RODA/4000)*cos(position.teta) + (yTemp*PHI*D_RODA/4000)*-sin(position.teta);        /* butuh 4.5 us */
00048     position.y +=  (xTemp*PHI*D_RODA/4000)*sin(position.teta) + (yTemp*PHI*D_RODA/4000)*cos(position.teta);         /* butuh 4.5 us */
00049 
00050 }
00051 
00052 /* to reset all the position */
00053 void odometriKRAI::resetOdom(void){
00054     position.x = 0;      // initiate all Value
00055     position.y = 0;      
00056     position.teta = 0;
00057     
00058     kompass.compassResetOffsetValue();
00059 }
00060 
00061 
00062