Needs imu
Dependencies: chair_BNO055 ros_lib_kinetic
Diff: main.cpp
- Revision:
- 12:5df2c0576333
- Parent:
- 10:347b11f2e19c
- Child:
- 14:1223bef993b5
diff -r 3aef071cace2 -r 5df2c0576333 main.cpp --- a/main.cpp Mon Jul 01 16:43:10 2019 +0000 +++ b/main.cpp Fri Jul 05 19:41:43 2019 +0000 @@ -1,158 +1,249 @@ #include "wheelchair.h" /* Intializes the right encoder */ -QEI wheelS(D9, D10, NC, 1200); +QEI wheelS(D9, D10, NC, 1200); /* Set pull-up resistors to read analog signals into digital signals */ -DigitalIn pt3(D10, PullUp); +DigitalIn pt3(D10, PullUp); DigitalIn pt4(D9, PullUp); /* Initializes Left encoder */ -QEI wheel (D7, D8, NC, 1200); +QEI wheel (D7, D8, NC, 1200); /* Set pull-up resistors to read analog signals into digital signals */ -DigitalIn pt1(D7, PullUp); +DigitalIn pt1(D7, PullUp); DigitalIn pt2(D8, PullUp); /* Initializes analog axis for the joystick */ -AnalogIn x(A0); +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 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); + -VL53L1X sensor1(PB_11, PB_10, D0); //initializes ToF sensors -VL53L1X sensor2(PB_11, PB_10, D1); -VL53L1X sensor3(PB_11, PB_10, D2); -VL53L1X sensor4(PB_11, PB_10, D3); -VL53L1X sensor5(PB_11, PB_10, D4); -VL53L1X sensor6(PB_11, PB_10, D5); -VL53L1X sensor7(PB_11, PB_10, PE_14); -VL53L1X sensor8(PB_11, PB_10, PE_12); -VL53L1X sensor9(PB_11, PB_10, PE_10); -VL53L1X sensor10(PB_11, PB_10, PE_15); -VL53L1X sensor11(PB_11, PB_10, D6); -VL53L1X sensor12(PB_11, PB_10, D11); - -VL53L1X* ToF[12] = {&sensor1, &sensor2, &sensor3, &sensor4, &sensor5, &sensor6, -&sensor7, &sensor8, &sensor9, &sensor10, &sensor11, &sensor12}; //puts ToF sensor pointers into an array +/************************************************************************* +* 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; +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; - -void handlerFunction(const geometry_msgs::Twist& command){ +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 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 assistSafe; - +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.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); //Sets up sampling frequency of the velosity_thread - queue.call_every(200, rosCom_thread); + //nh.advertise(chatter2); + nh.advertise(sensor_chatter); + nh.advertise(left_encoder_chatter); + nh.advertise(right_encoder_chatter); - t.reset(); //resets the time + /****************************************************************************************** + * 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)); //Starts running the velosity thread - ros_com.start(callback(&queue, &EventQueue::dispatch_forever)); - - + 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) - { + 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 veloc - ity comands*/ - else - { + 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 */ +/****************************************************************************************** + * 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; - - /* Publishes 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); + /*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; - /* Publishes 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 the Time of Flight values for ROS */ + for(int i = 0; i < 12; i++) + { + tof_sensors.data[i] = smart.ToFV[i]; + } - /* Publishes 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; + /* 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); - /* Allows for Odometry to be published and sent to ROS */ - chatter.publish(&odom); - //chatter2.publish(&commandRead); - - /*Checks for incoming messages */ - nh.spinOnce(); -} \ No newline at end of file + /* ROS communication callbacks are handled. * + * spinOnce() will handle passing messages to the subscriber callback. */ + nh.spinOnce(); + +} \ No newline at end of file