File content as of revision 8:db780b392bae:

#include "wheelchair.h"

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

/*QEI wheel2 (D3, D6, NC, 1200);
DigitalIn pt1(D3, PullUp);
DigitalIn pt2(D6, PullUp);*/

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

DigitalOut up(D2);
DigitalOut down(D3);
DigitalOut on(D12);
DigitalOut off(D11);
bool manual = false;

Serial pc(USBTX, USBRX, 57600);

Timer t;
EventQueue queue;

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


int main(void)
{
    queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::compass_thread);
    t.reset();
    thread.start(callback(&queue, &EventQueue::dispatch_forever));
    while(1) {
        if( pc.readable()) {
            char c = pc.getc();
            if( c == 'w') {
                //pc.printf("up \r\n");
                smart.forward();
                pc.printf("%f\r\n", wheel.getDistance(53.975));
            }
            else if( c == 'a') {
                //pc.printf("left \r\n");
                smart.left();
            }

            else if( c == 'd') {
                //pc.printf("right \r\n");
                smart.right();
            }

            else if( c == 's') {
                //pc.printf("down \r\n");
                smart.backward();
               // wheel.reset();
            }

            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: ");
                char d = pc.getc();
                int angle = 0;
                //fgets (buffer, 256, stdin);
                if(d == 'r')
                {
                     angle = 90;//atoi (buffer);
                }                
                if(d == 'l')
                {
                     angle = -90;//atoi (buffer);
                }
                if(angle == 0) {
                    pc.printf("invalid input try again\r\n");
                } else {
                    smart.pid_turn(angle);
                }

            } else if(c == 'o') {
                pc.printf("turning on\r\n");
                on = 1;
                wait(1);
                on = 0;
            } else if(c == 'f') {
                pc.printf("turning off\r\n");
                off = 1;
                wait(1);
                off = 0;
            } else if(c == 'k'){
                pc.printf("kitchen\r\n");
                smart.kitchen();
            } else if(c == 'e'){
                pc.printf("desk\r\n");
                smart.desk();
            } else if(c == 'x'){
                pc.printf("desk to kitchen\r\n");
                smart.desk_to_kitchen();                
            } else if(c == 'u') {
                pc.printf ("Enter a long number: ");
                char d = pc.getc();
                double displacement = 0;
                if(d == '1')
                    displacement = 5461;
                if(d == '2')
                    displacement = 3658;
                if(d == '3')
                    displacement = 305;
                if(d == '4')
                    displacement = 1000;
                if(displacement > 0)
                {
                    smart.pid_forward(displacement);
                }
                else if(displacement < 0)
                {
                    smart.pid_reverse(displacement);
                }    
            } else if( c == 'm' || manual) {
                pc.printf("turning on joystick\r\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\r\n");
                            manual = false;
                        }
                    }
                }   
            }
            else {
                pc.printf("none \r\n");
                smart.stop();
            }
        }

        else {
            //        pc.printf("Nothing pressed \r\n");
            smart.stop();
        }
        wait(process);
    }

}