threadDeclaration.hpp

00001 #ifndef ZETA_STM_KINETIC_THREADDECLARATION_H_
00002 #define ZETA_STM_KINETIC_THREADDECLARATION_H_
00003 #include "mbedHeader.hpp"
00004 #include "rosHeader.hpp"
00005 #include "moduleHeader.hpp"
00006 #include "variables/defineHeader.h"
00007 #include "variables/instanceHeader.hpp"
00008 #include "variables/globalVariable.h"
00009 #include "configurations/robotConfig.h"
00010 #include "myUtil.hpp"
00011 
00012 
00013 /* Threads begin ------------------------------------------------------------ */
00014 using myUtil::set_msg;
00015 using myUtil::calibrationProcess;
00016 using myUtil::applyCalbratedValue;
00017 #if ((ROBOT_TYPE == MODEL_C) || (ROBOT_TYPE == MODEL_D))
00018 //extern ChargingControl charging_control;
00019 #endif
00020 #ifdef NO_ROS
00021 void print_thread() {
00022     const float time2print = 10.0;
00023     const float print_thread_hz = 3.0;
00024     waitTmr.start();
00025     while(waitTmr.read() < time2print) {;}
00026     waitTmr.stop();
00027     waitTmr.reset();
00028     while(true) {
00029         /*
00030         for(int i = 0; i < NUM_SONAR; i++) {
00031             //pc.printf("%2.2f\t%2.2f\t",dist[i],dist_raw[i]);
00032         }*/
00033         pc.printf("\n\r");
00034         pc.printf("Acc_x:%2.2f\tAcc_y:%2.2f\tAcc_z:%2.2f\n\r",
00035             gAcc_raw.x,gAcc_raw.y,gAcc_raw.z);
00036         pc.printf("Gyro_x:%2.2f\tGyro_y:%2.2f\tGyro_z:%2.2f\n\r",
00037             gGyro_raw.x,gGyro_raw.y,gGyro_raw.z);
00038         pc.printf("Mag_x:%2.2f\tMag_y:%2.2f\tMag_z:%2.2f\n\r",
00039             gMag_raw.x,gMag_raw.y,gMag_raw.z);
00040         pc.printf("quaternion: %2.2f  %2.2f  %2.2f  %2.2f\n\r",
00041             gQ[0],gQ[1],gQ[2],gQ[3]);
00042         pc.printf("RPY: %2.2f\t%2.2f\t%2.2f\n\r",gRoll,gPitch,gYaw);
00043         pc.printf("Theta_z: %2.2f degree\n\r", gTheta);
00044         ThisThread::sleep_for(1000/print_thread_hz);
00045     }    
00046 }
00047 #else
00048 
00049 #if ((ROBOT_TYPE == MODEL_C) || (ROBOT_TYPE == MODEL_D))
00050 void bt_pub_thread() {
00051     const float bt_pub_hz = 40.0;
00052     char seq_state_msg[SEQ_STATE_MSG_SIZE] = {'\0'};
00053     const char contact[] = "contact";
00054     const char BT_waiting[] = "BT_waiting";
00055     const char start[] = "start";
00056     const char search[] = "search";
00057     const char adjustment[] = "adjustment";
00058     const char guidance[] = "guidance";
00059     const char charging[] = "charging";
00060     const char finish[] = "finish";
00061     const char not_connected[] = "not_connected";
00062     const char left[] = "left";
00063     const char right[] = "right";
00064     const char disconnected[] = "disconnected";
00065     char* buff = NULL;
00066     enum class AUTO_CHARGE_STATE : int {
00067         start = 1,
00068         BT_waiting = 1,
00069         search,
00070         adjustment,
00071         guidance,
00072         charging,
00073         finish,
00074     };
00075     charging_control.off();
00076     //while(waitTmr.read() < 5.0) {;}
00077     while(1) {
00078         if(bt_data.rec == 0)
00079         {
00080             charging_control.off();
00081         }
00082         switch(NUC_sub_state) {
00083             case static_cast<int>(AUTO_CHARGE_STATE::start):
00084                 if(bt_data.rec == 1 || bt_data.rec == 2) {
00085                     set_msg(seq_state_msg, start, sizeof(start));
00086                     bt_data.sen = 3;
00087                 }
00088                 else {
00089                     set_msg(seq_state_msg, BT_waiting, sizeof(BT_waiting));
00090                     bt_data.sen = 2;
00091                 }
00092                 charging_control.off();
00093                 break;
00094             case static_cast<int>(AUTO_CHARGE_STATE::search):
00095                 set_msg(seq_state_msg, search, sizeof(search));
00096                 bt_data.sen = 4;
00097                 break;
00098             case static_cast<int>(AUTO_CHARGE_STATE::adjustment):
00099                 set_msg(seq_state_msg, adjustment, sizeof(adjustment));
00100                 bt_data.sen = 5;
00101                 break;
00102             case static_cast<int>(AUTO_CHARGE_STATE::guidance):
00103                 set_msg(seq_state_msg, guidance, sizeof(guidance));
00104                 bt_data.sen = 6;
00105                 break;
00106             case static_cast<int>(AUTO_CHARGE_STATE::charging):
00107                 set_msg(seq_state_msg, charging, sizeof(charging));
00108                 bt_data.sen = 7;
00109                 break;
00110             case static_cast<int>(AUTO_CHARGE_STATE::finish):
00111                 set_msg(seq_state_msg, finish, sizeof(finish));
00112                 charging_control.off();
00113                 bt_data.sen = 8;
00114                 break;    
00115         }
00116         
00117         if(strstr(seq_state_msg,guidance) != NULL) {
00118             switch(bt_data.rec) {
00119                 case 69:
00120                     set_msg(seq_state_msg, contact, sizeof(contact));
00121                     break;
00122                 case 66:
00123                     set_msg(seq_state_msg, left, sizeof(left));
00124                     break;
00125                 case 63:
00126                     set_msg(seq_state_msg, right, sizeof(right));
00127                     break;
00128                 default:
00129                     set_msg(seq_state_msg, not_connected, sizeof(not_connected));
00130                     charging_control.off();
00131                     break;
00132             }
00133         }
00134         else if(strstr(seq_state_msg,charging) != NULL) {
00135             switch(bt_data.rec) {
00136                 case 79:
00137                     set_msg(seq_state_msg, contact, sizeof(contact));
00138                     charging_control.on();
00139                     break;
00140                 default:
00141                     set_msg(seq_state_msg, disconnected, sizeof(disconnected));
00142                     charging_control.off();
00143             }    
00144         }
00145         Bluetooth_msg.data = seq_state_msg;
00146         Bluetooth_publisher.publish(&Bluetooth_msg);
00147         nh.spinOnce();
00148         ThisThread::sleep_for(1000/bt_pub_hz);
00149     }
00150 }
00151 #elif (ROBOT_TYPE == MODEL_I)
00152 void bt_pub_thread() {
00153     const float bt_pub_hz = 40.0;
00154     char seq_state_msg[SEQ_STATE_MSG_SIZE] = {'\0'};
00155     const char contact[] = "contact";
00156     const char BT_waiting[] = "BT_waiting";
00157     const char start[] = "start";
00158     const char search[] = "search";
00159     const char adjustment[] = "adjustment";
00160     const char guidance[] = "guidance";
00161     const char charging[] = "charging";
00162     const char finish[] = "finish";
00163     const char not_connected[] = "not_connected";
00164     const char left[] = "left";
00165     const char right[] = "right";
00166     const char disconnected[] = "disconnected";
00167     char* buff = NULL;
00168     enum class AUTO_CHARGE_STATE : int {
00169         start = 1,
00170         BT_waiting = 1,
00171         search,
00172         adjustment,
00173         guidance,
00174         charging,
00175         finish,
00176     };
00177     ChargingSsr = 0;
00178     //while(waitTmr.read() < 5.0) {;}
00179     while(1) {
00180         switch(NUC_sub_state) {
00181             case static_cast<int>(AUTO_CHARGE_STATE::start):
00182                 if(bt_data.rec == 1 || bt_data.rec == 2) {
00183                     set_msg(seq_state_msg, start, sizeof(start));
00184                     bt_data.sen = 3;
00185                 }
00186                 else {
00187                     set_msg(seq_state_msg, BT_waiting, sizeof(BT_waiting));
00188                     bt_data.sen = 2;
00189                 }
00190                 ChargingSsr = 0;
00191                 break;
00192             case static_cast<int>(AUTO_CHARGE_STATE::search):
00193                 set_msg(seq_state_msg, search, sizeof(search));
00194                 bt_data.sen = 4;
00195                 break;
00196             case static_cast<int>(AUTO_CHARGE_STATE::adjustment):
00197                 set_msg(seq_state_msg, adjustment, sizeof(adjustment));
00198                 bt_data.sen = 5;
00199                 break;
00200             case static_cast<int>(AUTO_CHARGE_STATE::guidance):
00201                 set_msg(seq_state_msg, guidance, sizeof(guidance));
00202                 bt_data.sen = 6;
00203                 break;
00204             case static_cast<int>(AUTO_CHARGE_STATE::charging):
00205                 set_msg(seq_state_msg, charging, sizeof(charging));
00206                 bt_data.sen = 7;
00207                 break;
00208             case static_cast<int>(AUTO_CHARGE_STATE::finish):
00209                 set_msg(seq_state_msg, finish, sizeof(finish));
00210                 ChargingSsr = 0;
00211                 bt_data.sen = 8;
00212                 break;    
00213         }
00214         
00215         if(strstr(seq_state_msg,guidance) != NULL) {
00216             switch(bt_data.rec) {
00217                 case 69:
00218                     set_msg(seq_state_msg, contact, sizeof(contact));
00219                     break;
00220                 case 66:
00221                     set_msg(seq_state_msg, left, sizeof(left));
00222                     break;
00223                 case 63:
00224                     set_msg(seq_state_msg, right, sizeof(right));
00225                     break;
00226                 default:
00227                     set_msg(seq_state_msg, not_connected, sizeof(not_connected));
00228                     ChargingSsr = 0;
00229                     break;
00230             }
00231         }
00232         else if(strstr(seq_state_msg,charging) != NULL) {
00233             switch(bt_data.rec) {
00234                 case 79:
00235                     set_msg(seq_state_msg, contact, sizeof(contact));
00236                     ChargingSsr = 1;
00237                     break;
00238                 default:
00239                     set_msg(seq_state_msg, disconnected, sizeof(disconnected));
00240                     ChargingSsr = 0;
00241             }    
00242         }
00243         Bluetooth_msg.data = seq_state_msg;
00244         Bluetooth_publisher.publish(&Bluetooth_msg);
00245         nh.spinOnce();
00246         ThisThread::sleep_for(1000/bt_pub_hz);
00247     }
00248 }
00249 #endif
00250 
00251 void imu_pub_thread() {
00252     const float imu_pub_hz = 100.0;
00253 //    const float imu_pub_hz = 50.0;
00254     imu_msg.header.frame_id = "imu_link";
00255     //mag_msg.header.frame_id = "/imu/mag";
00256     waitTmr.start();
00257     while(waitTmr.read() < 5.0) {;}
00258     while(1) {
00259         imu_msg.header.stamp = nh.now();
00260         imu_msg.orientation.w = gQ[0];
00261         imu_msg.orientation.x = gQ[1];
00262         imu_msg.orientation.y = gQ[2];
00263         imu_msg.orientation.z = gQ[3];
00264               
00265         imu_msg.angular_velocity.x = gGyro_raw.x;
00266         imu_msg.angular_velocity.y = gGyro_raw.y;
00267         imu_msg.angular_velocity.z = gGyro_raw.z;
00268         
00269         imu_msg.linear_acceleration.x = gAcc_raw.x;
00270         imu_msg.linear_acceleration.y = gAcc_raw.y;
00271         imu_msg.linear_acceleration.z = gAcc_raw.z;
00272         
00273         IMU_publisher.publish(&imu_msg);
00274         nh.spinOnce();
00275         
00276         ThisThread::sleep_for(1000/imu_pub_hz);
00277     }
00278 }
00279 
00280 void sonar_pub_thread() {
00281     const float sonar_pub_hz = 15.0;
00282     int ii = 0;
00283     for(;ii < NUM_SONAR; ii++) {
00284         US_msg.data[ii] = 0.0; 
00285     }
00286     while(waitTmr.read() < 5.0) {;}
00287     while(1) {
00288         //memcpy(US_msg.data, (float*)dist, sizeof(float)*NUM_SONAR);
00289         for(ii = 0;ii < NUM_SONAR; ii++) {
00290             US_msg.data[ii] = dist[ii]; 
00291         }
00292         US_publisher.publish(&US_msg);
00293         nh.spinOnce();
00294         ThisThread::sleep_for(1000/sonar_pub_hz);
00295     }  
00296 }
00297 
00298 void estop_pub_thread() {
00299     const float estop_pub_hz = 10; // ms
00300 #if (ROBOT_TYPE == MODEL_I)
00301     LidarStopPin.mode(PullDown);
00302     EmergencyStopPin.mode(PullDown);
00303 #endif
00304     while(waitTmr.read() < 5.0) {;}
00305     while(1) {
00306 #if ((ROBOT_TYPE == MODEL_D) || (ROBOT_TYPE == MODEL_C))
00307         EStop_msg.data = estop;
00308         EStop_publisher.publish(&EStop_msg);
00309         //Bumper_publisher.publish(&Bumper_msg);
00310         nh.spinOnce();
00311 #elif (ROBOT_TYPE == MODEL_I)
00312         uint8_t stop_status = 0;
00313         if(LidarStopPin)
00314         {
00315             stop_status += (0b1 << 0);
00316         }
00317         if(EmergencyStopPin)
00318         {
00319             stop_status += (0b1 << 1);
00320         }
00321         emergency_stop_msg.data = stop_status;
00322         EmergencyStop_publisher.publish(&emergency_stop_msg);
00323         nh.spinOnce();
00324 #endif
00325         ThisThread::sleep_for(1000/estop_pub_hz);
00326     }   
00327 }
00328 
00329 #if (ROBOT_TYPE == MODEL_D)
00330 void level_sensor_thread()
00331 {
00332     const float level_pub_hz = 1;
00333     /*
00334     bool yellow,blue = true;
00335     while(1)
00336     {
00337         water_level_msg.data = 0;
00338         blue = level_blue;
00339         yellow = level_yellow;
00340         if(!blue)
00341         {
00342             water_level_msg.data += 1;
00343         }
00344         if(!yellow)
00345         {
00346             water_level_msg.data += 1;
00347         }
00348         water_level_publisher.publish(&water_level_msg);
00349         ThisThread::sleep_for(1000/level_pub_hz);
00350     }
00351     */
00352     bool sense = false;
00353     while(1)
00354     {
00355         if(level_sensor)
00356         {
00357             water_level_msg.data = true;
00358         }
00359         else
00360         {
00361             water_level_msg.data = false;
00362         }
00363         water_level_publisher.publish(&water_level_msg);
00364         ThisThread::sleep_for(1000/level_pub_hz);
00365     }
00366 }
00367 #endif
00368 
00369 #if (ROBOT_TYPE == MODEL_I)
00370 void lidar_dusty_pub_thread()
00371 {
00372     const float lidar_dust_pub_hz = 10.0f;
00373     int dust_count = 0;
00374     LidarDustSensingPin.mode(PullUp);
00375     while(1)
00376     {
00377         if(!LidarDustSensingPin)
00378         {
00379             dust_count++;
00380         }
00381         else
00382         {
00383             dust_count = 0;
00384         }
00385         if(dust_count > 3)
00386         {
00387             lidar_dusty_msg.data = true;
00388         }
00389         else
00390         {
00391             lidar_dusty_msg.data = false;
00392         }
00393         LidarDusty_publisher.publish(&lidar_dusty_msg);
00394         ThisThread::sleep_for(1000/lidar_dust_pub_hz);
00395     }   
00396 }
00397 #endif
00398 #endif /* #ifdef NO_ROS */
00399 
00400 void bt_thread() {
00401     const float bt_thread_hz = 30.0f;
00402     const float bt_sen_hz    = 10.0f;
00403     const float bt_timeout   = 3.0f;
00404     char temp[2] = {'\0',};
00405     Timer bt_sen_tmr;
00406     Timer bt_timeout_tmr;
00407     bt_sen_tmr.start();
00408     bt_timeout_tmr.start();
00409     while(1)
00410     {
00411         if(bt.readable())
00412         {
00413             bt_data.rec = bt.getc();
00414             bt_timeout_tmr.reset();
00415             bt_timeout_tmr.start();
00416         }
00417         if(bt_sen_tmr.read() > (1.0 / bt_sen_hz))
00418         {
00419             bt_sen_tmr.reset();
00420             bt.putc(bt_data.sen);
00421         }
00422         if(bt_timeout_tmr.read() > bt_timeout)
00423         {
00424             bt_timeout_tmr.stop();
00425             bt_data.rec = 0;
00426             bt_data.sen = 0;
00427         }
00428         ThisThread::sleep_for(1000/bt_thread_hz);
00429     }
00430 }
00431 
00432 void sonar_thread() {
00433     const float sonar_hz = 15.0;
00434     sonar_manager.Begin(sonar_hz);
00435     while(true) {
00436         sonar_manager.GetDistance(dist);
00437         ThisThread::sleep_for(1000/sonar_hz);
00438     }
00439 }
00440 
00441 #define CALIBRATION_MODE 1
00442 void IMU_thread() {
00443     const float imu_hz = 100.0;
00444     Vect3  a, g, m;
00445     mpu.setup();
00446     mpu.enableDataReadyInterrupt();
00447     #if (CALIBRATION_MODE)
00448     calibrationProcess();
00449     #endif
00450     applyCalbratedValue();
00451     
00452     memset(gQ,0.,4*sizeof(float));
00453     
00454     while(true){
00455         if (mpu.isDataReady()){
00456             Vect3  a, g, m;  // acc/gyro/mag vectors
00457             mpu.update(MADGWICK);
00458             a = mpu.getAccelVect();
00459             
00460             
00461             a.x = 0.0;
00462             a.y = 0.0;
00463             a.z = 1.0;
00464             
00465             
00466             g = mpu.getGyroVect();
00467             m = mpu.getMagVect();
00468 #ifdef NO_ROS 
00469             gAcc_raw.x = a.x*G;
00470             gAcc_raw.y = a.y*G;
00471             gAcc_raw.z = a.z*G;
00472             gGyro_raw.x = g.x*DEG_TO_RAD;
00473             gGyro_raw.y = g.y*DEG_TO_RAD;
00474             gGyro_raw.z = g.z*DEG_TO_RAD;
00475             gQ[0] = mpu.getq0();
00476             gQ[1] = mpu.getq1();
00477             gQ[2] = mpu.getq2();
00478             gQ[3] = mpu.getq3();
00479             gRoll = mpu.getRoll()*RAD_TO_DEG;
00480             gPitch = mpu.getPitch()*RAD_TO_DEG;
00481             gYaw = mpu.getYaw()*RAD_TO_DEG;
00482             gTheta = atan2f(gQ[1] * gQ[2] + gQ[0] * gQ[3], 0.5f - gQ[2] * gQ[2] - gQ[3] * gQ[3]) * RAD_TO_DEG;
00483 #else   //   ros
00484             mpu.update(MADGWICK);
00485             a = mpu.getAccelVect();
00486             g = mpu.getGyroVect();
00487             m = mpu.getMagVect();
00488             
00489             gAcc_raw.x = a.x*G;
00490             gAcc_raw.y = a.y*G;
00491             gAcc_raw.z = a.z*G;
00492             gGyro_raw.x = g.x*DEG_TO_RAD;
00493             gGyro_raw.y = g.y*DEG_TO_RAD;
00494             gGyro_raw.z = g.z*DEG_TO_RAD;
00495             
00496             //gMag_raw.x = m.x; // not use for our robot
00497             //gMag_raw.y = m.y;
00498             //gMag_raw.z = m.z;
00499             //gRoll = mpu.getRoll()*RAD_TO_DEG;
00500             //gPitch = mpu.getPitch()*RAD_TO_DEG;
00501             //gYaw = mpu.getYaw()*RAD_TO_DEG;
00502             
00503             gQ[0] = mpu.getq0();
00504             gQ[1] = mpu.getq1();
00505             gQ[2] = mpu.getq2();
00506             gQ[3] = mpu.getq3();
00507             //gTheta = atan2f(gQ[1] * gQ[2] + gQ[0] * gQ[3], 0.5f - gQ[2] * gQ[2] - gQ[3] * gQ[3]) * RAD_TO_DEG;
00508             //char myStr[64] = {'\0',};
00509             //sprintf(myStr,"theta: %f\troll: %f\tpitch: %f\tyaw: %f",gTheta,gRoll,gPitch,gYaw);
00510             //nh.loginfo(myStr);
00511 #endif
00512         }
00513         ThisThread::sleep_for(1000/imu_hz);
00514     }
00515 }
00516 
00517 void test_thread() {
00518     while(true) {
00519         bt.putc('a');
00520         ThisThread::sleep_for(10);
00521         if(bt.readable())
00522         {
00523             char debug_str[6];
00524             sprintf(debug_str,"%c",bt.getc());
00525             nh.loginfo(debug_str);
00526             nh.spinOnce();
00527         }
00528         ThisThread::sleep_for(500);
00529     }   
00530 }
00531 
00532 /* Threads end -------------------------------------------------------------- */
00533 #endif