File content as of revision 12:82b8fe254222:

#include "mbed.h"
#include "General.hpp"
#include "ros.h"
#include "rtos.h"
#include "ROS_Handler.hpp"
#include <motordriver.h>
#include "math.h"
#include "Motors.hpp"
#include "Pins.h"
#include "VL6180.hpp"

extern Motor A; // pwm, fwd, rev, can brake
extern Motor B; // pwm, fwd, rev, can brake

/******************************************************************************/
/*  ROS serial hardware setup                                                 */
/******************************************************************************/
// Create the ROS node handle
class HaseHardware : public MbedHardware
{
public:
    HaseHardware(): MbedHardware(ROS_Tx, ROS_Rx, ROS_BaudRate) {};
};
/******************************************************************************/

ros::NodeHandle_<HaseHardware>   Node_;
ros::Subscriber<std_msgs::Float32MultiArray> Control_Subscriber(Control_Topic, &Control_CallBack);
std_msgs::String State_msg;
std_msgs::String TOF_msg;
ros::Publisher Status_PUB(Status_Topic, &State_msg);
ros::Publisher TOF_PUB(TOF_Topic, &TOF_msg);

extern Mutex Global_Battery_Level_Mutex;
extern float Global_Battery_Level;
extern float Global_Battery_Value;
extern bool Battery_Update_Flag;

extern int Global_TOF_Ranges[num_VL6180];
extern bool TOF_Range_Flag;
extern Mutex Global_TOF_Range_Mutex;

float LIN_Val, ANG_Val;
Mutex Duty_Mutex;

// Main function for thread to handle ROS comms
void ROS_Handler(void){
    LIN_Val = 0;
    ANG_Val = 0;
       
    Node_.initNode();
    Node_.advertise(Status_PUB);
    Node_.advertise(TOF_PUB);
    Node_.subscribe(Control_Subscriber);

    while (1) {
        Global_Battery_Level_Mutex.lock();
        if(Battery_Update_Flag == true)
        {
            float Battery_Level = Global_Battery_Level;
            float Battery_Value = Global_Battery_Value;
            Battery_Update_Flag = false;
            Global_Battery_Level_Mutex.unlock();
            char buffer[50];
            Duty_Mutex.lock();
            sprintf(buffer, "Battery Level : %1.3f%     | Battery Voltage %1.3fV    | Linear Duty %1.3f     | Angular Duty %1.3f    |", (Battery_Level*100), Battery_Value, LIN_Val, ANG_Val);
            Duty_Mutex.unlock();
            State_msg.data = buffer;
            Status_PUB.publish(&State_msg);
        } else {
          Global_Battery_Level_Mutex.unlock();  
        }
        
        Global_TOF_Range_Mutex.lock();
        if(TOF_Range_Flag == true)
        {
            int TOF_Range[num_VL6180];
            for(int IDX = 0; IDX < num_VL6180; IDX++) {
                TOF_Range[IDX] = Global_TOF_Ranges[IDX];
            }
            TOF_Range_Flag = false;
            Global_TOF_Range_Mutex.unlock();
            char buffer[50];
            sprintf(buffer, "Range 1: %d   | Range 2: %d    | Range 3: %d   | Range 4: %d", TOF_Range[0], TOF_Range[1], TOF_Range[2], TOF_Range[3]);
            TOF_msg.data = buffer;
            TOF_PUB.publish(&State_msg);
        } else {
          Global_TOF_Range_Mutex.unlock();  
        }       
        
        Node_.spinOnce();
        wait_ms(20);     
    }
}

int Remap(float value, float start1, float stop1, float start2, float stop2) 
{
    return (int)(start2 + (stop2 - start2) * ((value - start1) / (stop1 - start1)));
}



void Control_CallBack(const std_msgs::Float32MultiArray& cmd_vel_msg)
{
    /**************************************************************************/
    //Extracting the only commands that will be used in the multiarray, and assuming that the int value attached to each point is 100 times
    //the intended PWM constant for that movement command (e.g. a value of 50 in msg.data[6] would give a PWM duty cycle of 0.5 for rotation):
    float xlin, zang;
    if((cmd_vel_msg.data[0] <= 1.0f) && (cmd_vel_msg.data[0] >= -1.0f))
    {
        xlin = cmd_vel_msg.data[0];
        Duty_Mutex.lock();
        LIN_Val = xlin;
        Duty_Mutex.unlock();;
    }
    if((cmd_vel_msg.data[1] <= 1.0f) && (cmd_vel_msg.data[1] >= -1.0f))
    {
        zang = cmd_vel_msg.data[1];
        Duty_Mutex.lock();
        ANG_Val = zang;
        Duty_Mutex.unlock();
    }
    
    //assume rotation needs to be taken care of first, generally, then linear movement for this basic controller
    //assume positive z is clockwise, negative is anticlockwise, A is left motor, B is right motor (viewed from bottom layer battery switch direction)
    if(zang == 0.0f && xlin == 0.0f) {
        A.stop(0.5f);
        B.stop(0.5f);
        wait(1);
        A.coast();
        B.coast();
    } else if(zang != 0.0f && xlin !=0.0f) {
        // Needs testing
        A.speed((-xlin + zang)/2.0f);
        B.speed((-xlin + -zang)/2.0f);
    } else if(xlin != 0) {
        A.speed(-xlin);
        B.speed(-xlin);
    } else if(zang != 0) {
        A.speed(zang);
        B.speed(-zang);
    }
    /**************************************************************************/
}