Affordable Smart Wheelchair
Needs imu
Dependencies: chair_BNO055 ros_lib_kinetic
Diff: main.cpp
- Revision:
- 12:5df2c0576333
- Parent:
- 10:347b11f2e19c
- Child:
- 14:1223bef993b5
--- 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