with pid foward right left foward
Dependencies: wheelchaircontrolRosCom
Fork of wheelchaircontrolrealtime by
main.cpp
00001 #include "wheelchair.h" 00002 00003 QEI wheel(D0, D1, NC, 1200); //Initializes right encoder 00004 DigitalIn pt3(D1, PullUp); //Pull up resistors to read analog signals into digital signals 00005 DigitalIn pt4(D0, PullUp); 00006 00007 QEI wheelS (D3, D6, NC, 1200); //Initializes Left encoder 00008 DigitalIn pt1(D3, PullUp); //Pull up resistors to read analog signals into digital signals 00009 DigitalIn pt2(D6, PullUp); 00010 00011 AnalogIn x(A0); //Initializes analog axis for the joystick 00012 AnalogIn y(A1); 00013 00014 DigitalOut up(D2); //Turn up speed mode for joystick 00015 DigitalOut down(D8); //Turn down speed mode for joystick 00016 DigitalOut on(D12); //Turn Wheelchair On 00017 DigitalOut off(D11); //Turn Wheelchair Off 00018 bool manual = false; //Turns chair joystic to automatic and viceverza 00019 00020 Serial pc(USBTX, USBRX, 57600); //Serial Monitor 00021 00022 Timer t; //Initialize time object t 00023 EventQueue queue; //Class to organize threads 00024 00025 ros::NodeHandle nh; 00026 nav_msgs::Odometry odom; 00027 geometry_msgs::Twist commandRead; 00028 00029 void handlerFunction(const geometry_msgs::Twist& command){ 00030 commandRead = command; 00031 } 00032 00033 ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &handlerFunction); 00034 ros::Publisher chatter("odom", &odom); 00035 ros::Publisher chatter2("cmd_vel", &commandRead); 00036 00037 Wheelchair smart(xDir,yDir, &pc, &t, &wheel, &wheelS); //Initialize wheelchair object 00038 Thread compass; //Thread for compass 00039 Thread velosity; //Thread for velosity 00040 Thread ros_com; //Thread for velosity 00041 00042 void rosCom_thread() 00043 { 00044 odom.pose.pose.position.x = smart.x_position; 00045 odom.pose.pose.position.y = smart.y_position; 00046 odom.pose.pose.position.z = 0; 00047 //set the orientation 00048 odom.pose.pose.orientation.z = smart.z_angular; 00049 odom.pose.pose.orientation.x = 0; 00050 odom.pose.pose.orientation.y = 0; 00051 odom.twist.twist.linear.x = smart.curr_vel; 00052 odom.twist.twist.linear.y = 0; 00053 odom.twist.twist.linear.z = 0; 00054 odom.twist.twist.angular.x = 0; 00055 odom.twist.twist.angular.y = 0; 00056 odom.twist.twist.angular.z = smart.getTwistZ(); 00057 chatter.publish(&odom); 00058 chatter2.publish(&commandRead); 00059 nh.spinOnce(); 00060 } 00061 int main(void) 00062 { 00063 nh.initNode(); 00064 nh.advertise(chatter); 00065 nh.advertise(chatter2); 00066 nh.subscribe(sub); 00067 00068 queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::compass_thread); //Sets up sampling frequency of the compass_thread 00069 queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::velosity_thread); //Sets up sampling frequency of the velosity_thread 00070 queue.call_every(200, rosCom_thread); //Sets up sampling frequency of the velosity_thread 00071 t.reset(); 00072 compass.start(callback(&queue, &EventQueue::dispatch_forever)); //Starts running the compass thread 00073 velosity.start(callback(&queue, &EventQueue::dispatch_forever)); //Starts running the velosity thread 00074 ros_com.start(callback(&queue, &EventQueue::dispatch_forever)); //Starts running the velosity thread 00075 while(1) { 00076 if( pc.readable()) { 00077 char c = pc.getc(); //Read the instruction sent 00078 if( c == 'w') { 00079 smart.forward(); //Move foward 00080 } 00081 else if( c == 'a') { 00082 smart.left(); //Turn left 00083 } 00084 else if( c == 'd') { 00085 smart.right(); //Turn right 00086 } 00087 else if( c == 's') { 00088 smart.backward(); //Turn rackwards 00089 } 00090 00091 else if( c == 't') { 00092 smart.pid_twistA(); 00093 /* 00094 char buffer[256]; //Buffer for Angle to turn 00095 pc.printf ("Enter a long number: "); 00096 int angle = 0; 00097 00098 //if you can not send a string 00099 char d = pc.getc(); //Reads wether to turn right or left 00100 if(d == 'r') 00101 { 00102 angle = 90; 00103 } 00104 if(d == 'l') 00105 { 00106 angle = -90; 00107 } 00108 00109 //if you can send a string 00110 /* 00111 fgets (buffer, 256, stdin); //Reads string and puts it in a buffer 00112 angle = atoi(buffer); //Converst string into integer 00113 */ 00114 /* 00115 if(angle == 0) { 00116 pc.printf("invalid input try again\r\n"); 00117 } else { 00118 smart.pid_turn(angle); //Sends instruction to turn desired angle 00119 }*/ 00120 } else if(c == 'v'){ 00121 smart.showOdom(); 00122 } else if(c == 'o') { //Turns on chair 00123 pc.printf("turning on\r\n"); 00124 on = 1; 00125 wait(1); 00126 on = 0; 00127 } else if(c == 'f') { //Turns off chair 00128 pc.printf("turning off\r\n"); 00129 off = 1; 00130 wait(1); 00131 off = 0; 00132 00133 } else if(c == 'k'){ //Sends command to go to the kitchen 00134 smart.pid_twistV(); 00135 } else if( c == 'm' || manual) { //Turns wheelchair to joystick 00136 pc.printf("turning on joystick\r\n"); 00137 manual = true; 00138 t.reset(); 00139 while(manual) { 00140 smart.move(x,y); //Reads from joystick and moves 00141 if( pc.readable()) { 00142 char d = pc.getc(); 00143 if( d == 'm') { //Turns wheelchair from joystick into auto 00144 pc.printf("turning off joystick\r\n"); 00145 manual = false; 00146 } 00147 } 00148 } 00149 } else { 00150 pc.printf("none \r\n"); 00151 smart.stop(); //If nothing else is happening stop the chair 00152 } 00153 } 00154 else { 00155 smart.stop(); //If nothing else is happening stop the chair 00156 } 00157 00158 wait(process); 00159 } 00160 }
Generated on Tue Jul 12 2022 23:48:18 by 1.7.2