Diff: odometriKRAI/odometriKRAI.cpp

Revision:

0:49e87dcad299

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/odometriKRAI/odometriKRAI.cpp	Fri Jun 18 11:04:08 2021 +0000
@@ -0,0 +1,62 @@
+/***************************************************************************
+ * Title      : Library untuk odometri base
+ * Name       : odometriKrai.cpp
+ * Version    : 1.0
+ * Author     : Oktavianus Irvan Sitanggang EL 18
+ * Date       : 17 Desember 2019
+ * Description:
+ *
+ * Library ini digunakan untuk melakukan perhitungan posisi robot
+ * 
+ * 
+ ***************************************************************************/
+
+/******************************* library ***********************************/
+
+#include "odometriKRAI.h"
+#include "mbed.h"
+
+
+
+#ifndef PHI
+#define PHI  3.14159265359
+#endif
+
+#ifndef D_RODA
+#define D_RODA 0.06
+#endif
+/*************************** inisiasi class *******************************/
+odometriKRAI::odometriKRAI(TIM_TypeDef *_TIMEncX, TIM_TypeDef *_TIMEncY, PinName SDA, PinName SCL)
+: encX(_TIMEncX), encY(_TIMEncY), kompass(SDA, SCL, 0xC0) {
+    position.x = 0;      // initiate all Value
+    position.y = 0;      
+    position.teta = 0;  
+    kompass.compassResetOffsetValue();
+    }
+
+
+/*************************** definisi fungsi ******************************/
+
+/* update position from base */
+void odometriKRAI::updatePosition(void){
+    float xTemp = encX.getPulses(1);                            /* butuh 1.5us */
+    float yTemp = encY.getPulses(1);                            /* butuh 1.5us */
+
+    kompass.compassUpdateValue();                               /* ??? */
+    position.teta = kompass.compassValue();
+    position.x +=  (xTemp*PHI*D_RODA/4000)*cos(position.teta) + (yTemp*PHI*D_RODA/4000)*-sin(position.teta);        /* butuh 4.5 us */
+    position.y +=  (xTemp*PHI*D_RODA/4000)*sin(position.teta) + (yTemp*PHI*D_RODA/4000)*cos(position.teta);         /* butuh 4.5 us */
+
+}
+
+/* to reset all the position */
+void odometriKRAI::resetOdom(void){
+    position.x = 0;      // initiate all Value
+    position.y = 0;      
+    position.teta = 0;
+    
+    kompass.compassResetOffsetValue();
+}
+
+
+