File content as of revision 11:2cb176f5b345:

#include "wheelchair.h"

AnalogIn x(A0);
AnalogIn y(A1);

/*
DigitalOut on(D12);
DigitalOut off(D11);
DigitalOut up(D2);
DigitalOut down(D3);*/

Serial pc(USBTX, USBRX, 57600);
Timer t;
Thread thread;
EventQueue queue;

QEI wheel(D0, D1, NC, 1200);
DigitalIn pt3(D1, PullUp);
DigitalIn pt4(D0, PullUp);

Wheelchair smart(xDir,yDir, &pc, &t, &wheel);

char c;
bool manual = false;

ros::NodeHandle nh;
//geometry_msgs::Twist commandRead;
std_msgs::String commandRead;

ros::Publisher chatter("chatter", &commandRead);
volatile bool received = false;

void handlerFunction(const std_msgs::String& commandIn)
{
    commandRead = commandIn;
    received = true;
}

ros::Subscriber<std_msgs::String> sub("cmd_vel", &handlerFunction);


void setupNode()
{
    nh.initNode();
    nh.subscribe(sub);
    nh.advertise(chatter);
    char initialVal[2] = "z";
    commandRead.data = initialVal;
}

int main(void)
{
    
    queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::compass_thread);
    t.reset();
    thread.start(callback(&queue, &EventQueue::dispatch_forever));
    setupNode();
    
    while(1) {
        if( received) {
        c = commandRead.data[0];
            if( c == 'w') {
                pc.printf("up \n");
                smart.forward();
            }

            else if( c == 'a') {
                //pc.printf("left \n");
                smart.left();
            }

            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.pid_right(90);
                c = 'z';
            }

            else if( c == 'l') {
                smart.pid_left(90);
                c = 'z';
            }

            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 == 'o') {
                pc.printf("turning on\n");
                char* temp = "turning on";
                commandRead.data = temp;
                chatter.publish(&commandRead);
                /*on = 1;
                wait(1);
                on = 0;*/
                smart.turn_on();
                c = 'z';
            } else if(c == 'f') {
                pc.printf("turning off\n");
                /*off = 1;
                wait(1);
                off = 0;*/
                smart.turn_off();
                c = 'z';
            }

            else if( c == 'm' || manual) {
                pc.printf("turning on joystick\n");
                manual = true;
                t.reset();
                received = false;
                
                while(manual) {
                    smart.move(x,y);
                    if(received) {
                        char d = commandRead.data[0];
                        if( d == 'u') {
                            pc.printf("turning off joystick\n");
                            manual = false;
                        }
                    received = false;
                    }
                }
            }
        else if( c == 'k') {
            smart.kitchen();
            
            c = 'z';
            }
        else if( c == 'i') {
            smart.desk();
            }
        else if( c == 'c') {
            smart.back();
            }
            else {
                pc.printf("none \n");
                smart.stop();
            }
            received = false;
        }
        
        else {
            //        pc.printf("Nothing pressed \n");
            smart.stop();
        }
        nh.spinOnce();
        wait(process);
    }

}