File content as of revision 4:db3aa99ab312:

#include "wheelchair.h"

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

DigitalOut off(D0);
DigitalOut on(D1);
DigitalOut up(D2);
DigitalOut down(D3);

char c;
bool manual = false;
bool received = false;
Serial pc(USBTX, USBRX, 57600);

Timer t;
EventQueue queue;

//MPU9250 imu(D14, D15);
Wheelchair smart(xDir,yDir, &pc, &t);
Thread thread;

ros::NodeHandle nh;
geometry_msgs::Twist commandRead;
ros::Publisher chatter("chatter", &commandRead);

void handlerFunction(const geometry_msgs::Twist& command)
{
    if(command.linear.x >0) {
        c = 'w';
        }
    received = true;
}

ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &handlerFunction);


void setupNode()
{
    nh.initNode();
    nh.subscribe(sub);
    nh.advertise(chatter);
}

int main(void)
{
    queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::compass_thread);
    t.reset();
    thread.start(callback(&queue, &EventQueue::dispatch_forever));
    setupNode();
    if(received) {

        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.turn_right(90);
        }

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

        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 == 'm' || manual) {
            pc.printf("turning on joystick\n");
            manual = true;
            t.reset();
            while(manual) {
                smart.move(x,y);
                if( pc.readable()) {
                    char d = pc.getc();
                    if( d == 'm') {
                        pc.printf("turning off joystick\n");
                        manual = false;
                    }
                }
            }
        }

        else {
            pc.printf("none \n");
            smart.stop();
        }

        wait(process);
        received = !received;
    }

}



/*
void publishTwist(ros::Publisher* toChat) [
    toChat->publish( &commandRead);
    nh.spinOnce();
}*/