main.cpp

00001 #include "wheelchair.h"
00002 
00003 AnalogIn x(A0);
00004 AnalogIn y(A1);
00005 
00006 /*
00007 DigitalOut on(D12);
00008 DigitalOut off(D11);
00009 DigitalOut up(D2);
00010 DigitalOut down(D3);*/
00011 
00012 Serial pc(USBTX, USBRX, 57600);
00013 Timer t;
00014 Thread thread;
00015 EventQueue queue;
00016 
00017 QEI wheel(D0, D1, NC, 1200);
00018 DigitalIn pt3(D1, PullUp);
00019 DigitalIn pt4(D0, PullUp);
00020 
00021 Wheelchair smart(xDir,yDir, &pc, &t, &wheel);
00022 
00023 char c;
00024 bool manual = false;
00025 
00026 ros::NodeHandle nh;
00027 //geometry_msgs::Twist commandRead;
00028 std_msgs::String commandRead;
00029 
00030 ros::Publisher chatter("chatter", &commandRead);
00031 volatile bool received = false;
00032 
00033 void handlerFunction(const std_msgs::String& commandIn)
00034 {
00035     commandRead = commandIn;
00036     received = true;
00037 }
00038 
00039 ros::Subscriber<std_msgs::String> sub("cmd_vel", &handlerFunction);
00040 
00041 
00042 void setupNode()
00043 {
00044     nh.initNode();
00045     nh.subscribe(sub);
00046     nh.advertise(chatter);
00047     char initialVal[2] = "z";
00048     commandRead.data = initialVal;
00049 }
00050 
00051 int main(void)
00052 {
00053     
00054     queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::compass_thread);
00055     t.reset();
00056     thread.start(callback(&queue, &EventQueue::dispatch_forever));
00057     setupNode();
00058     
00059     while(1) {
00060         if( received) {
00061         c = commandRead.data[0];
00062             if( c == 'w') {
00063                 pc.printf("up \n");
00064                 smart.forward();
00065             }
00066 
00067             else if( c == 'a') {
00068                 //pc.printf("left \n");
00069                 smart.left();
00070             }
00071 
00072             else if( c == 'd') {
00073                 //pc.printf("right \n");
00074                 smart.right();
00075             }
00076 
00077             else if( c == 's') {
00078                 pc.printf("down \n");
00079                 smart.backward();
00080             }
00081 
00082             else if( c == 'r') {
00083                 smart.pid_right(90);
00084                 c = 'z';
00085             }
00086 
00087             else if( c == 'l') {
00088                 smart.pid_left(90);
00089                 c = 'z';
00090             }
00091 
00092             else if( c == 't') {
00093                 char buffer[256];
00094                 pc.printf ("Enter a long number: ");
00095                 fgets (buffer, 256, stdin);
00096                 int angle = atoi (buffer);
00097 
00098                 if(angle == 0) {
00099                     pc.printf("invalid input try again\n");
00100                 } else {
00101                     smart.pid_turn(angle);
00102                 }
00103 
00104             } else if(c == 'o') {
00105                 pc.printf("turning on\n");
00106                 char* temp = "turning on";
00107                 commandRead.data = temp;
00108                 chatter.publish(&commandRead);
00109                 /*on = 1;
00110                 wait(1);
00111                 on = 0;*/
00112                 smart.turn_on();
00113                 c = 'z';
00114             } else if(c == 'f') {
00115                 pc.printf("turning off\n");
00116                 /*off = 1;
00117                 wait(1);
00118                 off = 0;*/
00119                 smart.turn_off();
00120                 c = 'z';
00121             }
00122 
00123             else if( c == 'm' || manual) {
00124                 pc.printf("turning on joystick\n");
00125                 manual = true;
00126                 t.reset();
00127                 received = false;
00128                 
00129                 while(manual) {
00130                     smart.move(x,y);
00131                     if(received) {
00132                         char d = commandRead.data[0];
00133                         if( d == 'u') {
00134                             pc.printf("turning off joystick\n");
00135                             manual = false;
00136                         }
00137                     received = false;
00138                     }
00139                 }
00140             }
00141         else if( c == 'k') {
00142             smart.kitchen();
00143             
00144             c = 'z';
00145             }
00146         else if( c == 'i') {
00147             smart.desk();
00148             }
00149         else if( c == 'c') {
00150             smart.back();
00151             }
00152             else {
00153                 pc.printf("none \n");
00154                 smart.stop();
00155             }
00156             received = false;
00157         }
00158         
00159         else {
00160             //        pc.printf("Nothing pressed \n");
00161             smart.stop();
00162         }
00163         nh.spinOnce();
00164         wait(process);
00165     }
00166 
00167 }
00168 
00169 
00170 
00171 
00172 
00173 
00174 
00175