main.cpp

00001 #include "wheelchair.h"
00002 
00003 QEI wheel (D10, D9, NC, 1200);          // Initializes right encoder
00004 DigitalIn pt3(D10, PullUp);             // Pull up resistors to read analog signals into digital signals
00005 DigitalIn pt4(D9, PullUp);
00006 
00007 /*added*/
00008 //DigitalIn e_button(D4);               // Emergency button will start at HIGH
00009 
00010 QEI wheelS (D7, D8, NC, 1200);          // Initializes Left encoder
00011 DigitalIn pt1(D7, PullUp);              // Pull up resistors to read analog signals into digital signals
00012 DigitalIn pt2(D8, PullUp);
00013 
00014 int max_velocity;
00015 //Timer testAccT;
00016 
00017 AnalogIn x(A0);                         // Initializes analog axis for the joystick
00018 AnalogIn y(A1);
00019 
00020 //Watchdog limit should be 0.1; Set to 1 for Testing Only
00021 double watchdogLimit = 1;               // Set timeout limit for watchdog timer in seconds
00022 int buttonCheck = 0;
00023 int iteration = 1;
00024 
00025 DigitalOut up(D12);                     // Turn up speed mode for joystick 
00026 DigitalOut down(D13);                   // Turn down speed mode for joystick 
00027 DigitalOut on(D14);                     // Turn Wheelchair On
00028 DigitalOut off(D15);                    // Turn Wheelchair Off
00029 bool manual = false;                    // Turns chair joystic to automatic and viceverza
00030 
00031 Serial pc(USBTX, USBRX, 57600);         // Serial Monitor
00032 
00033 VL53L1X sensor1(PD_13, PD_12, PC_7);    // Initializes ToF sensors
00034 VL53L1X sensor2(PD_13, PD_12, PA_15);
00035 VL53L1X sensor3(PD_13, PD_12, PB_5);
00036 VL53L1X sensor4(PD_13, PD_12, PF_14);
00037 VL53L1X sensor5(PD_13, PD_12, PE_11);
00038 VL53L1X sensor6(PD_13, PD_12, PE_13);
00039 VL53L1X sensor7(PD_13, PD_12, D6);
00040 VL53L1X sensor8(PD_13, PD_12, PE_12);
00041 VL53L1X sensor9(PD_13, PD_12, PE_10);
00042 VL53L1X sensor10(PD_13, PD_12, PE_15);
00043 VL53L1X sensor11(PD_13, PD_12, D6);
00044 VL53L1X sensor12(PB_11, PB_10, D11);
00045 
00046 VL53L1X* ToF[12] = {&sensor1, &sensor2, &sensor3, &sensor4, &sensor5, &sensor6, 
00047 &sensor7, &sensor8, &sensor9, &sensor10, &sensor11, &sensor12};                 // Puts ToF sensor pointers into an array
00048 VL53L1X** ToFT = ToF;
00049 
00050 Timer imuTimer;   
00051 IMUWheelchair IMU(&pc,&imuTimer); //initialize IMU
00052 
00053 Timer t;                                // Initialize time object t
00054 EventQueue queue;                       // Class to organize threads
00055 Wheelchair smart(xDir,yDir, &pc, &t, &imuTimer, &wheel, &wheelS, ToFT);                    // Initialize wheelchair object
00056 Thread compass;                         // Thread for compass
00057 Thread velocity;                        // Thread for velosity
00058 Thread  ToFSafe;                        // Thread for safety stuff
00059 
00060 
00061 int main(void)
00062 {   
00063 /*    nh.initNode();
00064     nh.advertise(chatter);
00065     nh.advertise(chatter2);
00066     nh.subscribe(sub);*/
00067     //testAccT.start();
00068     
00069     pc.printf("before starting\r\n");
00070     //queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::compass_thread);    // Sets up sampling frequency of the compass_thread
00071     queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::velocity_thread);     // Sets up sampling frequency of the velosity_thread
00072     queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::ToFSafe_thread);      // Sets up sampling frequency of the velosity_thread
00073     //queue.call_every(200, rosCom_thread);                                 // Sets up sampling frequency of the velosity_thread
00074     t.reset();
00075     //compass.start(callback(&queue, &EventQueue::dispatch_forever));       // Starts running the compass thread
00076     velocity.start(callback(&queue, &EventQueue::dispatch_forever));        // Starts running the velosity thread
00077     ToFSafe.start(callback(&queue, &EventQueue::dispatch_forever));         // Starts running the velosity thread
00078     //ros_com.start(callback(&queue, &EventQueue::dispatch_forever));       // Starts running the velosity thread
00079         pc.printf("After starting\r\n");
00080 
00081     //added
00082  //   int emerg_button = e_button;
00083     Watchdog dog;                                                           // Creates Watchdog object
00084     dog.Configure(watchdogLimit);                                           // Configures timeout for Watchdog
00085     pc.printf("Code initiated\n");
00086     int set = 0;
00087     while(1) {
00088         if( pc.readable()) {
00089             set = 1;
00090             char c = pc.getc();                                             // Read the instruction sent
00091             if( c == 'w') {
00092                 smart.forward();                                            // Move foward
00093 
00094             }
00095             else if( c == 'a') {
00096                 smart.left();                                               // Turn left
00097             }
00098             else if( c == 'd') {
00099                 smart.right();                                              // Turn right
00100             }
00101             else if( c == 's') {
00102                 smart.backward();                                           // Turn backwards
00103             }
00104             
00105             else if( c == 't') {                                        
00106                 smart.pid_twistA();
00107             } else if(c == 'v'){
00108                 smart.showOdom();
00109             } else if(c == 'o') {                                           // Turns on chair
00110                 pc.printf("turning on\r\n");
00111                 on = 1;
00112                 wait(1);
00113                 on = 0;
00114             } else if(c == 'f') {                                           // Turns off chair
00115                 pc.printf("turning off\r\n");
00116                 off = 1;
00117                 wait(1);
00118                 off = 0;
00119             
00120             } else if(c == 'k'){                                            // Sends command to go to the kitchen
00121                 smart.pid_twistV();
00122             } else if( c == 'm' || manual) {                                // Turns wheelchair to joystick
00123                 pc.printf("turning on joystick\r\n");
00124                 manual = true;
00125                 t.reset();
00126                 while(manual) {
00127                     smart.move(x,y);                                        // Reads from joystick and moves
00128                     if( pc.readable()) {
00129                         char d = pc.getc();
00130                         if( d == 'm') {                                     // Turns wheelchair from joystick into auto
00131                             pc.printf("turning off joystick\r\n");
00132                             manual = false;
00133                         }
00134                     }
00135                 }   
00136             }
00137              else {
00138                     pc.printf("none \r\n");
00139                     smart.stop();                                           // If nothing else is happening stop the chair
00140             }
00141         }
00142         else {
00143             
00144             smart.stop();                                                   // If nothing else is happening stop the chair
00145         }
00146         
00147         wait(process);
00148         
00149         t.stop();
00150         //pc.printf("Time elapsed: %f seconds and iteration = %d\n", t.read(), iteration);
00151         dog.Service();                                                      // Service the Watchdog so it does not cause a system reset - "Kicking"/"Feeding" the dog
00152 
00153     }
00154 }
00155