Needs imu
Dependencies: chair_BNO055 ros_lib_kinetic
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
- Child:
- 12:5df2c0576333
a
Who changed what in which revision?
User | Revision | Line number | New 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 | } |