File content as of revision 15:d26162b5b9b0:

#include "wheelchair.h"
 
/* Intializes the right encoder */
QEI wheelS(D9, D10, NC, 1200);
/* Set pull-up resistors to read analog signals into digital signals */
DigitalIn pt3(D10, PullUp);
DigitalIn pt4(D9, PullUp);
 
/* Initializes Left encoder */
QEI wheel (D7, D8, NC, 1200);
/* Set pull-up resistors to read analog signals into digital signals */
DigitalIn pt1(D7, PullUp);
DigitalIn pt2(D8, PullUp);
 
/* Initializes analog axis for the joystick */
AnalogIn x(A0);
AnalogIn y(A1);
 
double test1;
double test2;
 
/* Initializing more pins for wheelchair control */
DigitalOut up(D2);                                                  //Turn up speed mode for joystick
DigitalOut down(D8);                                                //Turn down speed mode for joystick
DigitalOut on(D12);                                                 //Turn Wheelchair On
DigitalOut off(D11);                                                //Turn Wheelchair Off
 
/*************************************************************************
* Initializing Time of Flight sensors with respective Nucleo digital pins*
**************************************************************************/
//Parameter: SDA, SCL, Digital Pin
VL53L1X sensor1(PD_13, PD_12, PA_15);   // Block 1 
VL53L1X sensor2(PD_13, PD_12, PC_7);
VL53L1X sensor3(PD_13, PD_12, PB_5);
 
VL53L1X sensor4(PD_13, PD_12, PE_11);   // Block 2
VL53L1X sensor5(PD_13, PD_12, PF_14);
VL53L1X sensor6(PD_13, PD_12, PE_13);
 
VL53L1X sensor7(PD_13, PD_12, PG_15);   // Block 3
VL53L1X sensor8(PD_13, PD_12, PG_14);
VL53L1X sensor9(PD_13, PD_12, PG_9);
 
VL53L1X sensor10(PD_13, PD_12, PE_10);  // Block 4
VL53L1X sensor11(PD_13, PD_12, PE_12);     
VL53L1X sensor12(PD_13, PD_12, PE_14);
 
 
/*************************************************************************
*      Creating a pointer to point to the addressed of the sensors       *
*      To print value, you need to dereference. Ex: *Tof[0]              *
**************************************************************************/
VL53L1X* ToF[12] = {&sensor1, &sensor2, &sensor3, &sensor4, &sensor5, &sensor6,
                    &sensor7, &sensor8, &sensor9, &sensor10, &sensor11, &sensor12
                   };
// Creating a double pointer to point to the array.
VL53L1X** ToFT = ToF;
int ToFV[12];
/* Changes the control mode of wheelchair: Automatic or Manual */
bool manual = false;
 
Serial pc(USBTX, USBRX, 57600);                                     //Serial Monitor
Timer t;                                                            //Initialize time object t
EventQueue queue;                                                   //Class to organize threads
 
/******************************************************************************************
 * Instantiate node handle, which allows our program to create publishers and subscribers *
 * This also takes care of serial port communication. Always needs to be included!!       *
 ******************************************************************************************/
ros::NodeHandle nh;
 
/******************************************************************************************
 *                  Instantiate the message instance to be used for publishing            *
 ******************************************************************************************/
nav_msgs::Odometry odom;
geometry_msgs::Twist commandRead;
std_msgs::Float64MultiArray tof_sensors;
std_msgs::Float64 left_encoder;
std_msgs::Float64 right_encoder;
/******************************************************************************************
 * Creating the callback function for our Subcriber.                                      *
 *   Our message name will be command. Here we reference command to one of our Publishers *
 ******************************************************************************************/
void handlerFunction(const geometry_msgs::Twist& command)
{
    commandRead = command;
}
 
/*******************************************************************************************
 * Instantaiate a Subcriber with a topic of name "cmd_vel" and type geometry_msgs::Twist   *
 * Its two arguments are the topic it will be subscribing to and the callback function it  *
 * will be using. "sub(...)" is just the name of the subscriber.                           *
 *******************************************************************************************/
ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &handlerFunction);
 
 
 
/*******************************************************************************************
 * Instantiate a Publisher with a topic name of "odom". The second parameter to Publisher  *
 * is a reference to the message instance to be used for publishing                        *
 *******************************************************************************************/
ros::Publisher chatter("odom", &odom);
//ros::Publisher chatter2("cmd_vel", &commandRead);
ros::Publisher sensor_chatter("tof_sensors", &tof_sensors);
ros::Publisher left_encoder_chatter("left_encoder", &left_encoder);
ros::Publisher right_encoder_chatter("right_encoder", &right_encoder);
 
/* Initialize Wheelchair objects and threads */
Wheelchair smart(xDir,yDir, &pc, &t, &wheel, &wheelS, ToFT); //Initialize wheelchair object
Thread compass;
Thread velocity;
Thread ros_com;
Thread tof_encoder_roscom;
Thread assistSafe;
/* This thread continues the communication with ROS and Mbed */
void rosCom_thread();
void tof_encoder_roscom_thread();
 
int main(void)
{
    /* Initialize ROS commands to publish and subscribe to nodes */
    nh.initNode();
 
    /* We are initializing the multi-array dimension and size         *
     * Dimension for outputing label, size, and stride does not work. */
    tof_sensors.data_length = 12;
    tof_sensors.layout.dim[0].label = "sensors";
    tof_sensors.layout.dim[0].size = 12;
    tof_sensors.layout.dim[0].stride = 3;
    tof_sensors.layout.data_offset = 0;
 
 
    /******************************************************************************************
    *                     We are advertising any topics being published.                      *
    *******************************************************************************************/
    nh.advertise(chatter);
    //nh.advertise(chatter2);
    nh.advertise(sensor_chatter);
    nh.advertise(left_encoder_chatter);
    nh.advertise(right_encoder_chatter);
    
    /******************************************************************************************
    *                     We are subscribing to any topics we wish to listen to.              *
    *******************************************************************************************/
    nh.subscribe(sub);
 
    /* Sets up sampling frequency of threads */
    queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::compass_thread);
    queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::velocity_thread);
    queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::assistSafe_thread);   
    queue.call_every(200, rosCom_thread);
    queue.call_every(200, tof_encoder_roscom_thread);
    
    /* Reset the time */
    t.reset();                
                                                
    /* Start running threads */
    compass.start(callback(&queue, &EventQueue::dispatch_forever));
    velocity.start(callback(&queue, &EventQueue::dispatch_forever));
    assistSafe.start(callback(&queue, &EventQueue::dispatch_forever));    
    ros_com.start(callback(&queue, &EventQueue::dispatch_forever));
    tof_encoder_roscom.start(callback(&queue, &EventQueue::dispatch_forever));
 
    while(1) {
        /* If Ros comands the wheelchair to move fowards or backwards*/
        if(commandRead.linear.x != 0) {
            smart.pid_twistV();                                           //Updates the twist linear velocity of chair
            test1 = 3.14159;
        }
        /* If Ros comands the wheelchair to turn at a certain speed*/
        else if(commandRead.angular.z != 0) {
            smart.pid_twistA();                                       //Updates the twist angular velocity of chair
            test2 = 2.782;
        }
        /* If Ros does not give velocity comands*/
        else {
            smart.stop();                                                 //Stops the chair
            test2 = 0;
            test1 = 0;
        }
        wait(process);                                                    //Delay
    }
 
}
 
/******************************************************************************************
 * This thread allows for continuous update and publishing of ROS Odometry/Twist message  *
 ******************************************************************************************/
void rosCom_thread()
{
    /*Determines linear and angular velocity */
    smart.linearV = commandRead.linear.x;
    smart.angularV = commandRead.angular.z*180/3.1415926;
 
    /* Publishes the position of the Wheelchair for Odometry */
    odom.pose.pose.position.x = smart.x_position;
    odom.pose.pose.position.y = smart.y_position;
    odom.pose.pose.position.z = 0;
 
    /* Store the orientation of the Wheelchair for Odometry */
    odom.pose.pose.orientation.z = sin(smart.z_angular*.5*3.1415926/180);
    odom.pose.pose.orientation.x = 0;
    odom.pose.pose.orientation.y = 0;
    odom.pose.pose.orientation.w = cos(smart.z_angular*.5*3.1415926/180);
 
    /* Store Twist linear velocity of Wheelchair */
    odom.twist.twist.linear.x = smart.vel;
    odom.twist.twist.linear.y = commandRead.linear.x;
    odom.twist.twist.linear.z = commandRead.angular.z;
 
    /* Store Twist angular velocity of Wheelchair */
    odom.twist.twist.angular.x = test1;
    odom.twist.twist.angular.y = test2;
    odom.twist.twist.angular.z = smart.getTwistZ()*3.1415926/180;
 
    /* Allows for Odometry to be published and sent to ROS */
    chatter.publish(&odom);
    //chatter2.publish(&commandRead);
 
    /* ROS communication callbacks are handled.                            *
     * spinOnce() will handle passing messages to the subscriber callback. */
    nh.spinOnce();
}
 
/******************************************************************************************
 *  This method will publishes to ROS the Time of Flight sensors and the Encoder values.  *
 ******************************************************************************************/
void tof_encoder_roscom_thread()
{
    /* Store the Encoder values for ROS */
    left_encoder.data = smart.dist_new;  
    right_encoder.data = smart.dist_newS;
    
    /* Store the Time of Flight values for ROS */
    for(int i = 0; i < 12; i++)
    {
        tof_sensors.data[i] = smart.ToFV[i];
    }
    
    /* Publish Time of Flight sensors and Encoders and send to ROS */
    sensor_chatter.publish(&tof_sensors);
    left_encoder_chatter.publish(&left_encoder);
    right_encoder_chatter.publish(&right_encoder);
    
    /* ROS communication callbacks are handled.                             *
     * spinOnce() will handle passing messages to the subscriber callback.  */
    nh.spinOnce();
 
}