File content as of revision 10:7d954fba5e7a:

#include "PROJ515.hpp" // Contains Libraries, Definitions & Function Prototypes

float cfDist1 = 0;     // Distance returned by cliff u_sensor1 (m)
float cfDist2 = 0;     // Distance returned by cliff u_sensor2 (m)
float cfDist3 = 0;     // Distance returned by cliff u_sensor3 (m)
float cfDist4 = 0;     // Distance returned by cliff u_sensor4 (m)

float cbDist1 = 0;     // Distance returned by curb  u_sensor1 (m)
float cbDist2 = 0;     // Distance returned by curb  u_sensor2 (m)
float cbDist3 = 0;     // Distance returned by curb  u_sensor3 (m)
float cbDist4 = 0;     // Distance returned by curb  u_sensor4 (m)

float wPeriod = 10;    // Wait period(ms) for ROS publisher to finish publishing

char gps_c = 0;                                    // GPS serial character
int  sats_n = 0;                                    // GPS Satelite count

int main()
{
    nh.getHardware()->setBaud(ROS_Baud);    // Set Baud Rate for ROS Serial
    nh.initNode();                          // Initialise ROS Node Handler
    
    advRosPub();                            // Adverstise Ultrasound Topic
    nh.advertise(gps_odom_pub);             // Adverstise Odometry topic
    
    nh.subscribe(AudioStatus);
    nh.subscribe(Velocity);
    
    setupRosMsg();                          // Setup the message constant values
    gps_odom_msg.header.frame_id = frameID_g;        // Pack ROS frame ID
    gps_odom_msg.status.status = UNAGUMENTED;      // Location fix method
    gps_odom_msg.status.service = GPS_CONSTELL;    // Satelite constellation used
    gps_odom_msg.altitude  = 0;                    // Altitude (Always fixed)
    gps_odom_msg.position_covariance[ 0] = CVX;    // X pos covariance
    gps_odom_msg.position_covariance[ 1] = CVO;    // Zero
    gps_odom_msg.position_covariance[ 2] = CVO;    // Zero
    gps_odom_msg.position_covariance[ 3] = CVO;    // Zero
    gps_odom_msg.position_covariance[ 4] = CVY;    // Y pos covariance
    gps_odom_msg.position_covariance[ 5] = CVO;    // Zero
    gps_odom_msg.position_covariance[ 6] = CVO;    // Zero
    gps_odom_msg.position_covariance[ 7] = CVO;    // Zero
    gps_odom_msg.position_covariance[ 8] = CVZ;    // Z pos covariance
    gps_odom_msg.position_covariance_type = UNNWN; // Covariance type
    gpsSer.baud(GPS_Baud);                         // Set Baud rate for GPS Serial
    
    int LEDBrightness = 128;
    float led_tmp = LED_COUNT/4;
    int led_num = floor(led_tmp);
    int k = 0;
    rgb_color led_colour = (rgb_color){0, 0, 0};
    
    while(!nh.connected()) {                // While node handler is !connected
        nh.spinOnce();                      // Attempt to connect & synchronise
    }

    while(true) {
        startSensors();                     // Start the Sensors
        recordDist();                       // Record the Distances (meters)
        checkCliff();                       // Check the Cliff Sensors Readings
        setRosDist();                       // Put distance in ROS messages
        setRosStamp();                      // Put time into ROS messages
        publishRosMsg();                    // Publish ROS messages
        
        if(gpsSer.readable()) {                    // If serial buffer has character
            gps_c = gpsSer.getc();                 // Read serial buffer and store character
            gpsModule.encode(gps_c);               // Encode character from GPS

        } else {
            leds = leds & 0b100;                   // Flash LED[0,1] bus OFF. Leave LED[2] on if its on to indicate that the signal was present
        }
        if (gpsModule.location.isValid()) {        // If GPS location is Valid
            gps_odom_msg.latitude  = gpsModule.location.lng(); // Get Longtitude
            gps_odom_msg.longitude = gpsModule.location.lat(); // Get Latitude
            nh.spinOnce(); // Reccuring connect and synchronise
            gps_odom_msg.header.stamp = nh.now();      // Get current time
            gps_odom_pub.publish(&gps_odom_msg);       // Publish the Odometry message
        }
        sats_n = gpsModule.satellites.value();     // Aquire satelite number
        if (sats_n > 0){
            leds = LEDS_ON;
        }
        
        nh.spinOnce();
        if (((abs(vel) || abs(ang)) > 0.1f) && (audio_state == "Playing")){
            if(abs(vel) >= abs(ang)) {
                LEDBrightness = floor(Map(abs(vel), 0, MaxVelocity, 0, 255));
            } else {
                LEDBrightness = floor(Map(abs(ang), 0, MaxVelocity, 0, 255));
            }
            led_colour = (rgb_color) {0, 0, 255};
            for(int i = 0; i < led_num; i++){
                colors[i] = led_colour;
            }
            led_colour = (rgb_color) {LEDBrightness, 0, 0};
            for(int i = led_num; i < ((3*led_num)+1); i++){
                colors[i] = led_colour;
            }
            led_colour = (rgb_color) {0, 0, 255};
            for(int i = ((3*led_num)+1); i < LED_COUNT; i++){
                colors[i] = led_colour;
            }
        } else if((abs(vel) || abs(ang)) > 0.1f) {     //If Not In DeadZone (Robot Is Moving)
            if(abs(vel) >= abs(ang)) {
                LEDBrightness = floor(Map(abs(vel), 0, MaxVelocity, 0, 255));
            } else {
                LEDBrightness = floor(Map(abs(ang), 0, MaxVelocity, 0, 255));
            }
            led_colour = (rgb_color) {0, LEDBrightness, 0};
            for(int i = 0; i < LED_COUNT; i++){
                colors[i] = led_colour;
            }
        } else if (audio_state == "Playing") {           //Else If Audio Is Playing
            led_colour = (rgb_color) {0, 0, 255};
            for(int i = 0; i < LED_COUNT; i++){
                colors[i] = led_colour;
            }
        } else {                                        //Robot Is Neither Moving Nor Playing Audio
            led_colour = (rgb_color) {255, 0, 0};
            for(int i = 0; i < LED_COUNT; i++){
                colors[i] = led_colour;
            }
        }
             
        ledStripFront.write(colors, LED_COUNT);
        ledStripBack.write(colors, LED_COUNT);
        wait_ms(5);
    }
}

/*============================================================================*/
/*                                  FUNCTIONS                                 */
/*============================================================================*/


void advRosPub()
{
    // Adverstise Ultrasound topic
    nh.advertise(ultrasound_pub_cf1);
    nh.advertise(ultrasound_pub_cf2);
    nh.advertise(ultrasound_pub_cf3);
    nh.advertise(ultrasound_pub_cf4);
    nh.advertise(ultrasound_pub_cb1);
    nh.advertise(ultrasound_pub_cb2);
    nh.advertise(ultrasound_pub_cb3);
    nh.advertise(ultrasound_pub_cb4);
}

void setupRosMsg()
{
    // Setup the message constant values

    // Frame ID
    ultrasound_msg_cf1.header.frame_id = "uss_cf1";
    ultrasound_msg_cf2.header.frame_id = "uss_cf2";
    ultrasound_msg_cf3.header.frame_id = "uss_cf3";
    ultrasound_msg_cf4.header.frame_id = "uss_cf4";
    ultrasound_msg_cb1.header.frame_id = "uss_cb1";
    ultrasound_msg_cb2.header.frame_id = "uss_cb2";
    ultrasound_msg_cb3.header.frame_id = "uss_cb3";
    ultrasound_msg_cb4.header.frame_id = "uss_cb4";

    // Radiation Type
    ultrasound_msg_cf1.radiation_type = ULTRASOUND;
    ultrasound_msg_cf2.radiation_type = ULTRASOUND;
    ultrasound_msg_cf3.radiation_type = ULTRASOUND;
    ultrasound_msg_cf4.radiation_type = ULTRASOUND;
    ultrasound_msg_cb1.radiation_type = ULTRASOUND;
    ultrasound_msg_cb2.radiation_type = ULTRASOUND;
    ultrasound_msg_cb3.radiation_type = ULTRASOUND;
    ultrasound_msg_cb4.radiation_type = ULTRASOUND;

    // Field of View
    ultrasound_msg_cf1.field_of_view = FOV;
    ultrasound_msg_cf2.field_of_view = FOV;
    ultrasound_msg_cf3.field_of_view = FOV;
    ultrasound_msg_cf4.field_of_view = FOV;
    ultrasound_msg_cb1.field_of_view = FOV;
    ultrasound_msg_cb2.field_of_view = FOV;
    ultrasound_msg_cb3.field_of_view = FOV;
    ultrasound_msg_cb4.field_of_view = FOV;

    // Minumum Range
    ultrasound_msg_cf1.min_range = MIN_RANGE;
    ultrasound_msg_cf2.min_range = MIN_RANGE;
    ultrasound_msg_cf3.min_range = MIN_RANGE;
    ultrasound_msg_cf4.min_range = MIN_RANGE;
    ultrasound_msg_cb1.min_range = MIN_RANGE;
    ultrasound_msg_cb2.min_range = MIN_RANGE;
    ultrasound_msg_cb3.min_range = MIN_RANGE;
    ultrasound_msg_cb4.min_range = MIN_RANGE;

    // Maximum Range
    ultrasound_msg_cf1.max_range = MAX_RANGE;
    ultrasound_msg_cf2.max_range = MAX_RANGE;
    ultrasound_msg_cf3.max_range = MAX_RANGE;
    ultrasound_msg_cf4.max_range = MAX_RANGE;
    ultrasound_msg_cb1.max_range = MAX_RANGE;
    ultrasound_msg_cb2.max_range = MAX_RANGE;
    ultrasound_msg_cb3.max_range = MAX_RANGE;
    ultrasound_msg_cb4.max_range = MAX_RANGE;
}

void startSensors()
{
    // Start the Sensors
    USS_CF1.start();
    USS_CF2.start();
    USS_CF3.start();
    USS_CF4.start();
    USS_CB1.start();
    USS_CB2.start();
    USS_CB3.start();
    USS_CB4.start();
}

void recordDist()
{
    // Record the Distances (meters)
    cfDist1 = USS_CF1.get_dist();
    cfDist2 = USS_CF2.get_dist();
    cfDist3 = USS_CF3.get_dist();
    cfDist4 = USS_CF4.get_dist();
    cbDist1 = USS_CB1.get_dist();
    cbDist2 = USS_CB2.get_dist();
    cbDist3 = USS_CB3.get_dist();
    cbDist4 = USS_CB4.get_dist();
}

void checkCliff()
{
    /* Check the Cliff Sensors (If distance more than clif treshold,
    hardcode the cliff value in, else no obstacle)*/
    if (static_cast<double>(cfDist1) <= CLIFF_TRH) {
        cfDist1 = CLIFF_ZRO;
    } else {
        cfDist1 = EXCL_ZONE;
    }
    if (static_cast<double>(cfDist2) <= CLIFF_TRH) {
        cfDist2 = CLIFF_ZRO;
    } else {
        cfDist2 = EXCL_ZONE;
    }
    if (static_cast<double>(cfDist3) <= CLIFF_TRH) {
        cfDist3 = CLIFF_ZRO;
    } else {
        cfDist3 = EXCL_ZONE;
    }
    if (static_cast<double>(cfDist4) <= CLIFF_TRH) {
        cfDist4 = CLIFF_ZRO;
    } else {
        cfDist4 = EXCL_ZONE;
    }
}

void setRosDist()
{
    // Put distance in ROS messages
    ultrasound_msg_cf1.range = cfDist1;
    ultrasound_msg_cf2.range = cfDist2;
    ultrasound_msg_cf3.range = cfDist3;
    ultrasound_msg_cf4.range = cfDist4;
    ultrasound_msg_cb1.range = cbDist1;
    ultrasound_msg_cb2.range = cbDist2;
    ultrasound_msg_cb3.range = cbDist3;
    ultrasound_msg_cb4.range = cbDist4;
}

void setRosStamp()
{
    // Get current time and put into ROS messages
    nh.spinOnce(); // Reccuring connect and synchronise
    ultrasound_msg_cf1.header.stamp = nh.now();
    ultrasound_msg_cf2.header.stamp = nh.now();
    ultrasound_msg_cf3.header.stamp = nh.now();
    ultrasound_msg_cf4.header.stamp = nh.now();
    ultrasound_msg_cb1.header.stamp = nh.now();
    ultrasound_msg_cb2.header.stamp = nh.now();
    ultrasound_msg_cb3.header.stamp = nh.now();
    ultrasound_msg_cb4.header.stamp = nh.now();
}

void publishRosMsg()
{
    // Publish ROS messages
    ultrasound_pub_cf1.publish(&ultrasound_msg_cf1);
    wait_ms(wPeriod);
    ultrasound_pub_cf2.publish(&ultrasound_msg_cf2);
    wait_ms(wPeriod);
    ultrasound_pub_cf3.publish(&ultrasound_msg_cf3);
    wait_ms(wPeriod);
    ultrasound_pub_cf4.publish(&ultrasound_msg_cf4);
    wait_ms(wPeriod);
    ultrasound_pub_cb1.publish(&ultrasound_msg_cb1);
    wait_ms(wPeriod);
    ultrasound_pub_cb2.publish(&ultrasound_msg_cb2);
    wait_ms(wPeriod);
    ultrasound_pub_cb3.publish(&ultrasound_msg_cb3);
    wait_ms(wPeriod);
    ultrasound_pub_cb4.publish(&ultrasound_msg_cb4);
    wait_ms(wPeriod);
}

void AudioStatusCB(const std_msgs::String &status) {
    audio_state = status.data;
}

oid VelocityCB(const nav_msgs::Odometry &odom) {
    vel = odom.twist.twist.linear.x;
    ang = odom.twist.twist.angular.z;
}

float Map(float x, float in_min, float in_max, float out_min, float out_max){
    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}