Jesus Fausto / Mbed OS wheelchaircontrolrealroscom

a

Dependencies:  

main.cpp@10:347b11f2e19c, 2019-06-25 (annotated)

Committer:
jvfausto
Date:
Tue Jun 25 17:55:17 2019 +0000
Revision:
10:347b11f2e19c
Parent:
9:ca11e4db63a7 
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ryanlin97 0:7e6b349182bc 1 #include "wheelchair.h"
ryanlin97 0:7e6b349182bc 2
jvfausto 10:347b11f2e19c 3 /* Intializes the right encoder */
jvfausto 10:347b11f2e19c 4 QEI wheelS(D9, D10, NC, 1200);
jvfausto 10:347b11f2e19c 5 /* Set pull-up resistors to read analog signals into digital signals */
jvfausto 10:347b11f2e19c 6 DigitalIn pt3(D10, PullUp);
jvfausto 9:ca11e4db63a7 7 DigitalIn pt4(D9, PullUp);
jvfausto 9:ca11e4db63a7 8
jvfausto 10:347b11f2e19c 9 /* Initializes Left encoder */
jvfausto 10:347b11f2e19c 10 QEI wheel (D7, D8, NC, 1200);
jvfausto 10:347b11f2e19c 11 /* Set pull-up resistors to read analog signals into digital signals */
jvfausto 10:347b11f2e19c 12 DigitalIn pt1(D7, PullUp);
jvfausto 9:ca11e4db63a7 13 DigitalIn pt2(D8, PullUp);
jvfausto 8:db780b392bae 14
jvfausto 10:347b11f2e19c 15 /* Initializes analog axis for the joystick */
jvfausto 10:347b11f2e19c 16 AnalogIn x(A0);
ryanlin97 0:7e6b349182bc 17 AnalogIn y(A1);
ryanlin97 0:7e6b349182bc 18
jvfausto 10:347b11f2e19c 19 double test1;
jvfausto 10:347b11f2e19c 20 double test2;
jvfausto 9:ca11e4db63a7 21
jvfausto 10:347b11f2e19c 22 /* Initializing more pins for wheelchair control */
jvfausto 10:347b11f2e19c 23 DigitalOut up(D2); //Turn up speed mode for joystick
jvfausto 10:347b11f2e19c 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
jvfausto 10:347b11f2e19c 27
ryanlin97 0:7e6b349182bc 28
jvfausto 9:ca11e4db63a7 29 VL53L1X sensor1(PB_11, PB_10, D0); //initializes ToF sensors
jvfausto 9:ca11e4db63a7 30 VL53L1X sensor2(PB_11, PB_10, D1);
jvfausto 9:ca11e4db63a7 31 VL53L1X sensor3(PB_11, PB_10, D2);
jvfausto 9:ca11e4db63a7 32 VL53L1X sensor4(PB_11, PB_10, D3);
jvfausto 9:ca11e4db63a7 33 VL53L1X sensor5(PB_11, PB_10, D4);
jvfausto 9:ca11e4db63a7 34 VL53L1X sensor6(PB_11, PB_10, D5);
jvfausto 9:ca11e4db63a7 35 VL53L1X sensor7(PB_11, PB_10, PE_14);
jvfausto 9:ca11e4db63a7 36 VL53L1X sensor8(PB_11, PB_10, PE_12);
jvfausto 9:ca11e4db63a7 37 VL53L1X sensor9(PB_11, PB_10, PE_10);
jvfausto 9:ca11e4db63a7 38 VL53L1X sensor10(PB_11, PB_10, PE_15);
jvfausto 9:ca11e4db63a7 39 VL53L1X sensor11(PB_11, PB_10, D6);
jvfausto 9:ca11e4db63a7 40 VL53L1X sensor12(PB_11, PB_10, D11);
ryanlin97 5:90bf5f0d86e9 41
jvfausto 9:ca11e4db63a7 42 VL53L1X* ToF[12] = {&sensor1, &sensor2, &sensor3, &sensor4, &sensor5, &sensor6,
jvfausto 9:ca11e4db63a7 43 &sensor7, &sensor8, &sensor9, &sensor10, &sensor11, &sensor12}; //puts ToF sensor pointers into an array
jvfausto 9:ca11e4db63a7 44 VL53L1X** ToFT = ToF;
ryanlin97 0:7e6b349182bc 45
jvfausto 10:347b11f2e19c 46 /* Changes the control mode of wheelchair: Automatic or Manual */
jvfausto 10:347b11f2e19c 47 bool manual = false;
jvfausto 10:347b11f2e19c 48
jvfausto 10:347b11f2e19c 49 Serial pc(USBTX, USBRX, 57600); //Serial Monitor
jvfausto 10:347b11f2e19c 50 Timer t; //Initialize time object t
jvfausto 10:347b11f2e19c 51 EventQueue queue; //Class to organize threads
jvfausto 10:347b11f2e19c 52
jvfausto 10:347b11f2e19c 53 ros::NodeHandle nh;
jvfausto 10:347b11f2e19c 54 nav_msgs::Odometry odom;
jvfausto 10:347b11f2e19c 55 geometry_msgs::Twist commandRead;
jvfausto 10:347b11f2e19c 56
jvfausto 10:347b11f2e19c 57 void handlerFunction(const geometry_msgs::Twist& command){
jvfausto 10:347b11f2e19c 58 commandRead = command;
jvfausto 10:347b11f2e19c 59 }
jvfausto 10:347b11f2e19c 60
jvfausto 10:347b11f2e19c 61 ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &handlerFunction);
jvfausto 10:347b11f2e19c 62 ros::Publisher chatter("odom", &odom);
jvfausto 10:347b11f2e19c 63 ros::Publisher chatter2("cmd_vel", &commandRead);
jvfausto 10:347b11f2e19c 64
jvfausto 10:347b11f2e19c 65 /* Initialize Wheelchair objects and threads */
jvfausto 9:ca11e4db63a7 66 Wheelchair smart(xDir,yDir, &pc, &t, &wheel, &wheelS, ToFT); //Initialize wheelchair object
jvfausto 10:347b11f2e19c 67 Thread compass;
jvfausto 10:347b11f2e19c 68 Thread velocity;
jvfausto 10:347b11f2e19c 69 Thread ros_com;
jvfausto 10:347b11f2e19c 70 Thread assistSafe;
ryanlin97 0:7e6b349182bc 71
jvfausto 10:347b11f2e19c 72 /* This thread continues the communication with ROS and Mbed */
jvfausto 10:347b11f2e19c 73 void rosCom_thread();
ryanlin97 5:90bf5f0d86e9 74
ryanlin97 0:7e6b349182bc 75 int main(void)
jvfausto 9:ca11e4db63a7 76 {
jvfausto 10:347b11f2e19c 77 /* Initialize ROS commands to publish and subscribe to nodes */
jvfausto 10:347b11f2e19c 78 nh.initNode();
jvfausto 9:ca11e4db63a7 79 nh.advertise(chatter);
jvfausto 9:ca11e4db63a7 80 nh.advertise(chatter2);
jvfausto 10:347b11f2e19c 81 nh.subscribe(sub);
jvfausto 10:347b11f2e19c 82
jvfausto 10:347b11f2e19c 83 /* Sets up sampling frequency of threads */
jvfausto 10:347b11f2e19c 84 queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::compass_thread);
jvfausto 10:347b11f2e19c 85 queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::velocity_thread);
jvfausto 9:ca11e4db63a7 86 queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::assistSafe_thread); //Sets up sampling frequency of the velosity_thread
jvfausto 10:347b11f2e19c 87 queue.call_every(200, rosCom_thread);
jvfausto 10:347b11f2e19c 88
jvfausto 10:347b11f2e19c 89 t.reset(); //resets the time
jvfausto 10:347b11f2e19c 90
jvfausto 10:347b11f2e19c 91 /* Start running threads */
jvfausto 10:347b11f2e19c 92 compass.start(callback(&queue, &EventQueue::dispatch_forever));
jvfausto 10:347b11f2e19c 93 velocity.start(callback(&queue, &EventQueue::dispatch_forever));
jvfausto 9:ca11e4db63a7 94 assistSafe.start(callback(&queue, &EventQueue::dispatch_forever)); //Starts running the velosity thread
jvfausto 10:347b11f2e19c 95 ros_com.start(callback(&queue, &EventQueue::dispatch_forever));
jvfausto 10:347b11f2e19c 96
jvfausto 10:347b11f2e19c 97
ryanlin97 0:7e6b349182bc 98 while(1) {
jvfausto 10:347b11f2e19c 99 /* If Ros comands the wheelchair to move fowards or backwards*/
jvfausto 10:347b11f2e19c 100 if(commandRead.linear.x != 0)
jvfausto 10:347b11f2e19c 101 {
jvfausto 10:347b11f2e19c 102 smart.pid_twistV(); //Updates the twist linear velocity of chair
jvfausto 10:347b11f2e19c 103 test1 = 3.14159;
jvfausto 10:347b11f2e19c 104 }
jvfausto 10:347b11f2e19c 105 /* If Ros comands the wheelchair to turn at a certain speed*/
jvfausto 10:347b11f2e19c 106 else if(commandRead.angular.z != 0)
jvfausto 10:347b11f2e19c 107 {
jvfausto 10:347b11f2e19c 108 smart.pid_twistA(); //Updates the twist angular velocity of chair
jvfausto 10:347b11f2e19c 109 test2 = 2.782;
jvfausto 10:347b11f2e19c 110 }
jvfausto 10:347b11f2e19c 111 /* If Ros does not give veloc
jvfausto 10:347b11f2e19c 112 ity comands*/
jvfausto 10:347b11f2e19c 113 else
jvfausto 10:347b11f2e19c 114 {
jvfausto 10:347b11f2e19c 115 smart.stop(); //Stops the chair
jvfausto 10:347b11f2e19c 116 test2 = 0;
jvfausto 10:347b11f2e19c 117 test1 = 0;
ryanlin97 0:7e6b349182bc 118 }
jvfausto 10:347b11f2e19c 119 wait(process); //Delay
ryanlin97 0:7e6b349182bc 120 }
jvfausto 10:347b11f2e19c 121
ryanlin97 0:7e6b349182bc 122 }
ryanlin97 0:7e6b349182bc 123
jvfausto 10:347b11f2e19c 124 /* This thread allows for continuous update and publishing of ROS Odometry/Twist message */
jvfausto 10:347b11f2e19c 125 void rosCom_thread()
jvfausto 10:347b11f2e19c 126 {
jvfausto 10:347b11f2e19c 127 /*Determines linear and angular velocity */
jvfausto 10:347b11f2e19c 128 smart.linearV = commandRead.linear.x;
jvfausto 10:347b11f2e19c 129 smart.angularV = commandRead.angular.z*180/3.1415926;
jvfausto 10:347b11f2e19c 130
jvfausto 10:347b11f2e19c 131 /* Publishes the position of the Wheelchair for Odometry */
jvfausto 10:347b11f2e19c 132 odom.pose.pose.position.x = smart.x_position;
jvfausto 10:347b11f2e19c 133 odom.pose.pose.position.y = smart.y_position;
jvfausto 10:347b11f2e19c 134 odom.pose.pose.position.z = 0;
jvfausto 10:347b11f2e19c 135
jvfausto 10:347b11f2e19c 136 /* Publishes the orientation of the Wheelchair for Odometry */
jvfausto 10:347b11f2e19c 137 odom.pose.pose.orientation.z = sin(smart.z_angular*.5*3.1415926/180);
jvfausto 10:347b11f2e19c 138 odom.pose.pose.orientation.x = 0;
jvfausto 10:347b11f2e19c 139 odom.pose.pose.orientation.y = 0;
jvfausto 10:347b11f2e19c 140 odom.pose.pose.orientation.w = cos(smart.z_angular*.5*3.1415926/180);
jvfausto 10:347b11f2e19c 141
jvfausto 10:347b11f2e19c 142 /* Publishes Twist linear velocity of Wheelchair */
jvfausto 10:347b11f2e19c 143 odom.twist.twist.linear.x = smart.vel;
jvfausto 10:347b11f2e19c 144 odom.twist.twist.linear.y = commandRead.linear.x;
jvfausto 10:347b11f2e19c 145 odom.twist.twist.linear.z = commandRead.angular.z;
jvfausto 10:347b11f2e19c 146
jvfausto 10:347b11f2e19c 147 /* Publishes Twist angular velocity of Wheelchair */
jvfausto 10:347b11f2e19c 148 odom.twist.twist.angular.x = test1;
jvfausto 10:347b11f2e19c 149 odom.twist.twist.angular.y = test2;
jvfausto 10:347b11f2e19c 150 odom.twist.twist.angular.z = smart.getTwistZ()*3.1415926/180;
jvfausto 10:347b11f2e19c 151
jvfausto 10:347b11f2e19c 152 /* Allows for Odometry to be published and sent to ROS */
jvfausto 10:347b11f2e19c 153 chatter.publish(&odom);
jvfausto 10:347b11f2e19c 154 //chatter2.publish(&commandRead);
jvfausto 10:347b11f2e19c 155
jvfausto 10:347b11f2e19c 156 /*Checks for incoming messages */
jvfausto 10:347b11f2e19c 157 nh.spinOnce();
jvfausto 10:347b11f2e19c 158 }