Integration of wheelchair control code, emergency button code and watchdog code
Dependencies: wheelchaircontrol3 Watchdog ros_lib_kinetic
Diff: main.cpp
- Revision:
- 11:e5823b1f5268
- Parent:
- 10:b5f6337c3a20
- Child:
- 12:14cac8544c97
diff -r b5f6337c3a20 -r e5823b1f5268 main.cpp --- a/main.cpp Wed Jun 26 04:21:41 2019 +0000 +++ b/main.cpp Wed Jun 26 18:24:00 2019 +0000 @@ -1,141 +1,185 @@ -#include "wheelchair.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); - - -/*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); - -int max_velocity; -//Timer testAccT; - -AnalogIn x(A0); //Initializes analog axis for the joystick -AnalogIn y(A1); + + #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); -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 - -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; - -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 + 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); -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(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 - //ros_com.start(callback(&queue, &EventQueue::dispatch_forever)); //Starts running the velosity thread - pc.printf("After starting\r\n"); + /*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); + + int max_velocity; + //Timer testAccT; + + 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 - //added - // int emerg_button = e_button; - - 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 + 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* ToF[12] = {&sensor1, &sensor2, &sensor3, &sensor4, &sensor5, &sensor6, + &sensor7, &sensor8, &sensor9, &sensor10, &sensor11, &sensor12}; //puts ToF sensor pointers into an array + VL53L1X** ToFT = ToF; - } - 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; + 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; +// while(!button) { +// +// //Send something to stop wheelchair +// signal = 1; +// +// pc.printf("Hello there!\n"); +// pc.printf("I'm dead\n\n\n"); +// +// //Reset Board +// NVIC_SystemReset(); +// +// } +// +// } +// } + + 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 + + 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 + + } + 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 { + } + } + 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 + else { + + smart.stop(); //If nothing else is happening stop the chair + } + + wait(process); + dog.Service(); } - - wait(process); - } -} - + } \ No newline at end of file