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/threadDeclaration.hpp@1:2594a70c1ddd, 2021-05-26 (annotated)

Committer:
_seminahn
Date:
Wed May 26 05:26:16 2021 +0000
Revision:
1:2594a70c1ddd
Parent:
0:4ff8aeb3e4d1
Child:
2:0de4854743f7
sonar num 11 -> 10

Who changed what in which revision?

UserRevisionLine numberNew contents of line
_seminahn 0:4ff8aeb3e4d1 1 #ifndef ZETA_STM_KINETIC_THREADDECLARATION_H_
_seminahn 0:4ff8aeb3e4d1 2 #define ZETA_STM_KINETIC_THREADDECLARATION_H_
_seminahn 0:4ff8aeb3e4d1 3 #include "mbedHeader.hpp"
_seminahn 0:4ff8aeb3e4d1 4 #include "rosHeader.hpp"
_seminahn 0:4ff8aeb3e4d1 5 #include "moduleHeader.hpp"
_seminahn 0:4ff8aeb3e4d1 6 #include "defineHeader.h"
_seminahn 0:4ff8aeb3e4d1 7 #include "instanceHeader.hpp"
_seminahn 0:4ff8aeb3e4d1 8 #include "globalVariable.h"
_seminahn 0:4ff8aeb3e4d1 9 #include "myUtil.hpp"
_seminahn 0:4ff8aeb3e4d1 10
_seminahn 0:4ff8aeb3e4d1 11
_seminahn 0:4ff8aeb3e4d1 12 /* Threads begin ------------------------------------------------------------ */
_seminahn 0:4ff8aeb3e4d1 13 using myUtil::set_msg;
_seminahn 0:4ff8aeb3e4d1 14 using myUtil::calibrationProcess;
_seminahn 0:4ff8aeb3e4d1 15 using myUtil::applyCalbratedValue;
_seminahn 0:4ff8aeb3e4d1 16
_seminahn 1:2594a70c1ddd 17 #ifdef NO_ROS
_seminahn 0:4ff8aeb3e4d1 18 void print_thread() {
_seminahn 0:4ff8aeb3e4d1 19 const float time2print = 10.0;
_seminahn 0:4ff8aeb3e4d1 20 const float print_thread_hz = 3.0;
_seminahn 0:4ff8aeb3e4d1 21 waitTmr.start();
_seminahn 0:4ff8aeb3e4d1 22 while(waitTmr.read() < time2print) {;}
_seminahn 0:4ff8aeb3e4d1 23 waitTmr.stop();
_seminahn 0:4ff8aeb3e4d1 24 waitTmr.reset();
_seminahn 0:4ff8aeb3e4d1 25 while(true) {
_seminahn 0:4ff8aeb3e4d1 26 /*
_seminahn 0:4ff8aeb3e4d1 27 for(int i = 0; i < NUM_SONAR; i++) {
_seminahn 0:4ff8aeb3e4d1 28 //pc.printf("%2.2f\t%2.2f\t",dist[i],dist_raw[i]);
_seminahn 0:4ff8aeb3e4d1 29 }*/
_seminahn 0:4ff8aeb3e4d1 30 pc.printf("\n\r");
_seminahn 0:4ff8aeb3e4d1 31 pc.printf("Acc_x:%2.2f\tAcc_y:%2.2f\tAcc_z:%2.2f\n\r",
_seminahn 0:4ff8aeb3e4d1 32 gAcc_raw.x,gAcc_raw.y,gAcc_raw.z);
_seminahn 0:4ff8aeb3e4d1 33 pc.printf("Gyro_x:%2.2f\tGyro_y:%2.2f\tGyro_z:%2.2f\n\r",
_seminahn 0:4ff8aeb3e4d1 34 gGyro_raw.x,gGyro_raw.y,gGyro_raw.z);
_seminahn 0:4ff8aeb3e4d1 35 pc.printf("Mag_x:%2.2f\tMag_y:%2.2f\tMag_z:%2.2f\n\r",
_seminahn 0:4ff8aeb3e4d1 36 gMag_raw.x,gMag_raw.y,gMag_raw.z);
_seminahn 0:4ff8aeb3e4d1 37 pc.printf("quaternion: %2.2f %2.2f %2.2f %2.2f\n\r",
_seminahn 0:4ff8aeb3e4d1 38 gQ[0],gQ[1],gQ[2],gQ[3]);
_seminahn 0:4ff8aeb3e4d1 39 pc.printf("RPY: %2.2f\t%2.2f\t%2.2f\n\r",gRoll,gPitch,gYaw);
_seminahn 0:4ff8aeb3e4d1 40 pc.printf("Theta_z: %2.2f degree\n\r", gTheta);
_seminahn 0:4ff8aeb3e4d1 41 ThisThread::sleep_for(1000/print_thread_hz);
_seminahn 0:4ff8aeb3e4d1 42 }
_seminahn 0:4ff8aeb3e4d1 43 }
_seminahn 0:4ff8aeb3e4d1 44 #else
_seminahn 0:4ff8aeb3e4d1 45 void bt_pub_thread() {
_seminahn 0:4ff8aeb3e4d1 46 const float bt_pub_hz = 40.0;
_seminahn 0:4ff8aeb3e4d1 47 char seq_state_msg[SEQ_STATE_MSG_SIZE] = {'\0'};
_seminahn 0:4ff8aeb3e4d1 48 const char contact[] = "contact";
_seminahn 0:4ff8aeb3e4d1 49 const char BT_waiting[] = "BT_waiting";
_seminahn 0:4ff8aeb3e4d1 50 const char start[] = "start";
_seminahn 0:4ff8aeb3e4d1 51 const char search[] = "search";
_seminahn 0:4ff8aeb3e4d1 52 const char adjustment[] = "adjustment";
_seminahn 0:4ff8aeb3e4d1 53 const char guidance[] = "guidance";
_seminahn 0:4ff8aeb3e4d1 54 const char charging[] = "charging";
_seminahn 0:4ff8aeb3e4d1 55 const char finish[] = "finish";
_seminahn 0:4ff8aeb3e4d1 56 const char not_connected[] = "not_connected";
_seminahn 0:4ff8aeb3e4d1 57 const char left[] = "left";
_seminahn 0:4ff8aeb3e4d1 58 const char right[] = "right";
_seminahn 0:4ff8aeb3e4d1 59 const char disconnected[] = "disconnected";
_seminahn 0:4ff8aeb3e4d1 60 char* buff = NULL;
_seminahn 0:4ff8aeb3e4d1 61 enum class AUTO_CHARGE_STATE : int {
_seminahn 0:4ff8aeb3e4d1 62 start = 1,
_seminahn 0:4ff8aeb3e4d1 63 BT_waiting = 1,
_seminahn 0:4ff8aeb3e4d1 64 search,
_seminahn 0:4ff8aeb3e4d1 65 adjustment,
_seminahn 0:4ff8aeb3e4d1 66 guidance,
_seminahn 0:4ff8aeb3e4d1 67 charging,
_seminahn 0:4ff8aeb3e4d1 68 finish,
_seminahn 0:4ff8aeb3e4d1 69 };
_seminahn 0:4ff8aeb3e4d1 70 relay.off();
_seminahn 0:4ff8aeb3e4d1 71 while(waitTmr.read() < 5.0) {;}
_seminahn 0:4ff8aeb3e4d1 72 while(1) {
_seminahn 0:4ff8aeb3e4d1 73 switch(NUC_sub_state) {
_seminahn 0:4ff8aeb3e4d1 74 case static_cast<int>(AUTO_CHARGE_STATE::start):
_seminahn 0:4ff8aeb3e4d1 75 if(bt_data.rec == 1 || bt_data.rec == 2) {
_seminahn 0:4ff8aeb3e4d1 76 set_msg(seq_state_msg, start, sizeof(start));
_seminahn 0:4ff8aeb3e4d1 77 relay.off();
_seminahn 0:4ff8aeb3e4d1 78 bt_data.sen = 3;
_seminahn 0:4ff8aeb3e4d1 79 }
_seminahn 0:4ff8aeb3e4d1 80 else {
_seminahn 0:4ff8aeb3e4d1 81 set_msg(seq_state_msg, BT_waiting, sizeof(BT_waiting));
_seminahn 0:4ff8aeb3e4d1 82 relay.off();
_seminahn 0:4ff8aeb3e4d1 83 bt_data.sen = 2;
_seminahn 0:4ff8aeb3e4d1 84 }
_seminahn 0:4ff8aeb3e4d1 85 break;
_seminahn 0:4ff8aeb3e4d1 86 case static_cast<int>(AUTO_CHARGE_STATE::search):
_seminahn 0:4ff8aeb3e4d1 87 set_msg(seq_state_msg, search, sizeof(search));
_seminahn 0:4ff8aeb3e4d1 88 relay.off();
_seminahn 0:4ff8aeb3e4d1 89 bt_data.sen = 4;
_seminahn 0:4ff8aeb3e4d1 90 break;
_seminahn 0:4ff8aeb3e4d1 91 case static_cast<int>(AUTO_CHARGE_STATE::adjustment):
_seminahn 0:4ff8aeb3e4d1 92 set_msg(seq_state_msg, adjustment, sizeof(adjustment));
_seminahn 0:4ff8aeb3e4d1 93 relay.off();
_seminahn 0:4ff8aeb3e4d1 94 bt_data.sen = 5;
_seminahn 0:4ff8aeb3e4d1 95 break;
_seminahn 0:4ff8aeb3e4d1 96 case static_cast<int>(AUTO_CHARGE_STATE::guidance):
_seminahn 0:4ff8aeb3e4d1 97 set_msg(seq_state_msg, guidance, sizeof(guidance));
_seminahn 0:4ff8aeb3e4d1 98 bt_data.sen = 6;
_seminahn 0:4ff8aeb3e4d1 99 break;
_seminahn 0:4ff8aeb3e4d1 100 case static_cast<int>(AUTO_CHARGE_STATE::charging):
_seminahn 0:4ff8aeb3e4d1 101 set_msg(seq_state_msg, charging, sizeof(charging));
_seminahn 0:4ff8aeb3e4d1 102 bt_data.sen = 7;
_seminahn 0:4ff8aeb3e4d1 103 break;
_seminahn 0:4ff8aeb3e4d1 104 case static_cast<int>(AUTO_CHARGE_STATE::finish):
_seminahn 0:4ff8aeb3e4d1 105 set_msg(seq_state_msg, finish, sizeof(finish));
_seminahn 0:4ff8aeb3e4d1 106 bt_data.sen = 8;
_seminahn 0:4ff8aeb3e4d1 107 break;
_seminahn 0:4ff8aeb3e4d1 108 }
_seminahn 0:4ff8aeb3e4d1 109
_seminahn 0:4ff8aeb3e4d1 110 if(strstr(seq_state_msg,guidance) != NULL) {
_seminahn 0:4ff8aeb3e4d1 111 switch(bt_data.rec) {
_seminahn 0:4ff8aeb3e4d1 112 case 69:
_seminahn 0:4ff8aeb3e4d1 113 set_msg(seq_state_msg, contact, sizeof(contact));
_seminahn 0:4ff8aeb3e4d1 114 relay.on();
_seminahn 0:4ff8aeb3e4d1 115 break;
_seminahn 0:4ff8aeb3e4d1 116 case 66:
_seminahn 0:4ff8aeb3e4d1 117 set_msg(seq_state_msg, left, sizeof(left));
_seminahn 0:4ff8aeb3e4d1 118 relay.off();
_seminahn 0:4ff8aeb3e4d1 119 break;
_seminahn 0:4ff8aeb3e4d1 120 case 63:
_seminahn 0:4ff8aeb3e4d1 121 set_msg(seq_state_msg, right, sizeof(right));
_seminahn 0:4ff8aeb3e4d1 122 relay.off();
_seminahn 0:4ff8aeb3e4d1 123 break;
_seminahn 0:4ff8aeb3e4d1 124 default:
_seminahn 0:4ff8aeb3e4d1 125 set_msg(seq_state_msg, not_connected, sizeof(not_connected));
_seminahn 0:4ff8aeb3e4d1 126 relay.off();
_seminahn 0:4ff8aeb3e4d1 127 break;
_seminahn 0:4ff8aeb3e4d1 128 }
_seminahn 0:4ff8aeb3e4d1 129 }
_seminahn 0:4ff8aeb3e4d1 130 else if(strstr(seq_state_msg,charging) != NULL) {
_seminahn 0:4ff8aeb3e4d1 131 switch(bt_data.rec) {
_seminahn 0:4ff8aeb3e4d1 132 case 79:
_seminahn 0:4ff8aeb3e4d1 133 set_msg(seq_state_msg, contact, sizeof(contact));
_seminahn 0:4ff8aeb3e4d1 134 relay.on();
_seminahn 0:4ff8aeb3e4d1 135 break;
_seminahn 0:4ff8aeb3e4d1 136 default:
_seminahn 0:4ff8aeb3e4d1 137 set_msg(seq_state_msg, disconnected, sizeof(disconnected));
_seminahn 0:4ff8aeb3e4d1 138 relay.off();
_seminahn 0:4ff8aeb3e4d1 139 }
_seminahn 0:4ff8aeb3e4d1 140 }
_seminahn 0:4ff8aeb3e4d1 141 Bluetooth_msg.data = seq_state_msg;
_seminahn 0:4ff8aeb3e4d1 142 Bluetooth_publisher.publish(&Bluetooth_msg);
_seminahn 0:4ff8aeb3e4d1 143 nh.spinOnce();
_seminahn 0:4ff8aeb3e4d1 144 ThisThread::sleep_for(1000/bt_pub_hz);
_seminahn 0:4ff8aeb3e4d1 145 }
_seminahn 0:4ff8aeb3e4d1 146 }
_seminahn 0:4ff8aeb3e4d1 147
_seminahn 0:4ff8aeb3e4d1 148
_seminahn 0:4ff8aeb3e4d1 149 void imu_pub_thread() {
_seminahn 0:4ff8aeb3e4d1 150 const float imu_pub_hz = 40.0;
_seminahn 0:4ff8aeb3e4d1 151 imu_msg.header.frame_id = "imu_link";
_seminahn 0:4ff8aeb3e4d1 152 //mag_msg.header.frame_id = "/imu/mag";
_seminahn 0:4ff8aeb3e4d1 153 while(waitTmr.read() < 5.0) {;}
_seminahn 0:4ff8aeb3e4d1 154 while(1) {
_seminahn 0:4ff8aeb3e4d1 155 imu_msg.header.stamp = nh.now();
_seminahn 0:4ff8aeb3e4d1 156 imu_msg.orientation.w = gQ[0];
_seminahn 0:4ff8aeb3e4d1 157 imu_msg.orientation.x = gQ[1];
_seminahn 0:4ff8aeb3e4d1 158 imu_msg.orientation.y = gQ[2];
_seminahn 0:4ff8aeb3e4d1 159 imu_msg.orientation.z = gQ[3];
_seminahn 0:4ff8aeb3e4d1 160
_seminahn 0:4ff8aeb3e4d1 161 imu_msg.angular_velocity.x = gGyro_raw.x;
_seminahn 0:4ff8aeb3e4d1 162 imu_msg.angular_velocity.y = gGyro_raw.y;
_seminahn 0:4ff8aeb3e4d1 163 imu_msg.angular_velocity.z = gGyro_raw.z;
_seminahn 0:4ff8aeb3e4d1 164
_seminahn 0:4ff8aeb3e4d1 165 imu_msg.linear_acceleration.x = gAcc_raw.x;
_seminahn 0:4ff8aeb3e4d1 166 imu_msg.linear_acceleration.y = gAcc_raw.y;
_seminahn 0:4ff8aeb3e4d1 167 imu_msg.linear_acceleration.z = gAcc_raw.z;
_seminahn 0:4ff8aeb3e4d1 168
_seminahn 0:4ff8aeb3e4d1 169 IMU_publisher.publish(&imu_msg);
_seminahn 0:4ff8aeb3e4d1 170 nh.spinOnce();
_seminahn 0:4ff8aeb3e4d1 171
_seminahn 0:4ff8aeb3e4d1 172 ThisThread::sleep_for(1000/imu_pub_hz);
_seminahn 0:4ff8aeb3e4d1 173 }
_seminahn 0:4ff8aeb3e4d1 174 }
_seminahn 0:4ff8aeb3e4d1 175
_seminahn 0:4ff8aeb3e4d1 176 void sonar_pub_thread() {
_seminahn 0:4ff8aeb3e4d1 177 const float sonar_pub_hz = 15.0;
_seminahn 0:4ff8aeb3e4d1 178 int ii = 0;
_seminahn 0:4ff8aeb3e4d1 179 for(;ii < NUM_SONAR; ii++) {
_seminahn 0:4ff8aeb3e4d1 180 US_msg.data[ii] = 0.0;
_seminahn 0:4ff8aeb3e4d1 181 }
_seminahn 0:4ff8aeb3e4d1 182 while(waitTmr.read() < 5.0) {;}
_seminahn 0:4ff8aeb3e4d1 183 while(1) {
_seminahn 0:4ff8aeb3e4d1 184 //memcpy(US_msg.data, (float*)dist, sizeof(float)*NUM_SONAR);
_seminahn 0:4ff8aeb3e4d1 185 for(ii = 0;ii < NUM_SONAR; ii++) {
_seminahn 0:4ff8aeb3e4d1 186 US_msg.data[ii] = dist[ii];
_seminahn 0:4ff8aeb3e4d1 187 }
_seminahn 0:4ff8aeb3e4d1 188 US_publisher.publish(&US_msg);
_seminahn 0:4ff8aeb3e4d1 189 nh.spinOnce();
_seminahn 0:4ff8aeb3e4d1 190 ThisThread::sleep_for(1000/sonar_pub_hz);
_seminahn 0:4ff8aeb3e4d1 191 }
_seminahn 0:4ff8aeb3e4d1 192 }
_seminahn 0:4ff8aeb3e4d1 193
_seminahn 0:4ff8aeb3e4d1 194 void estop_pub_thread() {
_seminahn 0:4ff8aeb3e4d1 195 const float estop_pub_hz = 5; // ms
_seminahn 0:4ff8aeb3e4d1 196 while(waitTmr.read() < 5.0) {;}
_seminahn 0:4ff8aeb3e4d1 197 while(1) {
_seminahn 0:4ff8aeb3e4d1 198 /*
_seminahn 0:4ff8aeb3e4d1 199 if(bumperL.isTouch() || bumperC.isTouch() || bumperR.isTouch()) Bumper_msg.data = true;
_seminahn 0:4ff8aeb3e4d1 200 else Bumper_msg.data = false;
_seminahn 0:4ff8aeb3e4d1 201 */
_seminahn 0:4ff8aeb3e4d1 202 EStop_msg.data = estop.isTouch();
_seminahn 0:4ff8aeb3e4d1 203 EStop_publisher.publish(&EStop_msg);
_seminahn 0:4ff8aeb3e4d1 204 Bumper_publisher.publish(&Bumper_msg);
_seminahn 0:4ff8aeb3e4d1 205 nh.spinOnce();
_seminahn 0:4ff8aeb3e4d1 206 ThisThread::sleep_for(1000/estop_pub_hz);
_seminahn 0:4ff8aeb3e4d1 207 }
_seminahn 0:4ff8aeb3e4d1 208 }
_seminahn 0:4ff8aeb3e4d1 209 #endif
_seminahn 0:4ff8aeb3e4d1 210
_seminahn 0:4ff8aeb3e4d1 211 void bt_thread() {
_seminahn 0:4ff8aeb3e4d1 212 const float bt_thread_hz = 100.0;
_seminahn 0:4ff8aeb3e4d1 213 const float sen_period = 100.0;
_seminahn 0:4ff8aeb3e4d1 214 const int sen_cnt = int(sen_period * bt_thread_hz / 1000.0);
_seminahn 0:4ff8aeb3e4d1 215 int cnt = 0;
_seminahn 0:4ff8aeb3e4d1 216 char temp[2] = {'\0',};
_seminahn 0:4ff8aeb3e4d1 217 while(1) {
_seminahn 1:2594a70c1ddd 218 if(bt.readable())
_seminahn 1:2594a70c1ddd 219 {
_seminahn 1:2594a70c1ddd 220 bt_data.rec = bt.getc();
_seminahn 1:2594a70c1ddd 221 char log_char[2];
_seminahn 1:2594a70c1ddd 222 sprintf(log_char,"%c",bt_data.rec);
_seminahn 1:2594a70c1ddd 223 nh.loginfo(log_char);
_seminahn 1:2594a70c1ddd 224 }
_seminahn 0:4ff8aeb3e4d1 225 if(++cnt % 10 == 0) {
_seminahn 0:4ff8aeb3e4d1 226 bt.putc(bt_data.sen);
_seminahn 0:4ff8aeb3e4d1 227 cnt = 0;
_seminahn 0:4ff8aeb3e4d1 228 }
_seminahn 0:4ff8aeb3e4d1 229 ThisThread::sleep_for(1000/bt_thread_hz);
_seminahn 0:4ff8aeb3e4d1 230 }
_seminahn 0:4ff8aeb3e4d1 231 }
_seminahn 0:4ff8aeb3e4d1 232
_seminahn 0:4ff8aeb3e4d1 233 void sonar_thread() {
_seminahn 0:4ff8aeb3e4d1 234 const float sonar_hz = 15.0;
_seminahn 1:2594a70c1ddd 235 sonar_manager.Begin(sonar_hz);
_seminahn 0:4ff8aeb3e4d1 236 while(true) {
_seminahn 1:2594a70c1ddd 237 sonar_manager.GetDistance(dist);
_seminahn 0:4ff8aeb3e4d1 238 ThisThread::sleep_for(1000/sonar_hz);
_seminahn 0:4ff8aeb3e4d1 239 }
_seminahn 0:4ff8aeb3e4d1 240 }
_seminahn 1:2594a70c1ddd 241 /*
_seminahn 0:4ff8aeb3e4d1 242 void module_thread() {
_seminahn 0:4ff8aeb3e4d1 243 const float module_hz = 10.0;
_seminahn 0:4ff8aeb3e4d1 244 while(1) {
_seminahn 0:4ff8aeb3e4d1 245 if(isSubscribe) {
_seminahn 0:4ff8aeb3e4d1 246 char send_str[32] = {'\0',};
_seminahn 1:2594a70c1ddd 247 uvc.setMsg(&UVCcontrolMsg);
_seminahn 1:2594a70c1ddd 248 uvc.control();
_seminahn 0:4ff8aeb3e4d1 249 isSubscribe = false;
_seminahn 0:4ff8aeb3e4d1 250 }
_seminahn 0:4ff8aeb3e4d1 251 ThisThread::sleep_for(1000/module_hz);
_seminahn 0:4ff8aeb3e4d1 252 }
_seminahn 0:4ff8aeb3e4d1 253
_seminahn 0:4ff8aeb3e4d1 254 ;
_seminahn 0:4ff8aeb3e4d1 255 }
_seminahn 1:2594a70c1ddd 256 */
_seminahn 0:4ff8aeb3e4d1 257
_seminahn 0:4ff8aeb3e4d1 258 #define CALIBRATION_MODE 1
_seminahn 0:4ff8aeb3e4d1 259 void IMU_thread() {
_seminahn 0:4ff8aeb3e4d1 260 const float imu_hz = 100.0;
_seminahn 0:4ff8aeb3e4d1 261 Vect3 a, g, m;
_seminahn 0:4ff8aeb3e4d1 262 mpu.setup();
_seminahn 0:4ff8aeb3e4d1 263 mpu.enableDataReadyInterrupt();
_seminahn 0:4ff8aeb3e4d1 264 #if (CALIBRATION_MODE)
_seminahn 0:4ff8aeb3e4d1 265 calibrationProcess();
_seminahn 0:4ff8aeb3e4d1 266 #endif
_seminahn 0:4ff8aeb3e4d1 267 applyCalbratedValue();
_seminahn 0:4ff8aeb3e4d1 268
_seminahn 0:4ff8aeb3e4d1 269 memset(gQ,0.,4*sizeof(float));
_seminahn 0:4ff8aeb3e4d1 270
_seminahn 0:4ff8aeb3e4d1 271 while(true){
_seminahn 0:4ff8aeb3e4d1 272 if (mpu.isDataReady()){
_seminahn 0:4ff8aeb3e4d1 273 Vect3 a, g, m; // acc/gyro/mag vectors
_seminahn 0:4ff8aeb3e4d1 274 mpu.update(MADGWICK);
_seminahn 0:4ff8aeb3e4d1 275 a = mpu.getAccelVect();
_seminahn 0:4ff8aeb3e4d1 276 g = mpu.getGyroVect();
_seminahn 0:4ff8aeb3e4d1 277 m = mpu.getMagVect();
_seminahn 1:2594a70c1ddd 278 #ifdef NO_ROS
_seminahn 0:4ff8aeb3e4d1 279 gAcc_raw.x = a.x*G;
_seminahn 0:4ff8aeb3e4d1 280 gAcc_raw.y = a.y*G;
_seminahn 0:4ff8aeb3e4d1 281 gAcc_raw.z = a.z*G;
_seminahn 0:4ff8aeb3e4d1 282 gGyro_raw.x = g.x*DEG_TO_RAD;
_seminahn 0:4ff8aeb3e4d1 283 gGyro_raw.y = g.y*DEG_TO_RAD;
_seminahn 0:4ff8aeb3e4d1 284 gGyro_raw.z = g.z*DEG_TO_RAD;
_seminahn 0:4ff8aeb3e4d1 285 gQ[0] = mpu.getq0();
_seminahn 0:4ff8aeb3e4d1 286 gQ[1] = mpu.getq1();
_seminahn 0:4ff8aeb3e4d1 287 gQ[2] = mpu.getq2();
_seminahn 0:4ff8aeb3e4d1 288 gQ[3] = mpu.getq3();
_seminahn 0:4ff8aeb3e4d1 289 gRoll = mpu.getRoll()*RAD_TO_DEG;
_seminahn 0:4ff8aeb3e4d1 290 gPitch = mpu.getPitch()*RAD_TO_DEG;
_seminahn 0:4ff8aeb3e4d1 291 gYaw = mpu.getYaw()*RAD_TO_DEG;
_seminahn 0:4ff8aeb3e4d1 292 gTheta = atan2f(gQ[1] * gQ[2] + gQ[0] * gQ[3], 0.5f - gQ[2] * gQ[2] - gQ[3] * gQ[3]) * RAD_TO_DEG;
_seminahn 0:4ff8aeb3e4d1 293 #else // ros
_seminahn 0:4ff8aeb3e4d1 294 mpu.update(MADGWICK);
_seminahn 0:4ff8aeb3e4d1 295 a = mpu.getAccelVect();
_seminahn 0:4ff8aeb3e4d1 296 g = mpu.getGyroVect();
_seminahn 0:4ff8aeb3e4d1 297 m = mpu.getMagVect();
_seminahn 0:4ff8aeb3e4d1 298
_seminahn 0:4ff8aeb3e4d1 299 gAcc_raw.x = a.x*G;
_seminahn 0:4ff8aeb3e4d1 300 gAcc_raw.y = a.y*G;
_seminahn 0:4ff8aeb3e4d1 301 gAcc_raw.z = a.z*G;
_seminahn 0:4ff8aeb3e4d1 302 gGyro_raw.x = g.x*DEG_TO_RAD;
_seminahn 0:4ff8aeb3e4d1 303 gGyro_raw.y = g.y*DEG_TO_RAD;
_seminahn 0:4ff8aeb3e4d1 304 gGyro_raw.z = g.z*DEG_TO_RAD;
_seminahn 0:4ff8aeb3e4d1 305
_seminahn 0:4ff8aeb3e4d1 306 //gMag_raw.x = m.x; // not use for our robot
_seminahn 0:4ff8aeb3e4d1 307 //gMag_raw.y = m.y;
_seminahn 0:4ff8aeb3e4d1 308 //gMag_raw.z = m.z;
_seminahn 0:4ff8aeb3e4d1 309 //gRoll = mpu.getRoll()*RAD_TO_DEG;
_seminahn 0:4ff8aeb3e4d1 310 //gPitch = mpu.getPitch()*RAD_TO_DEG;
_seminahn 0:4ff8aeb3e4d1 311 //gYaw = mpu.getYaw()*RAD_TO_DEG;
_seminahn 0:4ff8aeb3e4d1 312
_seminahn 0:4ff8aeb3e4d1 313 gQ[0] = mpu.getq0();
_seminahn 0:4ff8aeb3e4d1 314 gQ[1] = mpu.getq1();
_seminahn 0:4ff8aeb3e4d1 315 gQ[2] = mpu.getq2();
_seminahn 0:4ff8aeb3e4d1 316 gQ[3] = mpu.getq3();
_seminahn 0:4ff8aeb3e4d1 317 //gTheta = atan2f(gQ[1] * gQ[2] + gQ[0] * gQ[3], 0.5f - gQ[2] * gQ[2] - gQ[3] * gQ[3]) * RAD_TO_DEG;
_seminahn 0:4ff8aeb3e4d1 318 //char myStr[64] = {'\0',};
_seminahn 0:4ff8aeb3e4d1 319 //sprintf(myStr,"theta: %f\troll: %f\tpitch: %f\tyaw: %f",gTheta,gRoll,gPitch,gYaw);
_seminahn 0:4ff8aeb3e4d1 320 //nh.loginfo(myStr);
_seminahn 0:4ff8aeb3e4d1 321 #endif
_seminahn 0:4ff8aeb3e4d1 322 }
_seminahn 0:4ff8aeb3e4d1 323 ThisThread::sleep_for(1000/imu_hz);
_seminahn 0:4ff8aeb3e4d1 324 }
_seminahn 0:4ff8aeb3e4d1 325 }
_seminahn 0:4ff8aeb3e4d1 326 extern RELAY relay;
_seminahn 0:4ff8aeb3e4d1 327 void test_thread() {
_seminahn 1:2594a70c1ddd 328
_seminahn 0:4ff8aeb3e4d1 329 while(true) {
_seminahn 1:2594a70c1ddd 330 bt.putc('a');
_seminahn 1:2594a70c1ddd 331 ThisThread::sleep_for(10);
_seminahn 1:2594a70c1ddd 332 if(bt.readable())
_seminahn 1:2594a70c1ddd 333 {
_seminahn 1:2594a70c1ddd 334 char debug_str[6];
_seminahn 1:2594a70c1ddd 335 sprintf(debug_str,"%c",bt.getc());
_seminahn 1:2594a70c1ddd 336 nh.loginfo(debug_str);
_seminahn 1:2594a70c1ddd 337 nh.spinOnce();
_seminahn 1:2594a70c1ddd 338 }
_seminahn 1:2594a70c1ddd 339 ThisThread::sleep_for(500);
_seminahn 0:4ff8aeb3e4d1 340 }
_seminahn 0:4ff8aeb3e4d1 341 }
_seminahn 0:4ff8aeb3e4d1 342
_seminahn 0:4ff8aeb3e4d1 343 /* Threads end -------------------------------------------------------------- */
_seminahn 0:4ff8aeb3e4d1 344 #endif