Diff: src/myUtil.hpp

Revision:

0:4ff8aeb3e4d1

Child:

2:0de4854743f7

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/myUtil.hpp	Fri Apr 02 05:24:49 2021 +0000
@@ -0,0 +1,70 @@
+#ifndef ZETA_STM_KINETIC_MYUTIL_H_
+#define ZETA_STM_KINETIC_MYUTIL_H_
+#include "instanceHeader.hpp"
+#include "rosHeader.hpp" 
+extern ros::NodeHandle nh;
+extern MPU9250_SPI mpu;
+namespace myUtil {
+    void mydelay(int count) {
+        ThisThread::sleep_for(count);    // threads should yield by sleep
+    }
+    
+    void stop() {
+        #if (NO_ROS)
+        pc.printf("I stop here!\n\r");
+        #endif
+        while(true) {;}
+    }
+    
+        #if !(NO_ROS)
+
+    
+    
+    void myLog(const char* myLog) {
+        nh.loginfo(myLog);
+    }
+    // Template T_T
+    void myLog(const char* varName, uint16_t val) {
+        char temp[64] = {'\0',};
+        sprintf(temp,"var: %s\tvalue: %u",varName,val);
+        nh.loginfo(temp);
+    }
+    void myLog(const char* varName, int val) {
+        char temp[32] = {'\0',};
+        sprintf(temp,"var: %s\tvalue: %d",varName,val);
+        nh.loginfo(temp);
+    }
+    void myLog(const char* varName, float val) {
+        char temp[32] = {'\0',};
+        sprintf(temp,"var: %s\tvalue: %f",varName,val);
+        nh.loginfo(temp);
+    }
+        #endif
+    void calibrationProcess() {
+        nh.loginfo("IMU calibration is running... please don't move the robot");
+        mpu.calibrateGyro();   
+      //mpu.calibrateMag();
+    #if (NO_ROS)
+        pc.printf("Calibration Completed !!!!!!!!\n");  
+    #else
+        nh.loginfo("IMU calibration Completed!!!!!!!!");
+    #endif
+        //while(true) {;}   //stop here
+    }
+    
+    void applyCalbratedValue() {
+      //Vect3 gBias ={ 1.881,   -2.630  ,   0.226};
+      //mpu.setGyroBias(gBias); 
+      //Vect3 mBias ={12.885,  314.122  , -594.508};
+      //mpu.setMagBias(mBias);
+      //Vect3 mScale={ 1.852,    1.176  ,   0.621}; 
+      //mpu.setMagScale(mScale);
+      ;
+    }
+    
+    void set_msg(char* msg, const char* contents, size_t length) {
+        memset(msg, '\0', SEQ_STATE_MSG_SIZE);
+        memcpy(msg, contents, length);
+    }
+}
+#endif
\ No newline at end of file