ryan lin / Mbed OS wheelchaircontrolrealtimeROS

for ros integration

Dependencies:   wheelchaircontrolRos

Fork of wheelchaircontrolrealtime by ryan lin

main.cpp@5:186967c92ef5, 2018-08-13 (annotated)

Committer:
ryanlin97
Date:
Mon Aug 13 21:53:21 2018 +0000
Revision:
5:186967c92ef5
Parent:
4:db3aa99ab312
Child:
6:b8a1fa98707e
while loop;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ryanlin97 0:7e6b349182bc 1 #include "wheelchair.h"
ryanlin97 0:7e6b349182bc 2
ryanlin97 0:7e6b349182bc 3 AnalogIn x(A0);
ryanlin97 0:7e6b349182bc 4 AnalogIn y(A1);
ryanlin97 0:7e6b349182bc 5
ryanlin97 0:7e6b349182bc 6 DigitalOut off(D0);
ryanlin97 0:7e6b349182bc 7 DigitalOut on(D1);
ryanlin97 0:7e6b349182bc 8 DigitalOut up(D2);
ryanlin97 0:7e6b349182bc 9 DigitalOut down(D3);
ryanlin97 0:7e6b349182bc 10
ryanlin97 3:ef063fd4234e 11 char c;
ryanlin97 0:7e6b349182bc 12 bool manual = false;
ryanlin97 3:ef063fd4234e 13 bool received = false;
ryanlin97 3:ef063fd4234e 14 Serial pc(USBTX, USBRX, 57600);
ryanlin97 0:7e6b349182bc 15
ryanlin97 0:7e6b349182bc 16 Timer t;
ryanlin97 0:7e6b349182bc 17 EventQueue queue;
ryanlin97 0:7e6b349182bc 18
ryanlin97 0:7e6b349182bc 19 //MPU9250 imu(D14, D15);
ryanlin97 0:7e6b349182bc 20 Wheelchair smart(xDir,yDir, &pc, &t);
ryanlin97 0:7e6b349182bc 21 Thread thread;
ryanlin97 0:7e6b349182bc 22
ryanlin97 3:ef063fd4234e 23 ros::NodeHandle nh;
ryanlin97 3:ef063fd4234e 24 geometry_msgs::Twist commandRead;
ryanlin97 3:ef063fd4234e 25 ros::Publisher chatter("chatter", &commandRead);
ryanlin97 3:ef063fd4234e 26
ryanlin97 3:ef063fd4234e 27 void handlerFunction(const geometry_msgs::Twist& command)
ryanlin97 3:ef063fd4234e 28 {
ryanlin97 3:ef063fd4234e 29 if(command.linear.x >0) {
ryanlin97 4:db3aa99ab312 30 c = 'w';
ryanlin97 3:ef063fd4234e 31 }
ryanlin97 3:ef063fd4234e 32 received = true;
ryanlin97 3:ef063fd4234e 33 }
ryanlin97 3:ef063fd4234e 34
ryanlin97 3:ef063fd4234e 35 ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &handlerFunction);
ryanlin97 3:ef063fd4234e 36
ryanlin97 3:ef063fd4234e 37
ryanlin97 3:ef063fd4234e 38 void setupNode()
ryanlin97 3:ef063fd4234e 39 {
ryanlin97 3:ef063fd4234e 40 nh.initNode();
ryanlin97 3:ef063fd4234e 41 nh.subscribe(sub);
ryanlin97 3:ef063fd4234e 42 nh.advertise(chatter);
ryanlin97 3:ef063fd4234e 43 }
ryanlin97 3:ef063fd4234e 44
ryanlin97 0:7e6b349182bc 45 int main(void)
ryanlin97 0:7e6b349182bc 46 {
ryanlin97 0:7e6b349182bc 47 queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::compass_thread);
ryanlin97 0:7e6b349182bc 48 t.reset();
ryanlin97 0:7e6b349182bc 49 thread.start(callback(&queue, &EventQueue::dispatch_forever));
ryanlin97 3:ef063fd4234e 50 setupNode();
ryanlin97 5:186967c92ef5 51 while(1) {
ryanlin97 5:186967c92ef5 52 if( received) {
ryanlin97 0:7e6b349182bc 53
ryanlin97 5:186967c92ef5 54 if( c == 'w') {
ryanlin97 5:186967c92ef5 55 pc.printf("up \n");
ryanlin97 5:186967c92ef5 56 smart.forward();
ryanlin97 5:186967c92ef5 57 }
ryanlin97 0:7e6b349182bc 58
ryanlin97 5:186967c92ef5 59 else if( c == 'a') {
ryanlin97 5:186967c92ef5 60 //pc.printf("left \n");
ryanlin97 5:186967c92ef5 61 smart.left();
ryanlin97 5:186967c92ef5 62 }
ryanlin97 0:7e6b349182bc 63
ryanlin97 5:186967c92ef5 64 else if( c == 'd') {
ryanlin97 5:186967c92ef5 65 //pc.printf("right \n");
ryanlin97 5:186967c92ef5 66 smart.right();
ryanlin97 5:186967c92ef5 67 }
ryanlin97 0:7e6b349182bc 68
ryanlin97 5:186967c92ef5 69 else if( c == 's') {
ryanlin97 5:186967c92ef5 70 pc.printf("down \n");
ryanlin97 5:186967c92ef5 71 smart.backward();
ryanlin97 5:186967c92ef5 72 }
ryanlin97 0:7e6b349182bc 73
ryanlin97 5:186967c92ef5 74 else if( c == 'r') {
ryanlin97 5:186967c92ef5 75 smart.turn_right(90);
ryanlin97 5:186967c92ef5 76 }
ryanlin97 0:7e6b349182bc 77
ryanlin97 5:186967c92ef5 78 else if( c == 'l') {
ryanlin97 5:186967c92ef5 79 smart.turn_left(90);
ryanlin97 0:7e6b349182bc 80 }
ryanlin97 0:7e6b349182bc 81
ryanlin97 5:186967c92ef5 82 else if( c == 't') {
ryanlin97 5:186967c92ef5 83 char buffer[256];
ryanlin97 5:186967c92ef5 84 pc.printf ("Enter a long number: ");
ryanlin97 5:186967c92ef5 85 fgets (buffer, 256, stdin);
ryanlin97 5:186967c92ef5 86 int angle = atoi (buffer);
ryanlin97 5:186967c92ef5 87
ryanlin97 5:186967c92ef5 88 if(angle == 0) {
ryanlin97 5:186967c92ef5 89 pc.printf("invalid input try again\n");
ryanlin97 5:186967c92ef5 90 } else {
ryanlin97 5:186967c92ef5 91 smart.pid_turn(angle);
ryanlin97 5:186967c92ef5 92 }
ryanlin97 0:7e6b349182bc 93
ryanlin97 5:186967c92ef5 94 }
ryanlin97 5:186967c92ef5 95
ryanlin97 5:186967c92ef5 96 else if( c == 'm' || manual) {
ryanlin97 5:186967c92ef5 97 pc.printf("turning on joystick\n");
ryanlin97 5:186967c92ef5 98 manual = true;
ryanlin97 5:186967c92ef5 99 t.reset();
ryanlin97 5:186967c92ef5 100 while(manual) {
ryanlin97 5:186967c92ef5 101 smart.move(x,y);
ryanlin97 5:186967c92ef5 102 if( pc.readable()) {
ryanlin97 5:186967c92ef5 103 char d = pc.getc();
ryanlin97 5:186967c92ef5 104 if( d == 'm') {
ryanlin97 5:186967c92ef5 105 pc.printf("turning off joystick\n");
ryanlin97 5:186967c92ef5 106 manual = false;
ryanlin97 5:186967c92ef5 107 }
ryanlin97 0:7e6b349182bc 108 }
ryanlin97 0:7e6b349182bc 109 }
ryanlin97 0:7e6b349182bc 110 }
ryanlin97 5:186967c92ef5 111
ryanlin97 5:186967c92ef5 112 else {
ryanlin97 5:186967c92ef5 113 pc.printf("none \n");
ryanlin97 5:186967c92ef5 114 smart.stop();
ryanlin97 5:186967c92ef5 115 }
ryanlin97 0:7e6b349182bc 116 }
ryanlin97 0:7e6b349182bc 117
ryanlin97 0:7e6b349182bc 118 else {
ryanlin97 5:186967c92ef5 119 // pc.printf("Nothing pressed \n");
ryanlin97 0:7e6b349182bc 120 smart.stop();
ryanlin97 0:7e6b349182bc 121 }
ryanlin97 5:186967c92ef5 122 received = !received;
ryanlin97 0:7e6b349182bc 123 wait(process);
ryanlin97 0:7e6b349182bc 124 }
ryanlin97 0:7e6b349182bc 125
ryanlin97 0:7e6b349182bc 126 }
ryanlin97 0:7e6b349182bc 127
ryanlin97 3:ef063fd4234e 128
ryanlin97 3:ef063fd4234e 129
ryanlin97 3:ef063fd4234e 130 /*
ryanlin97 3:ef063fd4234e 131 void publishTwist(ros::Publisher* toChat) [
ryanlin97 3:ef063fd4234e 132 toChat->publish( &commandRead);
ryanlin97 3:ef063fd4234e 133 nh.spinOnce();
ryanlin97 3:ef063fd4234e 134 }*/
ryanlin97 3:ef063fd4234e 135
ryanlin97 3:ef063fd4234e 136
ryanlin97 3:ef063fd4234e 137
ryanlin97 3:ef063fd4234e 138
ryanlin97 3:ef063fd4234e 139