File content as of revision 0:4ff8aeb3e4d1:

#ifndef ZETA_STM_KINETIC_THREADDECLARATION_H_
#define ZETA_STM_KINETIC_THREADDECLARATION_H_
#include "mbedHeader.hpp"
#include "rosHeader.hpp"
#include "moduleHeader.hpp"
#include "defineHeader.h"
#include "instanceHeader.hpp"
#include "globalVariable.h"
#include "myUtil.hpp"


/* Threads begin ------------------------------------------------------------ */
using myUtil::set_msg;
using myUtil::calibrationProcess;
using myUtil::applyCalbratedValue;

    #if (NO_ROS)
void print_thread() {
    const float time2print = 10.0;
    const float print_thread_hz = 3.0;
    waitTmr.start();
    while(waitTmr.read() < time2print) {;}
    waitTmr.stop();
    waitTmr.reset();
    while(true) {
        /*
        for(int i = 0; i < NUM_SONAR; i++) {
            //pc.printf("%2.2f\t%2.2f\t",dist[i],dist_raw[i]);
        }*/
        pc.printf("\n\r");
        pc.printf("Acc_x:%2.2f\tAcc_y:%2.2f\tAcc_z:%2.2f\n\r",
            gAcc_raw.x,gAcc_raw.y,gAcc_raw.z);
        pc.printf("Gyro_x:%2.2f\tGyro_y:%2.2f\tGyro_z:%2.2f\n\r",
            gGyro_raw.x,gGyro_raw.y,gGyro_raw.z);
        pc.printf("Mag_x:%2.2f\tMag_y:%2.2f\tMag_z:%2.2f\n\r",
            gMag_raw.x,gMag_raw.y,gMag_raw.z);
        pc.printf("quaternion: %2.2f  %2.2f  %2.2f  %2.2f\n\r",
            gQ[0],gQ[1],gQ[2],gQ[3]);
        pc.printf("RPY: %2.2f\t%2.2f\t%2.2f\n\r",gRoll,gPitch,gYaw);
        pc.printf("Theta_z: %2.2f degree\n\r", gTheta);
        ThisThread::sleep_for(1000/print_thread_hz);
    }    
}
    #else
void bt_pub_thread() {
    const float bt_pub_hz = 40.0;
    char seq_state_msg[SEQ_STATE_MSG_SIZE] = {'\0'};
    const char contact[] = "contact";
    const char BT_waiting[] = "BT_waiting";
    const char start[] = "start";
    const char search[] = "search";
    const char adjustment[] = "adjustment";
    const char guidance[] = "guidance";
    const char charging[] = "charging";
    const char finish[] = "finish";
    const char not_connected[] = "not_connected";
    const char left[] = "left";
    const char right[] = "right";
    const char disconnected[] = "disconnected";
    char* buff = NULL;
    enum class AUTO_CHARGE_STATE : int {
        start = 1,
        BT_waiting = 1,
        search,
        adjustment,
        guidance,
        charging,
        finish,
    };
    relay.off();
    while(waitTmr.read() < 5.0) {;}
    while(1) {
        switch(NUC_sub_state) {
            case static_cast<int>(AUTO_CHARGE_STATE::start):
                if(bt_data.rec == 1 || bt_data.rec == 2) {
                    set_msg(seq_state_msg, start, sizeof(start));
                    relay.off();
                    bt_data.sen = 3;  
                }
                else {
                    set_msg(seq_state_msg, BT_waiting, sizeof(BT_waiting));
                    relay.off();
                    bt_data.sen = 2;
                }
                break;
            case static_cast<int>(AUTO_CHARGE_STATE::search):
                set_msg(seq_state_msg, search, sizeof(search));
                relay.off();
                bt_data.sen = 4;
                break;
            case static_cast<int>(AUTO_CHARGE_STATE::adjustment):
                set_msg(seq_state_msg, adjustment, sizeof(adjustment));
                relay.off();
                bt_data.sen = 5;
                break;
            case static_cast<int>(AUTO_CHARGE_STATE::guidance):
                set_msg(seq_state_msg, guidance, sizeof(guidance));
                bt_data.sen = 6;
                break;
            case static_cast<int>(AUTO_CHARGE_STATE::charging):
                set_msg(seq_state_msg, charging, sizeof(charging));
                bt_data.sen = 7;
                break;
            case static_cast<int>(AUTO_CHARGE_STATE::finish):
                set_msg(seq_state_msg, finish, sizeof(finish));
                bt_data.sen = 8;
                break;    
        }
        
        if(strstr(seq_state_msg,guidance) != NULL) {
            switch(bt_data.rec) {
                case 69:
                    set_msg(seq_state_msg, contact, sizeof(contact));
                    relay.on();
                    break;
                case 66:
                    set_msg(seq_state_msg, left, sizeof(left));
                    relay.off();   
                    break;
                case 63:
                    set_msg(seq_state_msg, right, sizeof(right));
                    relay.off();
                    break;
                default:
                    set_msg(seq_state_msg, not_connected, sizeof(not_connected));
                    relay.off();
                    break;
            }
        }
        else if(strstr(seq_state_msg,charging) != NULL) {
            switch(bt_data.rec) {
                case 79:
                    set_msg(seq_state_msg, contact, sizeof(contact));
                    relay.on();
                    break;
                default:
                    set_msg(seq_state_msg, disconnected, sizeof(disconnected));
                    relay.off();  
            }    
        }
        Bluetooth_msg.data = seq_state_msg;
        Bluetooth_publisher.publish(&Bluetooth_msg);
        nh.spinOnce();
        ThisThread::sleep_for(1000/bt_pub_hz);
    }
}


void imu_pub_thread() {
    const float imu_pub_hz = 40.0;
    imu_msg.header.frame_id = "imu_link";
    //mag_msg.header.frame_id = "/imu/mag";
    while(waitTmr.read() < 5.0) {;}
    while(1) {
        imu_msg.header.stamp = nh.now();
        imu_msg.orientation.w = gQ[0];
        imu_msg.orientation.x = gQ[1];
        imu_msg.orientation.y = gQ[2];
        imu_msg.orientation.z = gQ[3];
              
        imu_msg.angular_velocity.x = gGyro_raw.x;
        imu_msg.angular_velocity.y = gGyro_raw.y;
        imu_msg.angular_velocity.z = gGyro_raw.z;
        
        imu_msg.linear_acceleration.x = gAcc_raw.x;
        imu_msg.linear_acceleration.y = gAcc_raw.y;
        imu_msg.linear_acceleration.z = gAcc_raw.z;
        
        IMU_publisher.publish(&imu_msg);
        nh.spinOnce();
        
        ThisThread::sleep_for(1000/imu_pub_hz);
    }
}

void sonar_pub_thread() {
    const float sonar_pub_hz = 15.0;
    int ii = 0;
    for(;ii < NUM_SONAR; ii++) {
        US_msg.data[ii] = 0.0; 
    }
    while(waitTmr.read() < 5.0) {;}
    while(1) {
        //memcpy(US_msg.data, (float*)dist, sizeof(float)*NUM_SONAR);
        for(ii = 0;ii < NUM_SONAR; ii++) {
            US_msg.data[ii] = dist[ii]; 
        }
        US_publisher.publish(&US_msg);
        nh.spinOnce();
        ThisThread::sleep_for(1000/sonar_pub_hz);
    }  
}

void estop_pub_thread() {
    const float estop_pub_hz = 5; // ms
    while(waitTmr.read() < 5.0) {;}
    while(1) {
        /*
        if(bumperL.isTouch() || bumperC.isTouch() || bumperR.isTouch()) Bumper_msg.data = true;
        else Bumper_msg.data = false;
        */
        EStop_msg.data = estop.isTouch();
        EStop_publisher.publish(&EStop_msg);
        Bumper_publisher.publish(&Bumper_msg);
        nh.spinOnce();
        ThisThread::sleep_for(1000/estop_pub_hz);
    }   
}
    #endif

void bt_thread() {
    const float bt_thread_hz = 100.0;
    const float sen_period   = 100.0;
    const int sen_cnt = int(sen_period * bt_thread_hz / 1000.0);
    int cnt = 0;
    char temp[2] = {'\0',};
    while(1) {
        if(bt.readable()) bt_data.rec = bt.getc();
        if(++cnt % 10 == 0) {
            bt.putc(bt_data.sen);
            cnt = 0;
        }
        ThisThread::sleep_for(1000/bt_thread_hz);
    }   
}

void sonar_thread() {
    const float sonar_hz = 15.0;
    for(int i = 0 ; i < NUM_SONAR; i++) {
        sonar[i].setRanges(3, 70);
    }
    while(true) {
        for(int i = 0; i < NUM_SONAR; i++) {
            if(sonar[i].isNewDataReady()) {
                dist[i] = sonar[i].getDistance_cm();
                //dist_raw[i] = sonar[i]->getDistance_raw();
            }
            else sonar[i].startMeasurement();
        }
        ThisThread::sleep_for(1000/sonar_hz);
    }
}

void module_thread() {
    const float module_hz = 10.0;
    while(1) {
        if(isSubscribe) {
            char send_str[32] = {'\0',};
            module.setMsg(&moduleControlMsg);
            module.control();
            sprintf(send_str,"rec: %d|%d|%d|%d|%u",moduleControlMsg.module_power[0],moduleControlMsg.module_power[1],
            moduleControlMsg.module_power[2],moduleControlMsg.module_power[3],moduleControlMsg.pulifier);
            nh.loginfo(send_str);
            isSubscribe = false;
        }
        ThisThread::sleep_for(1000/module_hz);
    }
    
    ;  
}

#define CALIBRATION_MODE 1
void IMU_thread() {
    const float imu_hz = 100.0;
    Vect3  a, g, m;
    mpu.setup();
    mpu.enableDataReadyInterrupt();
    #if (CALIBRATION_MODE)
    calibrationProcess();
    #endif
    applyCalbratedValue();
    
    memset(gQ,0.,4*sizeof(float));
    
    while(true){
        if (mpu.isDataReady()){
            Vect3  a, g, m;  // acc/gyro/mag vectors
            mpu.update(MADGWICK);
            a = mpu.getAccelVect();
            g = mpu.getGyroVect();
            m = mpu.getMagVect();
    #if (NO_ROS)    
            gAcc_raw.x = a.x*G;
            gAcc_raw.y = a.y*G;
            gAcc_raw.z = a.z*G;
            gGyro_raw.x = g.x*DEG_TO_RAD;
            gGyro_raw.y = g.y*DEG_TO_RAD;
            gGyro_raw.z = g.z*DEG_TO_RAD;
            gQ[0] = mpu.getq0();
            gQ[1] = mpu.getq1();
            gQ[2] = mpu.getq2();
            gQ[3] = mpu.getq3();
            gRoll = mpu.getRoll()*RAD_TO_DEG;
            gPitch = mpu.getPitch()*RAD_TO_DEG;
            gYaw = mpu.getYaw()*RAD_TO_DEG;
            gTheta = atan2f(gQ[1] * gQ[2] + gQ[0] * gQ[3], 0.5f - gQ[2] * gQ[2] - gQ[3] * gQ[3]) * RAD_TO_DEG;
    #else   //   ros
            mpu.update(MADGWICK);
            a = mpu.getAccelVect();
            g = mpu.getGyroVect();
            m = mpu.getMagVect();
            
            gAcc_raw.x = a.x*G;
            gAcc_raw.y = a.y*G;
            gAcc_raw.z = a.z*G;
            gGyro_raw.x = g.x*DEG_TO_RAD;
            gGyro_raw.y = g.y*DEG_TO_RAD;
            gGyro_raw.z = g.z*DEG_TO_RAD;
            
            //gMag_raw.x = m.x; // not use for our robot
            //gMag_raw.y = m.y;
            //gMag_raw.z = m.z;
            //gRoll = mpu.getRoll()*RAD_TO_DEG;
            //gPitch = mpu.getPitch()*RAD_TO_DEG;
            //gYaw = mpu.getYaw()*RAD_TO_DEG;
            
            gQ[0] = mpu.getq0();
            gQ[1] = mpu.getq1();
            gQ[2] = mpu.getq2();
            gQ[3] = mpu.getq3();
            //gTheta = atan2f(gQ[1] * gQ[2] + gQ[0] * gQ[3], 0.5f - gQ[2] * gQ[2] - gQ[3] * gQ[3]) * RAD_TO_DEG;
            //char myStr[64] = {'\0',};
            //sprintf(myStr,"theta: %f\troll: %f\tpitch: %f\tyaw: %f",gTheta,gRoll,gPitch,gYaw);
            //nh.loginfo(myStr);
    #endif
        }
        ThisThread::sleep_for(1000/imu_hz);
    }
}
extern RELAY relay;
void test_thread() {
    while(true) {
        ;
    }   
}

/* Threads end -------------------------------------------------------------- */
#endif