semin ahn / Mbed OS zeta_stm_kinetic

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   BufferedSerial

src/myUtil.hpp@2:0de4854743f7, 2021-06-10 (annotated)

Committer:
_seminahn
Date:
Thu Jun 10 01:23:00 2021 +0000
Revision:
2:0de4854743f7
Parent:
0:4ff8aeb3e4d1
Child:
3:a4677501ae87
v1.1.0 change ssr logic

Who changed what in which revision?

UserRevisionLine numberNew contents of line
_seminahn 0:4ff8aeb3e4d1 1 #ifndef ZETA_STM_KINETIC_MYUTIL_H_
_seminahn 0:4ff8aeb3e4d1 2 #define ZETA_STM_KINETIC_MYUTIL_H_
_seminahn 0:4ff8aeb3e4d1 3 #include "instanceHeader.hpp"
_seminahn 0:4ff8aeb3e4d1 4 #include "rosHeader.hpp"
_seminahn 0:4ff8aeb3e4d1 5 extern ros::NodeHandle nh;
_seminahn 0:4ff8aeb3e4d1 6 extern MPU9250_SPI mpu;
_seminahn 0:4ff8aeb3e4d1 7 namespace myUtil {
_seminahn 0:4ff8aeb3e4d1 8 void mydelay(int count) {
_seminahn 0:4ff8aeb3e4d1 9 ThisThread::sleep_for(count); // threads should yield by sleep
_seminahn 0:4ff8aeb3e4d1 10 }
_seminahn 0:4ff8aeb3e4d1 11
_seminahn 0:4ff8aeb3e4d1 12 void stop() {
_seminahn 0:4ff8aeb3e4d1 13 #if (NO_ROS)
_seminahn 0:4ff8aeb3e4d1 14 pc.printf("I stop here!\n\r");
_seminahn 0:4ff8aeb3e4d1 15 #endif
_seminahn 0:4ff8aeb3e4d1 16 while(true) {;}
_seminahn 0:4ff8aeb3e4d1 17 }
_seminahn 0:4ff8aeb3e4d1 18
_seminahn 0:4ff8aeb3e4d1 19 #if !(NO_ROS)
_seminahn 0:4ff8aeb3e4d1 20
_seminahn 0:4ff8aeb3e4d1 21
_seminahn 0:4ff8aeb3e4d1 22
_seminahn 0:4ff8aeb3e4d1 23 void myLog(const char* myLog) {
_seminahn 0:4ff8aeb3e4d1 24 nh.loginfo(myLog);
_seminahn 0:4ff8aeb3e4d1 25 }
_seminahn 0:4ff8aeb3e4d1 26 // Template T_T
_seminahn 0:4ff8aeb3e4d1 27 void myLog(const char* varName, uint16_t val) {
_seminahn 0:4ff8aeb3e4d1 28 char temp[64] = {'\0',};
_seminahn 0:4ff8aeb3e4d1 29 sprintf(temp,"var: %s\tvalue: %u",varName,val);
_seminahn 0:4ff8aeb3e4d1 30 nh.loginfo(temp);
_seminahn 0:4ff8aeb3e4d1 31 }
_seminahn 0:4ff8aeb3e4d1 32 void myLog(const char* varName, int val) {
_seminahn 0:4ff8aeb3e4d1 33 char temp[32] = {'\0',};
_seminahn 0:4ff8aeb3e4d1 34 sprintf(temp,"var: %s\tvalue: %d",varName,val);
_seminahn 0:4ff8aeb3e4d1 35 nh.loginfo(temp);
_seminahn 0:4ff8aeb3e4d1 36 }
_seminahn 0:4ff8aeb3e4d1 37 void myLog(const char* varName, float val) {
_seminahn 0:4ff8aeb3e4d1 38 char temp[32] = {'\0',};
_seminahn 0:4ff8aeb3e4d1 39 sprintf(temp,"var: %s\tvalue: %f",varName,val);
_seminahn 0:4ff8aeb3e4d1 40 nh.loginfo(temp);
_seminahn 0:4ff8aeb3e4d1 41 }
_seminahn 0:4ff8aeb3e4d1 42 #endif
_seminahn 0:4ff8aeb3e4d1 43 void calibrationProcess() {
_seminahn 0:4ff8aeb3e4d1 44 nh.loginfo("IMU calibration is running... please don't move the robot");
_seminahn 0:4ff8aeb3e4d1 45 mpu.calibrateGyro();
_seminahn 0:4ff8aeb3e4d1 46 //mpu.calibrateMag();
_seminahn 0:4ff8aeb3e4d1 47 #if (NO_ROS)
_seminahn 0:4ff8aeb3e4d1 48 pc.printf("Calibration Completed !!!!!!!!\n");
_seminahn 0:4ff8aeb3e4d1 49 #else
_seminahn 0:4ff8aeb3e4d1 50 nh.loginfo("IMU calibration Completed!!!!!!!!");
_seminahn 0:4ff8aeb3e4d1 51 #endif
_seminahn 0:4ff8aeb3e4d1 52 //while(true) {;} //stop here
_seminahn 0:4ff8aeb3e4d1 53 }
_seminahn 0:4ff8aeb3e4d1 54
_seminahn 0:4ff8aeb3e4d1 55 void applyCalbratedValue() {
_seminahn 0:4ff8aeb3e4d1 56 //Vect3 gBias ={ 1.881, -2.630 , 0.226};
_seminahn 0:4ff8aeb3e4d1 57 //mpu.setGyroBias(gBias);
_seminahn 0:4ff8aeb3e4d1 58 //Vect3 mBias ={12.885, 314.122 , -594.508};
_seminahn 0:4ff8aeb3e4d1 59 //mpu.setMagBias(mBias);
_seminahn 0:4ff8aeb3e4d1 60 //Vect3 mScale={ 1.852, 1.176 , 0.621};
_seminahn 0:4ff8aeb3e4d1 61 //mpu.setMagScale(mScale);
_seminahn 0:4ff8aeb3e4d1 62 ;
_seminahn 0:4ff8aeb3e4d1 63 }
_seminahn 0:4ff8aeb3e4d1 64
_seminahn 0:4ff8aeb3e4d1 65 void set_msg(char* msg, const char* contents, size_t length) {
_seminahn 0:4ff8aeb3e4d1 66 memset(msg, '\0', SEQ_STATE_MSG_SIZE);
_seminahn 0:4ff8aeb3e4d1 67 memcpy(msg, contents, length);
_seminahn 0:4ff8aeb3e4d1 68 }
_seminahn 0:4ff8aeb3e4d1 69 }
_seminahn 2:0de4854743f7 70 #endif