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