Affordable Smart Wheelchair / Mbed OS wheelchaircontrolrealRosCom1

Needs imu

Dependencies:   chair_BNO055 ros_lib_kinetic

main.cpp@15:d26162b5b9b0, 2019-08-20 (annotated)

Committer:
JesiMiranda
Date:
Tue Aug 20 22:38:11 2019 +0000
Revision:
15:d26162b5b9b0
Parent:
14:1223bef993b5 
no changes

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ryanlin97 0:7e6b349182bc 1 #include "wheelchair.h"
JesiMiranda 14:1223bef993b5 2
jvfausto 10:347b11f2e19c 3 /* Intializes the right encoder */
JesiMiranda 12:5df2c0576333 4 QEI wheelS(D9, D10, NC, 1200);
jvfausto 10:347b11f2e19c 5 /* Set pull-up resistors to read analog signals into digital signals */
JesiMiranda 12:5df2c0576333 6 DigitalIn pt3(D10, PullUp);
jvfausto 9:ca11e4db63a7 7 DigitalIn pt4(D9, PullUp);
JesiMiranda 14:1223bef993b5 8
jvfausto 10:347b11f2e19c 9 /* Initializes Left encoder */
JesiMiranda 12:5df2c0576333 10 QEI wheel (D7, D8, NC, 1200);
jvfausto 10:347b11f2e19c 11 /* Set pull-up resistors to read analog signals into digital signals */
JesiMiranda 12:5df2c0576333 12 DigitalIn pt1(D7, PullUp);
jvfausto 9:ca11e4db63a7 13 DigitalIn pt2(D8, PullUp);
JesiMiranda 14:1223bef993b5 14
jvfausto 10:347b11f2e19c 15 /* Initializes analog axis for the joystick */
JesiMiranda 12:5df2c0576333 16 AnalogIn x(A0);
ryanlin97 0:7e6b349182bc 17 AnalogIn y(A1);
JesiMiranda 14:1223bef993b5 18
jvfausto 10:347b11f2e19c 19 double test1;
jvfausto 10:347b11f2e19c 20 double test2;
JesiMiranda 14:1223bef993b5 21
jvfausto 10:347b11f2e19c 22 /* Initializing more pins for wheelchair control */
JesiMiranda 12:5df2c0576333 23 DigitalOut up(D2); //Turn up speed mode for joystick
JesiMiranda 12:5df2c0576333 24 DigitalOut down(D8); //Turn down speed mode for joystick
jvfausto 10:347b11f2e19c 25 DigitalOut on(D12); //Turn Wheelchair On
jvfausto 10:347b11f2e19c 26 DigitalOut off(D11); //Turn Wheelchair Off
JesiMiranda 14:1223bef993b5 27
JesiMiranda 12:5df2c0576333 28 /*************************************************************************
JesiMiranda 12:5df2c0576333 29 * Initializing Time of Flight sensors with respective Nucleo digital pins*
JesiMiranda 12:5df2c0576333 30 **************************************************************************/
JesiMiranda 12:5df2c0576333 31 //Parameter: SDA, SCL, Digital Pin
JesiMiranda 12:5df2c0576333 32 VL53L1X sensor1(PD_13, PD_12, PA_15); // Block 1
JesiMiranda 12:5df2c0576333 33 VL53L1X sensor2(PD_13, PD_12, PC_7);
JesiMiranda 12:5df2c0576333 34 VL53L1X sensor3(PD_13, PD_12, PB_5);
JesiMiranda 12:5df2c0576333 35
JesiMiranda 12:5df2c0576333 36 VL53L1X sensor4(PD_13, PD_12, PE_11); // Block 2
JesiMiranda 12:5df2c0576333 37 VL53L1X sensor5(PD_13, PD_12, PF_14);
JesiMiranda 12:5df2c0576333 38 VL53L1X sensor6(PD_13, PD_12, PE_13);
JesiMiranda 12:5df2c0576333 39
JesiMiranda 12:5df2c0576333 40 VL53L1X sensor7(PD_13, PD_12, PG_15); // Block 3
JesiMiranda 12:5df2c0576333 41 VL53L1X sensor8(PD_13, PD_12, PG_14);
JesiMiranda 12:5df2c0576333 42 VL53L1X sensor9(PD_13, PD_12, PG_9);
JesiMiranda 12:5df2c0576333 43
JesiMiranda 12:5df2c0576333 44 VL53L1X sensor10(PD_13, PD_12, PE_10); // Block 4
JesiMiranda 12:5df2c0576333 45 VL53L1X sensor11(PD_13, PD_12, PE_12);
JesiMiranda 12:5df2c0576333 46 VL53L1X sensor12(PD_13, PD_12, PE_14);
JesiMiranda 12:5df2c0576333 47
JesiMiranda 14:1223bef993b5 48
JesiMiranda 12:5df2c0576333 49 /*************************************************************************
JesiMiranda 12:5df2c0576333 50 * Creating a pointer to point to the addressed of the sensors *
JesiMiranda 12:5df2c0576333 51 * To print value, you need to dereference. Ex: *Tof[0] *
JesiMiranda 12:5df2c0576333 52 **************************************************************************/
JesiMiranda 12:5df2c0576333 53 VL53L1X* ToF[12] = {&sensor1, &sensor2, &sensor3, &sensor4, &sensor5, &sensor6,
JesiMiranda 12:5df2c0576333 54 &sensor7, &sensor8, &sensor9, &sensor10, &sensor11, &sensor12
JesiMiranda 12:5df2c0576333 55 };
JesiMiranda 12:5df2c0576333 56 // Creating a double pointer to point to the array.
jvfausto 9:ca11e4db63a7 57 VL53L1X** ToFT = ToF;
JesiMiranda 12:5df2c0576333 58 int ToFV[12];
jvfausto 10:347b11f2e19c 59 /* Changes the control mode of wheelchair: Automatic or Manual */
JesiMiranda 12:5df2c0576333 60 bool manual = false;
JesiMiranda 14:1223bef993b5 61
jvfausto 10:347b11f2e19c 62 Serial pc(USBTX, USBRX, 57600); //Serial Monitor
jvfausto 10:347b11f2e19c 63 Timer t; //Initialize time object t
jvfausto 10:347b11f2e19c 64 EventQueue queue; //Class to organize threads
JesiMiranda 14:1223bef993b5 65
JesiMiranda 12:5df2c0576333 66 /******************************************************************************************
JesiMiranda 12:5df2c0576333 67 * Instantiate node handle, which allows our program to create publishers and subscribers *
JesiMiranda 12:5df2c0576333 68 * This also takes care of serial port communication. Always needs to be included!! *
JesiMiranda 12:5df2c0576333 69 ******************************************************************************************/
jvfausto 10:347b11f2e19c 70 ros::NodeHandle nh;
JesiMiranda 14:1223bef993b5 71
JesiMiranda 12:5df2c0576333 72 /******************************************************************************************
JesiMiranda 12:5df2c0576333 73 * Instantiate the message instance to be used for publishing *
JesiMiranda 12:5df2c0576333 74 ******************************************************************************************/
jvfausto 10:347b11f2e19c 75 nav_msgs::Odometry odom;
jvfausto 10:347b11f2e19c 76 geometry_msgs::Twist commandRead;
JesiMiranda 12:5df2c0576333 77 std_msgs::Float64MultiArray tof_sensors;
JesiMiranda 12:5df2c0576333 78 std_msgs::Float64 left_encoder;
JesiMiranda 12:5df2c0576333 79 std_msgs::Float64 right_encoder;
JesiMiranda 12:5df2c0576333 80 /******************************************************************************************
JesiMiranda 12:5df2c0576333 81 * Creating the callback function for our Subcriber. *
JesiMiranda 12:5df2c0576333 82 * Our message name will be command. Here we reference command to one of our Publishers *
JesiMiranda 12:5df2c0576333 83 ******************************************************************************************/
JesiMiranda 12:5df2c0576333 84 void handlerFunction(const geometry_msgs::Twist& command)
JesiMiranda 12:5df2c0576333 85 {
jvfausto 10:347b11f2e19c 86 commandRead = command;
jvfausto 10:347b11f2e19c 87 }
JesiMiranda 14:1223bef993b5 88
JesiMiranda 12:5df2c0576333 89 /*******************************************************************************************
JesiMiranda 12:5df2c0576333 90 * Instantaiate a Subcriber with a topic of name "cmd_vel" and type geometry_msgs::Twist *
JesiMiranda 12:5df2c0576333 91 * Its two arguments are the topic it will be subscribing to and the callback function it *
JesiMiranda 12:5df2c0576333 92 * will be using. "sub(...)" is just the name of the subscriber. *
JesiMiranda 12:5df2c0576333 93 *******************************************************************************************/
jvfausto 10:347b11f2e19c 94 ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &handlerFunction);
JesiMiranda 14:1223bef993b5 95
JesiMiranda 14:1223bef993b5 96
JesiMiranda 14:1223bef993b5 97
JesiMiranda 12:5df2c0576333 98 /*******************************************************************************************
JesiMiranda 12:5df2c0576333 99 * Instantiate a Publisher with a topic name of "odom". The second parameter to Publisher *
JesiMiranda 12:5df2c0576333 100 * is a reference to the message instance to be used for publishing *
JesiMiranda 12:5df2c0576333 101 *******************************************************************************************/
jvfausto 10:347b11f2e19c 102 ros::Publisher chatter("odom", &odom);
JesiMiranda 12:5df2c0576333 103 //ros::Publisher chatter2("cmd_vel", &commandRead);
JesiMiranda 12:5df2c0576333 104 ros::Publisher sensor_chatter("tof_sensors", &tof_sensors);
JesiMiranda 12:5df2c0576333 105 ros::Publisher left_encoder_chatter("left_encoder", &left_encoder);
JesiMiranda 12:5df2c0576333 106 ros::Publisher right_encoder_chatter("right_encoder", &right_encoder);
JesiMiranda 14:1223bef993b5 107
jvfausto 10:347b11f2e19c 108 /* Initialize Wheelchair objects and threads */
jvfausto 9:ca11e4db63a7 109 Wheelchair smart(xDir,yDir, &pc, &t, &wheel, &wheelS, ToFT); //Initialize wheelchair object
JesiMiranda 12:5df2c0576333 110 Thread compass;
JesiMiranda 12:5df2c0576333 111 Thread velocity;
JesiMiranda 12:5df2c0576333 112 Thread ros_com;
JesiMiranda 12:5df2c0576333 113 Thread tof_encoder_roscom;
JesiMiranda 12:5df2c0576333 114 Thread assistSafe;
jvfausto 10:347b11f2e19c 115 /* This thread continues the communication with ROS and Mbed */
jvfausto 10:347b11f2e19c 116 void rosCom_thread();
JesiMiranda 12:5df2c0576333 117 void tof_encoder_roscom_thread();
JesiMiranda 14:1223bef993b5 118
ryanlin97 0:7e6b349182bc 119 int main(void)
JesiMiranda 12:5df2c0576333 120 {
jvfausto 10:347b11f2e19c 121 /* Initialize ROS commands to publish and subscribe to nodes */
jvfausto 10:347b11f2e19c 122 nh.initNode();
JesiMiranda 14:1223bef993b5 123
JesiMiranda 12:5df2c0576333 124 /* We are initializing the multi-array dimension and size *
JesiMiranda 12:5df2c0576333 125 * Dimension for outputing label, size, and stride does not work. */
JesiMiranda 12:5df2c0576333 126 tof_sensors.data_length = 12;
JesiMiranda 12:5df2c0576333 127 tof_sensors.layout.dim[0].label = "sensors";
JesiMiranda 12:5df2c0576333 128 tof_sensors.layout.dim[0].size = 12;
JesiMiranda 12:5df2c0576333 129 tof_sensors.layout.dim[0].stride = 3;
JesiMiranda 12:5df2c0576333 130 tof_sensors.layout.data_offset = 0;
JesiMiranda 14:1223bef993b5 131
JesiMiranda 14:1223bef993b5 132
JesiMiranda 12:5df2c0576333 133 /******************************************************************************************
JesiMiranda 12:5df2c0576333 134 * We are advertising any topics being published. *
JesiMiranda 12:5df2c0576333 135 *******************************************************************************************/
jvfausto 9:ca11e4db63a7 136 nh.advertise(chatter);
JesiMiranda 12:5df2c0576333 137 //nh.advertise(chatter2);
JesiMiranda 12:5df2c0576333 138 nh.advertise(sensor_chatter);
JesiMiranda 12:5df2c0576333 139 nh.advertise(left_encoder_chatter);
JesiMiranda 12:5df2c0576333 140 nh.advertise(right_encoder_chatter);
jvfausto 10:347b11f2e19c 141
JesiMiranda 12:5df2c0576333 142 /******************************************************************************************
JesiMiranda 12:5df2c0576333 143 * We are subscribing to any topics we wish to listen to. *
JesiMiranda 12:5df2c0576333 144 *******************************************************************************************/
JesiMiranda 12:5df2c0576333 145 nh.subscribe(sub);
JesiMiranda 14:1223bef993b5 146
JesiMiranda 12:5df2c0576333 147 /* Sets up sampling frequency of threads */
JesiMiranda 12:5df2c0576333 148 queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::compass_thread);
JesiMiranda 12:5df2c0576333 149 queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::velocity_thread);
JesiMiranda 12:5df2c0576333 150 queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::assistSafe_thread);
JesiMiranda 12:5df2c0576333 151 queue.call_every(200, rosCom_thread);
JesiMiranda 12:5df2c0576333 152 queue.call_every(200, tof_encoder_roscom_thread);
jvfausto 10:347b11f2e19c 153
JesiMiranda 12:5df2c0576333 154 /* Reset the time */
JesiMiranda 12:5df2c0576333 155 t.reset();
JesiMiranda 12:5df2c0576333 156
jvfausto 10:347b11f2e19c 157 /* Start running threads */
JesiMiranda 12:5df2c0576333 158 compass.start(callback(&queue, &EventQueue::dispatch_forever));
JesiMiranda 12:5df2c0576333 159 velocity.start(callback(&queue, &EventQueue::dispatch_forever));
JesiMiranda 12:5df2c0576333 160 assistSafe.start(callback(&queue, &EventQueue::dispatch_forever));
JesiMiranda 12:5df2c0576333 161 ros_com.start(callback(&queue, &EventQueue::dispatch_forever));
JesiMiranda 12:5df2c0576333 162 tof_encoder_roscom.start(callback(&queue, &EventQueue::dispatch_forever));
JesiMiranda 14:1223bef993b5 163
ryanlin97 0:7e6b349182bc 164 while(1) {
jvfausto 10:347b11f2e19c 165 /* If Ros comands the wheelchair to move fowards or backwards*/
JesiMiranda 12:5df2c0576333 166 if(commandRead.linear.x != 0) {
jvfausto 10:347b11f2e19c 167 smart.pid_twistV(); //Updates the twist linear velocity of chair
jvfausto 10:347b11f2e19c 168 test1 = 3.14159;
JesiMiranda 12:5df2c0576333 169 }
jvfausto 10:347b11f2e19c 170 /* If Ros comands the wheelchair to turn at a certain speed*/
JesiMiranda 12:5df2c0576333 171 else if(commandRead.angular.z != 0) {
JesiMiranda 12:5df2c0576333 172 smart.pid_twistA(); //Updates the twist angular velocity of chair
JesiMiranda 12:5df2c0576333 173 test2 = 2.782;
JesiMiranda 12:5df2c0576333 174 }
JesiMiranda 12:5df2c0576333 175 /* If Ros does not give velocity comands*/
JesiMiranda 12:5df2c0576333 176 else {
jvfausto 10:347b11f2e19c 177 smart.stop(); //Stops the chair
jvfausto 10:347b11f2e19c 178 test2 = 0;
jvfausto 10:347b11f2e19c 179 test1 = 0;
ryanlin97 0:7e6b349182bc 180 }
jvfausto 10:347b11f2e19c 181 wait(process); //Delay
ryanlin97 0:7e6b349182bc 182 }
JesiMiranda 14:1223bef993b5 183
ryanlin97 0:7e6b349182bc 184 }
JesiMiranda 14:1223bef993b5 185
JesiMiranda 12:5df2c0576333 186 /******************************************************************************************
JesiMiranda 12:5df2c0576333 187 * This thread allows for continuous update and publishing of ROS Odometry/Twist message *
JesiMiranda 12:5df2c0576333 188 ******************************************************************************************/
jvfausto 10:347b11f2e19c 189 void rosCom_thread()
jvfausto 10:347b11f2e19c 190 {
JesiMiranda 12:5df2c0576333 191 /*Determines linear and angular velocity */
JesiMiranda 12:5df2c0576333 192 smart.linearV = commandRead.linear.x;
JesiMiranda 12:5df2c0576333 193 smart.angularV = commandRead.angular.z*180/3.1415926;
JesiMiranda 14:1223bef993b5 194
JesiMiranda 12:5df2c0576333 195 /* Publishes the position of the Wheelchair for Odometry */
JesiMiranda 12:5df2c0576333 196 odom.pose.pose.position.x = smart.x_position;
JesiMiranda 12:5df2c0576333 197 odom.pose.pose.position.y = smart.y_position;
JesiMiranda 12:5df2c0576333 198 odom.pose.pose.position.z = 0;
JesiMiranda 14:1223bef993b5 199
JesiMiranda 12:5df2c0576333 200 /* Store the orientation of the Wheelchair for Odometry */
JesiMiranda 12:5df2c0576333 201 odom.pose.pose.orientation.z = sin(smart.z_angular*.5*3.1415926/180);
JesiMiranda 12:5df2c0576333 202 odom.pose.pose.orientation.x = 0;
JesiMiranda 12:5df2c0576333 203 odom.pose.pose.orientation.y = 0;
JesiMiranda 12:5df2c0576333 204 odom.pose.pose.orientation.w = cos(smart.z_angular*.5*3.1415926/180);
JesiMiranda 14:1223bef993b5 205
JesiMiranda 12:5df2c0576333 206 /* Store Twist linear velocity of Wheelchair */
JesiMiranda 12:5df2c0576333 207 odom.twist.twist.linear.x = smart.vel;
JesiMiranda 12:5df2c0576333 208 odom.twist.twist.linear.y = commandRead.linear.x;
JesiMiranda 12:5df2c0576333 209 odom.twist.twist.linear.z = commandRead.angular.z;
JesiMiranda 14:1223bef993b5 210
JesiMiranda 12:5df2c0576333 211 /* Store Twist angular velocity of Wheelchair */
JesiMiranda 12:5df2c0576333 212 odom.twist.twist.angular.x = test1;
JesiMiranda 12:5df2c0576333 213 odom.twist.twist.angular.y = test2;
JesiMiranda 12:5df2c0576333 214 odom.twist.twist.angular.z = smart.getTwistZ()*3.1415926/180;
JesiMiranda 14:1223bef993b5 215
JesiMiranda 12:5df2c0576333 216 /* Allows for Odometry to be published and sent to ROS */
JesiMiranda 12:5df2c0576333 217 chatter.publish(&odom);
JesiMiranda 12:5df2c0576333 218 //chatter2.publish(&commandRead);
JesiMiranda 14:1223bef993b5 219
JesiMiranda 12:5df2c0576333 220 /* ROS communication callbacks are handled. *
JesiMiranda 12:5df2c0576333 221 * spinOnce() will handle passing messages to the subscriber callback. */
JesiMiranda 12:5df2c0576333 222 nh.spinOnce();
JesiMiranda 12:5df2c0576333 223 }
JesiMiranda 14:1223bef993b5 224
JesiMiranda 12:5df2c0576333 225 /******************************************************************************************
JesiMiranda 12:5df2c0576333 226 * This method will publishes to ROS the Time of Flight sensors and the Encoder values. *
JesiMiranda 12:5df2c0576333 227 ******************************************************************************************/
JesiMiranda 12:5df2c0576333 228 void tof_encoder_roscom_thread()
JesiMiranda 12:5df2c0576333 229 {
JesiMiranda 12:5df2c0576333 230 /* Store the Encoder values for ROS */
JesiMiranda 12:5df2c0576333 231 left_encoder.data = smart.dist_new;
JesiMiranda 12:5df2c0576333 232 right_encoder.data = smart.dist_newS;
jvfausto 10:347b11f2e19c 233
JesiMiranda 12:5df2c0576333 234 /* Store the Time of Flight values for ROS */
JesiMiranda 12:5df2c0576333 235 for(int i = 0; i < 12; i++)
JesiMiranda 12:5df2c0576333 236 {
JesiMiranda 12:5df2c0576333 237 tof_sensors.data[i] = smart.ToFV[i];
JesiMiranda 12:5df2c0576333 238 }
jvfausto 10:347b11f2e19c 239
JesiMiranda 12:5df2c0576333 240 /* Publish Time of Flight sensors and Encoders and send to ROS */
JesiMiranda 12:5df2c0576333 241 sensor_chatter.publish(&tof_sensors);
JesiMiranda 12:5df2c0576333 242 left_encoder_chatter.publish(&left_encoder);
JesiMiranda 12:5df2c0576333 243 right_encoder_chatter.publish(&right_encoder);
jvfausto 10:347b11f2e19c 244
JesiMiranda 12:5df2c0576333 245 /* ROS communication callbacks are handled. *
JesiMiranda 12:5df2c0576333 246 * spinOnce() will handle passing messages to the subscriber callback. */
JesiMiranda 12:5df2c0576333 247 nh.spinOnce();
JesiMiranda 14:1223bef993b5 248
JesiMiranda 14:1223bef993b5 249 }
JesiMiranda 14:1223bef993b5 250