Integration of wheelchair control code, emergency button code and watchdog code
Dependencies: wheelchaircontrol3 Watchdog ros_lib_kinetic
Diff: main.cpp
- Revision:
- 12:14cac8544c97
- Parent:
- 11:e5823b1f5268
diff -r e5823b1f5268 -r 14cac8544c97 main.cpp --- a/main.cpp Wed Jun 26 18:24:00 2019 +0000 +++ b/main.cpp Wed Jun 26 20:28:04 2019 +0000 @@ -1,67 +1,67 @@ - - #include "mbed.h" - #include <ros.h> - #include <nav_msgs/Odometry.h> //contains both the twist and pose - #include "Watchdog.h" - #include "wheelchair.h" - #include "rtos.h" - - double watchdogLimit = 1.00; // Set timeout limit for watchdog timer - int buttonCheck = 0; - //DigitalIn button(D4, PullDown); - //DigitalOut signal(D5); + +#include "mbed.h" +#include <ros.h> +#include <nav_msgs/Odometry.h> //contains both the twist and pose +#include "Watchdog.h" +#include "wheelchair.h" +#include "rtos.h" - QEI wheel (D10, D9, NC, 1200); // Initializes right encoder - DigitalIn pt3(D10, PullUp); // Pull up resistors to read analog signals into digital signals - DigitalIn pt4(D9, PullUp); +double watchdogLimit = 1.00; // Set timeout limit for watchdog timer +int buttonCheck = 0; +//DigitalIn button(D4, PullDown); +//DigitalOut signal(D5); + +QEI wheel (D10, D9, NC, 1200); // Initializes right encoder +DigitalIn pt3(D10, PullUp); // Pull up resistors to read analog signals into digital signals +DigitalIn pt4(D9, PullUp); - /*added*/ - //DigitalIn e_button(D4); // Emergency button will start at HIGH +/*added*/ +//DigitalIn e_button(D4); // Emergency button will start at HIGH - QEI wheelS (D7, D8, NC, 1200); // Initializes Left encoder - DigitalIn pt1(D7, PullUp); // Pull up resistors to read analog signals into digital signals - DigitalIn pt2(D8, PullUp); +QEI wheelS (D7, D8, NC, 1200); // Initializes Left encoder +DigitalIn pt1(D7, PullUp); // Pull up resistors to read analog signals into digital signals +DigitalIn pt2(D8, PullUp); - int max_velocity; - //Timer testAccT; +int max_velocity; +//Timer testAccT; - AnalogIn x(A0); // Initializes analog axis for the joystick - AnalogIn y(A1); +AnalogIn x(A0); // Initializes analog axis for the joystick +AnalogIn y(A1); - DigitalOut up(D12); // Turn up speed mode for joystick - DigitalOut down(D13); // Turn down speed mode for joystick - DigitalOut on(D14); // Turn Wheelchair On - DigitalOut off(D15); // Turn Wheelchair Off - bool manual = false; // Turns chair joystic to automatic and viceverza +DigitalOut up(D12); // Turn up speed mode for joystick +DigitalOut down(D13); // Turn down speed mode for joystick +DigitalOut on(D14); // Turn Wheelchair On +DigitalOut off(D15); // Turn Wheelchair Off +bool manual = false; // Turns chair joystic to automatic and viceverza - Serial pc(USBTX, USBRX, 57600); // Serial Monitor +Serial pc(USBTX, USBRX, 57600); // Serial Monitor - VL53L1X sensor1(PB_11, PB_10, D0); // initializes ToF sensors - VL53L1X sensor2(PB_11, PB_10, D1); - VL53L1X sensor3(PB_11, PB_10, D2); - VL53L1X sensor4(PB_11, PB_10, D3); - VL53L1X sensor5(PB_11, PB_10, D4); - VL53L1X sensor6(PB_11, PB_10, D5); - VL53L1X sensor7(PB_11, PB_10, PE_14); - VL53L1X sensor8(PB_11, PB_10, PE_12); - VL53L1X sensor9(PB_11, PB_10, PE_10); - VL53L1X sensor10(PB_11, PB_10, PE_15); - VL53L1X sensor11(PB_11, PB_10, D6); - VL53L1X sensor12(PB_11, PB_10, D11); +VL53L1X sensor1(PB_11, PB_10, D0); // initializes ToF sensors +VL53L1X sensor2(PB_11, PB_10, D1); +VL53L1X sensor3(PB_11, PB_10, D2); +VL53L1X sensor4(PB_11, PB_10, D3); +VL53L1X sensor5(PB_11, PB_10, D4); +VL53L1X sensor6(PB_11, PB_10, D5); +VL53L1X sensor7(PB_11, PB_10, PE_14); +VL53L1X sensor8(PB_11, PB_10, PE_12); +VL53L1X sensor9(PB_11, PB_10, PE_10); +VL53L1X sensor10(PB_11, PB_10, PE_15); +VL53L1X sensor11(PB_11, PB_10, D6); +VL53L1X sensor12(PB_11, PB_10, D11); - VL53L1X* ToF[12] = {&sensor1, &sensor2, &sensor3, &sensor4, &sensor5, &sensor6, - &sensor7, &sensor8, &sensor9, &sensor10, &sensor11, &sensor12}; //puts ToF sensor pointers into an array - VL53L1X** ToFT = ToF; +VL53L1X* ToF[12] = {&sensor1, &sensor2, &sensor3, &sensor4, &sensor5, &sensor6, +&sensor7, &sensor8, &sensor9, &sensor10, &sensor11, &sensor12}; //puts ToF sensor pointers into an array +VL53L1X** ToFT = ToF; - Timer t; // Initialize time object t - EventQueue queue; // Class to organize threads - Wheelchair smart(xDir,yDir, &pc, &t, &wheel, &wheelS, ToFT); //Initialize wheelchair object - Thread compass; // Thread for compass - Thread velocity; // Thread for velosity - Thread assistSafe; // Thread for safety stuff - Thread emergencyButton; // Thread to check button state and reset device - +Timer t; // Initialize time object t +EventQueue queue; // Class to organize threads +Wheelchair smart(xDir,yDir, &pc, &t, &wheel, &wheelS, ToFT); //Initialize wheelchair object +Thread compass; // Thread for compass +Thread velocity; // Thread for velosity +Thread assistSafe; // Thread for safety stuff +Thread emergencyButton; // Thread to check button state and reset device + // void emergencyButton () { // while(1) { // signal = 0; @@ -80,106 +80,107 @@ // // } // } + +int main(void) { + + /* nh.initNode(); + nh.advertise(chatter); + nh.advertise(chatter2); + nh.subscribe(sub);*/ +//testAccT.start(); + - int main(void) { - /* nh.initNode(); - nh.advertise(chatter); - nh.advertise(chatter2); - nh.subscribe(sub);*/ - //testAccT.start(); + + pc.printf("Before starting\r\n"); + queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::compass_thread); // Sets up sampling frequency of the compass_thread + queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::velocity_thread); // Sets up sampling frequency of the velosity_thread + queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::assistSafe_thread); // Sets up sampling frequency of the velosity_thread + queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::emergencyButton_thread); // Sets up sampling frequency of the velosity_thread + + //queue.call_every(200, rosCom_thread); //Sets up sampling frequency of the velosity_thread - - - pc.printf("Before starting\r\n"); - queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::compass_thread); //Sets up sampling frequency of the compass_thread - queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::velocity_thread); //Sets up sampling frequency of the velosity_thread - queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::assistSafe_thread); //Sets up sampling frequency of the velosity_thread - queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::emergencyButton_thread); //Sets up sampling frequency of the velosity_thread - - //queue.call_every(200, rosCom_thread); //Sets up sampling frequency of the velosity_thread - - t.reset(); - compass.start(callback(&queue, &EventQueue::dispatch_forever)); //Starts running the compass thread - velocity.start(callback(&queue, &EventQueue::dispatch_forever)); //Starts running the velosity thread - assistSafe.start(callback(&queue, &EventQueue::dispatch_forever)); //Starts running the velosity thread - emergencyButton.start(callback(&queue, &EventQueue::dispatch_forever)); - - //ros_com.start(callback(&queue, &EventQueue::dispatch_forever)); //Starts running the velosity thread - pc.printf("After starting\r\n"); + t.reset(); + compass.start(callback(&queue, &EventQueue::dispatch_forever)); // Starts running the compass thread + velocity.start(callback(&queue, &EventQueue::dispatch_forever)); // Starts running the velosity thread + assistSafe.start(callback(&queue, &EventQueue::dispatch_forever)); // Starts running the velosity thread + emergencyButton.start(callback(&queue, &EventQueue::dispatch_forever)); + + //ros_com.start(callback(&queue, &EventQueue::dispatch_forever)); // Starts running the velosity thread + pc.printf("After starting\r\n"); - //added - // int emerg_button = e_button; - - Watchdog dog; - dog.Configure(watchdogLimit); - pc.printf("Code initiated/reset"); - - int set = 0; - while(1) { - if( pc.readable()) { - set = 1; - char c = pc.getc(); //Read the instruction sent - if( c == 'w') { - smart.forward(); //Move foward + //added + // int emerg_button = e_button; + + Watchdog dog; + dog.Configure(watchdogLimit); + pc.printf("Code initiated/reset"); + + int set = 0; + while(1) { + if( pc.readable()) { + set = 1; + char c = pc.getc(); // Read the instruction sent + if( c == 'w') { + smart.forward(); // Move foward - } - else if( c == 'a') { - smart.left(); //Turn left - } - else if( c == 'd') { - smart.right(); //Turn right - } - else if( c == 's') { - smart.backward(); //Turn rackwards - } - else if( c == 't') { - smart.pid_twistA(); - } - else if(c == 'v'){ - smart.showOdom(); - } - else if(c == 'o') { //Turns on chair - pc.printf("turning on\r\n"); - on = 1; - wait(1); - on = 0; - } - else if(c == 'f') { //Turns off chair - pc.printf("turning off\r\n"); - off = 1; - wait(1); - off = 0; - - } - else if(c == 'k'){ //Sends command to go to the kitchen - smart.pid_twistV(); - } - else if( c == 'm' || manual) { //Turns wheelchair to joystick - pc.printf("turning on joystick\r\n"); - manual = true; - t.reset(); - while(manual) { - smart.move(x,y); //Reads from joystick and moves - if( pc.readable()) { - char d = pc.getc(); - if( d == 'm') { //Turns wheelchair from joystick into auto - pc.printf("turning off joystick\r\n"); - manual = false; - } + } + else if( c == 'a') { + smart.left(); // Turn left + } + else if( c == 'd') { + smart.right(); // Turn right + } + else if( c == 's') { + smart.backward(); // Turn backwards + } + else if( c == 't') { + smart.pid_twistA(); + } + else if(c == 'v'){ + smart.showOdom(); + } + else if(c == 'o') { // Turns on chair + pc.printf("turning on\r\n"); + on = 1; + wait(1); + on = 0; + } + else if(c == 'f') { // Turns off chair + pc.printf("turning off\r\n"); + off = 1; + wait(1); + off = 0; + + } + else if(c == 'k'){ // Sends command to go to the kitchen + smart.pid_twistV(); + } + else if( c == 'm' || manual) { // Turns wheelchair to joystick + pc.printf("turning on joystick\r\n"); + manual = true; + t.reset(); + while(manual) { + smart.move(x,y); // Reads from joystick and moves + if( pc.readable()) { + char d = pc.getc(); + if( d == 'm') { // Turns wheelchair from joystick into auto + pc.printf("turning off joystick\r\n"); + manual = false; } - } - } - else { - pc.printf("none \r\n"); - smart.stop(); //If nothing else is happening stop the chair - } + } + } } else { - - smart.stop(); //If nothing else is happening stop the chair + pc.printf("none \r\n"); + smart.stop(); // If nothing else is happening stop the chair } + } + else { + + smart.stop(); // If nothing else is happening stop the chair + } - wait(process); - dog.Service(); - } - } \ No newline at end of file + wait(process); + dog.Service(); + } +} \ No newline at end of file