ryan lin / Mbed OS wheelchaircontrolrealtimeROS

for ros integration

Dependencies:   wheelchaircontrolRos

Fork of wheelchaircontrolrealtime by ryan lin

main.cpp@11:2cb176f5b345, 2018-08-31 (annotated)

Committer:
ryanlin97
Date:
Fri Aug 31 19:42:08 2018 +0000
Revision:
11:2cb176f5b345
Parent:
9:90bc440405b6 
for alexa and path;

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