Needs imu
Dependencies: chair_BNO055 ros_lib_kinetic
Diff: main.cpp
- Revision:
- 14:1223bef993b5
- Parent:
- 12:5df2c0576333
--- a/main.cpp Fri Jul 05 19:44:12 2019 +0000 +++ b/main.cpp Tue Aug 20 22:29:56 2019 +0000 @@ -1,30 +1,30 @@ #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* **************************************************************************/ @@ -45,7 +45,7 @@ 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] * @@ -58,17 +58,17 @@ 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 * ******************************************************************************************/ @@ -85,16 +85,16 @@ { 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 * @@ -104,7 +104,7 @@ 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; @@ -115,12 +115,12 @@ /* 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; @@ -128,8 +128,8 @@ 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. * *******************************************************************************************/ @@ -143,7 +143,7 @@ * 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); @@ -160,7 +160,7 @@ 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) { @@ -180,9 +180,9 @@ } wait(process); //Delay } - + } - + /****************************************************************************************** * This thread allows for continuous update and publishing of ROS Odometry/Twist message * ******************************************************************************************/ @@ -191,37 +191,37 @@ /*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. * ******************************************************************************************/ @@ -245,5 +245,6 @@ /* ROS communication callbacks are handled. * * spinOnce() will handle passing messages to the subscriber callback. */ nh.spinOnce(); - -} \ No newline at end of file + +} + \ No newline at end of file