ryan lin / Mbed OS wheelchaircontrolrealtimeROS

for ros integration

Dependencies:   wheelchaircontrolRos

Fork of wheelchaircontrolrealtime by ryan lin

main.cpp@8:347b10e6bdf8, 2018-08-27 (annotated)

Committer:
ryanlin97
Date:
Mon Aug 27 23:12:06 2018 +0000
Revision:
8:347b10e6bdf8
Parent:
7:cead0e7b0c1e
Child:
9:90bc440405b6
latest ver.

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 7:cead0e7b0c1e 6 DigitalOut on(D12);
ryanlin97 7:cead0e7b0c1e 7 DigitalOut off(D11);
ryanlin97 0:7e6b349182bc 8 DigitalOut up(D2);
ryanlin97 0:7e6b349182bc 9 DigitalOut down(D3);
ryanlin97 8:347b10e6bdf8 10 DigitalOut horn(D10);
ryanlin97 0:7e6b349182bc 11
ryanlin97 7:cead0e7b0c1e 12 Serial pc(USBTX, USBRX, 57600);
ryanlin97 7:cead0e7b0c1e 13 Timer t;
ryanlin97 7:cead0e7b0c1e 14 Thread thread;
ryanlin97 7:cead0e7b0c1e 15 EventQueue queue;
ryanlin97 7:cead0e7b0c1e 16 Wheelchair smart(xDir,yDir, &pc, &t);
ryanlin97 7:cead0e7b0c1e 17
ryanlin97 3:ef063fd4234e 18 char c;
ryanlin97 0:7e6b349182bc 19 bool manual = false;
ryanlin97 0:7e6b349182bc 20
ryanlin97 3:ef063fd4234e 21 ros::NodeHandle nh;
ryanlin97 7:cead0e7b0c1e 22 //geometry_msgs::Twist commandRead;
ryanlin97 7:cead0e7b0c1e 23 std_msgs::String commandRead;
ryanlin97 7:cead0e7b0c1e 24 std_msgs::String out;
ryanlin97 3:ef063fd4234e 25
ryanlin97 7:cead0e7b0c1e 26 ros::Publisher chatter("chatter", &commandRead);
ryanlin97 7:cead0e7b0c1e 27 bool received = false;
ryanlin97 7:cead0e7b0c1e 28
ryanlin97 7:cead0e7b0c1e 29 void handlerFunction(const std_msgs::String& commandIn)
ryanlin97 3:ef063fd4234e 30 {
ryanlin97 7:cead0e7b0c1e 31 commandRead = commandIn;
ryanlin97 3:ef063fd4234e 32 received = true;
ryanlin97 3:ef063fd4234e 33 }
ryanlin97 3:ef063fd4234e 34
ryanlin97 7:cead0e7b0c1e 35 ros::Subscriber<std_msgs::String> 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 7:cead0e7b0c1e 43 char initialVal[2] = "z";
ryanlin97 7:cead0e7b0c1e 44 commandRead.data = initialVal;
ryanlin97 3:ef063fd4234e 45 }
ryanlin97 3:ef063fd4234e 46
ryanlin97 0:7e6b349182bc 47 int main(void)
ryanlin97 0:7e6b349182bc 48 {
ryanlin97 6:b8a1fa98707e 49
ryanlin97 0:7e6b349182bc 50 queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::compass_thread);
ryanlin97 0:7e6b349182bc 51 t.reset();
ryanlin97 0:7e6b349182bc 52 thread.start(callback(&queue, &EventQueue::dispatch_forever));
ryanlin97 3:ef063fd4234e 53 setupNode();
ryanlin97 7:cead0e7b0c1e 54
ryanlin97 6:b8a1fa98707e 55 while(1) {
ryanlin97 5:186967c92ef5 56 if( received) {
ryanlin97 7:cead0e7b0c1e 57 c = commandRead.data[0];
ryanlin97 5:186967c92ef5 58 if( c == 'w') {
ryanlin97 5:186967c92ef5 59 pc.printf("up \n");
ryanlin97 5:186967c92ef5 60 smart.forward();
ryanlin97 5:186967c92ef5 61 }
ryanlin97 0:7e6b349182bc 62
ryanlin97 5:186967c92ef5 63 else if( c == 'a') {
ryanlin97 5:186967c92ef5 64 //pc.printf("left \n");
ryanlin97 5:186967c92ef5 65 smart.left();
ryanlin97 5:186967c92ef5 66 }
ryanlin97 0:7e6b349182bc 67
ryanlin97 5:186967c92ef5 68 else if( c == 'd') {
ryanlin97 5:186967c92ef5 69 //pc.printf("right \n");
ryanlin97 5:186967c92ef5 70 smart.right();
ryanlin97 5:186967c92ef5 71 }
ryanlin97 0:7e6b349182bc 72
ryanlin97 5:186967c92ef5 73 else if( c == 's') {
ryanlin97 5:186967c92ef5 74 pc.printf("down \n");
ryanlin97 5:186967c92ef5 75 smart.backward();
ryanlin97 5:186967c92ef5 76 }
ryanlin97 0:7e6b349182bc 77
ryanlin97 5:186967c92ef5 78 else if( c == 'r') {
ryanlin97 7:cead0e7b0c1e 79 smart.pid_right(90);
ryanlin97 8:347b10e6bdf8 80 c = 'z';
ryanlin97 5:186967c92ef5 81 }
ryanlin97 0:7e6b349182bc 82
ryanlin97 5:186967c92ef5 83 else if( c == 'l') {
ryanlin97 7:cead0e7b0c1e 84 smart.pid_left(90);
ryanlin97 8:347b10e6bdf8 85 c = 'z';
ryanlin97 0:7e6b349182bc 86 }
ryanlin97 0:7e6b349182bc 87
ryanlin97 5:186967c92ef5 88 else if( c == 't') {
ryanlin97 5:186967c92ef5 89 char buffer[256];
ryanlin97 5:186967c92ef5 90 pc.printf ("Enter a long number: ");
ryanlin97 5:186967c92ef5 91 fgets (buffer, 256, stdin);
ryanlin97 5:186967c92ef5 92 int angle = atoi (buffer);
ryanlin97 5:186967c92ef5 93
ryanlin97 5:186967c92ef5 94 if(angle == 0) {
ryanlin97 5:186967c92ef5 95 pc.printf("invalid input try again\n");
ryanlin97 5:186967c92ef5 96 } else {
ryanlin97 5:186967c92ef5 97 smart.pid_turn(angle);
ryanlin97 5:186967c92ef5 98 }
ryanlin97 0:7e6b349182bc 99
ryanlin97 6:b8a1fa98707e 100 } else if(c == 'o') {
ryanlin97 6:b8a1fa98707e 101 pc.printf("turning on\n");
ryanlin97 6:b8a1fa98707e 102 on = 1;
ryanlin97 6:b8a1fa98707e 103 wait(1);
ryanlin97 6:b8a1fa98707e 104 on = 0;
ryanlin97 7:cead0e7b0c1e 105 c = 'z';
ryanlin97 6:b8a1fa98707e 106 } else if(c == 'f') {
ryanlin97 6:b8a1fa98707e 107 pc.printf("turning off\n");
ryanlin97 6:b8a1fa98707e 108 off = 1;
ryanlin97 6:b8a1fa98707e 109 wait(1);
ryanlin97 6:b8a1fa98707e 110 off = 0;
ryanlin97 7:cead0e7b0c1e 111 c = 'z';
ryanlin97 5:186967c92ef5 112 }
ryanlin97 5:186967c92ef5 113
ryanlin97 5:186967c92ef5 114 else if( c == 'm' || manual) {
ryanlin97 5:186967c92ef5 115 pc.printf("turning on joystick\n");
ryanlin97 5:186967c92ef5 116 manual = true;
ryanlin97 5:186967c92ef5 117 t.reset();
ryanlin97 5:186967c92ef5 118 while(manual) {
ryanlin97 5:186967c92ef5 119 smart.move(x,y);
ryanlin97 5:186967c92ef5 120 if( pc.readable()) {
ryanlin97 5:186967c92ef5 121 char d = pc.getc();
ryanlin97 5:186967c92ef5 122 if( d == 'm') {
ryanlin97 5:186967c92ef5 123 pc.printf("turning off joystick\n");
ryanlin97 5:186967c92ef5 124 manual = false;
ryanlin97 5:186967c92ef5 125 }
ryanlin97 0:7e6b349182bc 126 }
ryanlin97 0:7e6b349182bc 127 }
ryanlin97 0:7e6b349182bc 128 }
ryanlin97 5:186967c92ef5 129
ryanlin97 5:186967c92ef5 130 else {
ryanlin97 5:186967c92ef5 131 pc.printf("none \n");
ryanlin97 5:186967c92ef5 132 smart.stop();
ryanlin97 5:186967c92ef5 133 }
ryanlin97 7:cead0e7b0c1e 134 received = false;
ryanlin97 0:7e6b349182bc 135 }
ryanlin97 0:7e6b349182bc 136
ryanlin97 0:7e6b349182bc 137 else {
ryanlin97 5:186967c92ef5 138 // pc.printf("Nothing pressed \n");
ryanlin97 0:7e6b349182bc 139 smart.stop();
ryanlin97 0:7e6b349182bc 140 }
ryanlin97 8:347b10e6bdf8 141 nh.spinOnce();
ryanlin97 0:7e6b349182bc 142 wait(process);
ryanlin97 0:7e6b349182bc 143 }
ryanlin97 0:7e6b349182bc 144
ryanlin97 0:7e6b349182bc 145 }
ryanlin97 0:7e6b349182bc 146
ryanlin97 8:347b10e6bdf8 147 void beep() {
ryanlin97 8:347b10e6bdf8 148 horn = 1;
ryanlin97 8:347b10e6bdf8 149 wait(1);
ryanlin97 8:347b10e6bdf8 150 horn =0;
ryanlin97 8:347b10e6bdf8 151 }
ryanlin97 8:347b10e6bdf8 152
ryanlin97 3:ef063fd4234e 153
ryanlin97 3:ef063fd4234e 154
ryanlin97 3:ef063fd4234e 155
ryanlin97 3:ef063fd4234e 156
ryanlin97 3:ef063fd4234e 157
ryanlin97 3:ef063fd4234e 158