![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
changed pins for e_button and added code for turning off PCB
Dependencies: wheelchairControlSummer2019 Watchdog ros_lib_kinetic
Diff: main.cpp
- Revision:
- 3:ef063fd4234e
- Parent:
- 0:7e6b349182bc
- Child:
- 4:db3aa99ab312
diff -r 7f22873e729b -r ef063fd4234e main.cpp --- a/main.cpp Mon Aug 13 18:23:02 2018 +0000 +++ b/main.cpp Mon Aug 13 21:33:17 2018 +0000 @@ -8,9 +8,11 @@ DigitalOut up(D2); DigitalOut down(D3); +char c; bool manual = false; +bool received = false; +Serial pc(USBTX, USBRX, 57600); -Serial pc(USBTX, USBRX, 57600); Timer t; EventQueue queue; @@ -18,85 +20,114 @@ Wheelchair smart(xDir,yDir, &pc, &t); Thread thread; +ros::NodeHandle nh; +geometry_msgs::Twist commandRead; +ros::Publisher chatter("chatter", &commandRead); + +void handlerFunction(const geometry_msgs::Twist& command) +{ + if(command.linear.x >0) { + + } + received = true; +} + +ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &handlerFunction); + + +void setupNode() +{ + nh.initNode(); + nh.subscribe(sub); + nh.advertise(chatter); +} + int main(void) { queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::compass_thread); t.reset(); thread.start(callback(&queue, &EventQueue::dispatch_forever)); - while(1) { - if( pc.readable()) { - char c = pc.getc(); + setupNode(); + if(received) { + + if( c == 'w') { + pc.printf("up \n"); + smart.forward(); + } - if( c == 'w') { - pc.printf("up \n"); - smart.forward(); - } + else if( c == 'a') { + //pc.printf("left \n"); + smart.left(); + } - else if( c == 'a') { - //pc.printf("left \n"); - smart.left(); - } + else if( c == 'd') { + //pc.printf("right \n"); + smart.right(); + } - else if( c == 'd') { - //pc.printf("right \n"); - smart.right(); - } + else if( c == 's') { + pc.printf("down \n"); + smart.backward(); + } + + else if( c == 'r') { + smart.turn_right(90); + } - else if( c == 's') { - pc.printf("down \n"); - smart.backward(); - } + else if( c == 'l') { + smart.turn_left(90); + } - else if( c == 'r') { - smart.turn_right(90); - } + else if( c == 't') { + char buffer[256]; + pc.printf ("Enter a long number: "); + fgets (buffer, 256, stdin); + int angle = atoi (buffer); - else if( c == 'l') { - smart.turn_left(90); + if(angle == 0) { + pc.printf("invalid input try again\n"); + } else { + smart.pid_turn(angle); } - else if( c == 't') { - char buffer[256]; - pc.printf ("Enter a long number: "); - fgets (buffer, 256, stdin); - int angle = atoi (buffer); - - if(angle == 0) { - pc.printf("invalid input try again\n"); - } else { - smart.pid_turn(angle); - } + } - } - - else if( c == 'm' || manual) { - pc.printf("turning on joystick\n"); - manual = true; - t.reset(); - while(manual) { - smart.move(x,y); - if( pc.readable()) { - char d = pc.getc(); - if( d == 'm') { - pc.printf("turning off joystick\n"); - manual = false; - } + else if( c == 'm' || manual) { + pc.printf("turning on joystick\n"); + manual = true; + t.reset(); + while(manual) { + smart.move(x,y); + if( pc.readable()) { + char d = pc.getc(); + if( d == 'm') { + pc.printf("turning off joystick\n"); + manual = false; } } } - - else { - pc.printf("none \n"); - smart.stop(); - } } else { - // pc.printf("Nothing pressed \n"); + pc.printf("none \n"); smart.stop(); } + wait(process); + received = !received; } } + + +/* +void publishTwist(ros::Publisher* toChat) [ + toChat->publish( &commandRead); + nh.spinOnce(); +}*/ + + + + +