Needs imu
Dependencies: chair_BNO055 ros_lib_kinetic
main.cpp
- Committer:
- JesiMiranda
- Date:
- 2019-08-20
- Revision:
- 14:1223bef993b5
- Parent:
- 12:5df2c0576333
File content as of revision 14:1223bef993b5:
#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(); }