![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
no changes
Dependencies: wheelchaircontrol Watchdog ros_lib_kinetic
Diff: main.cpp
- Revision:
- 10:347b11f2e19c
- Parent:
- 9:ca11e4db63a7
diff -r ca11e4db63a7 -r 347b11f2e19c main.cpp --- a/main.cpp Fri Apr 19 23:08:27 2019 +0000 +++ b/main.cpp Tue Jun 25 17:55:17 2019 +0000 @@ -1,29 +1,30 @@ #include "wheelchair.h" -QEI wheel (D10, D9, NC, 1200); //Initializes right encoder -DigitalIn pt3(D10, PullUp); //Pull up resistors to read analog signals into digital signals +/* 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); -/*added*/ -//DigitalIn e_button(D4); //emergency button will start at HIGH - -QEI wheelS (D7, D8, NC, 1200); //Initializes Left encoder -DigitalIn pt1(D7, PullUp); //Pull up resistors to read analog signals into digital signals +/* 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); -int max_velocity; -//Timer testAccT; - -AnalogIn x(A0); //Initializes analog axis for the joystick +/* Initializes analog axis for the joystick */ +AnalogIn x(A0); AnalogIn y(A1); -DigitalOut up(D12); //Turn up speed mode for joystick -DigitalOut down(D13); //Turn down speed mode for joystick -DigitalOut on(D14); //Turn Wheelchair On -DigitalOut off(D15); //Turn Wheelchair Off -bool manual = false; //Turns chair joystic to automatic and viceverza +double test1; +double test2; -Serial pc(USBTX, USBRX, 57600); //Serial Monitor +/* 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 + VL53L1X sensor1(PB_11, PB_10, D0); //initializes ToF sensors VL53L1X sensor2(PB_11, PB_10, D1); @@ -42,98 +43,116 @@ &sensor7, &sensor8, &sensor9, &sensor10, &sensor11, &sensor12}; //puts ToF sensor pointers into an array VL53L1X** ToFT = ToF; -Timer t; //Initialize time object t -EventQueue queue; //Class to organize threads +/* 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 + +ros::NodeHandle nh; +nav_msgs::Odometry odom; +geometry_msgs::Twist commandRead; + +void handlerFunction(const geometry_msgs::Twist& command){ + commandRead = command; +} + +ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &handlerFunction); +ros::Publisher chatter("odom", &odom); +ros::Publisher chatter2("cmd_vel", &commandRead); + +/* Initialize Wheelchair objects and threads */ Wheelchair smart(xDir,yDir, &pc, &t, &wheel, &wheelS, ToFT); //Initialize wheelchair object -Thread compass; //Thread for compass -Thread velocity; //Thread for velosity -Thread assistSafe; //thread for safety stuff +Thread compass; +Thread velocity; +Thread ros_com; +Thread assistSafe; +/* This thread continues the communication with ROS and Mbed */ +void rosCom_thread(); int main(void) { -/* nh.initNode(); + /* Initialize ROS commands to publish and subscribe to nodes */ + nh.initNode(); nh.advertise(chatter); nh.advertise(chatter2); - nh.subscribe(sub);*/ - //testAccT.start(); - pc.printf("before starting\r\n"); - queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::compass_thread); //Sets up sampling frequency of the compass_thread - queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::velocity_thread); //Sets up sampling frequency of the velosity_thread + 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); //Sets up sampling frequency of the velosity_thread - t.reset(); - compass.start(callback(&queue, &EventQueue::dispatch_forever)); //Starts running the compass thread - velocity.start(callback(&queue, &EventQueue::dispatch_forever)); //Starts running the velosity thread + queue.call_every(200, rosCom_thread); + + t.reset(); //resets the time + + /* 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)); //Starts running the velosity thread - pc.printf("After starting\r\n"); - - //added - // int emerg_button = e_button; - - int set = 0; + ros_com.start(callback(&queue, &EventQueue::dispatch_forever)); + + while(1) { - if( pc.readable()) { - set = 1; - char c = pc.getc(); //Read the instruction sent - if( c == 'w') { - smart.forward(); //Move foward - - } - else if( c == 'a') { - smart.left(); //Turn left - } - else if( c == 'd') { - smart.right(); //Turn right - } - else if( c == 's') { - smart.backward(); //Turn rackwards - } - - else if( c == 't') { - smart.pid_twistA(); - } else if(c == 'v'){ - smart.showOdom(); - } else if(c == 'o') { //Turns on chair - pc.printf("turning on\r\n"); - on = 1; - wait(1); - on = 0; - } else if(c == 'f') { //Turns off chair - pc.printf("turning off\r\n"); - off = 1; - wait(1); - off = 0; - - } else if(c == 'k'){ //Sends command to go to the kitchen - smart.pid_twistV(); - } else if( c == 'm' || manual) { //Turns wheelchair to joystick - pc.printf("turning on joystick\r\n"); - manual = true; - t.reset(); - while(manual) { - smart.move(x,y); //Reads from joystick and moves - if( pc.readable()) { - char d = pc.getc(); - if( d == 'm') { //Turns wheelchair from joystick into auto - pc.printf("turning off joystick\r\n"); - manual = false; - } - } - } - } - else { - pc.printf("none \r\n"); - smart.stop(); //If nothing else is happening stop the chair - } + /* If Ros comands the wheelchair to move fowards or backwards*/ + 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 + { + smart.stop(); //Stops the chair + test2 = 0; + test1 = 0; } - else { - - smart.stop(); //If nothing else is happening stop the chair - } - - wait(process); + wait(process); //Delay } + } +/* 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); + + /* 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; + + /* 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; + + /* 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