![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Updated with Statistics Library
Dependencies: wheelchaircontrol5
Revision 12:43008ddd4a2f, committed 2019-06-28
- Comitter:
- t1jain
- Date:
- Fri Jun 28 23:54:01 2019 +0000
- Parent:
- 11:73b4380d82bf
- Commit message:
- Updated with Statistics Library
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
wheelchaircontrol.lib | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Fri Jun 28 21:18:51 2019 +0000 +++ b/main.cpp Fri Jun 28 23:54:01 2019 +0000 @@ -1,148 +1,149 @@ +#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 +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 +double watchdogLimit = 1.00; // Set timeout limit for watchdog timer in milliseconds -QEI wheelS (D7, D8, NC, 1200); //Initializes Left encoder -DigitalIn pt1(D7, PullUp); //Pull up resistors to read analog signals into digital signals +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); +//Added +DigitalIn e_button(D6, PullDown); int max_velocity; //Timer testAccT; -AnalogIn x(A0); //Initializes analog axis for the joystick +AnalogIn x(A0); // Initializes analog axis for the joystick AnalogIn y(A1); -double watchdogLimit = 0.1; // Set timeout limit for watchdog timer in seconds -int buttonCheck = 0; -int iteration = 1; +DigitalOut up(D12); // Turn up speed mode for joystick +DigitalOut down(D13); // Turn down speed mode for joystick +PwmOut on(PE_6); // Turn Wheelchair On +PwmOut off(PE_5); // Turn Wheelchair Off -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 +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(PD_13, PD_12, PC_7); //initializes ToF sensors -VL53L1X sensor2(PD_13, PD_12, PA_15); +VL53L1X sensor1(PD_13, PD_12, PA_15); // Initializes ToF sensors +VL53L1X sensor2(PD_13, PD_12, PC_7); VL53L1X sensor3(PD_13, PD_12, PB_5); -VL53L1X sensor4(PD_13, PD_12, PF_14); -VL53L1X sensor5(PD_13, PD_12, PE_11); +VL53L1X sensor4(PD_13, PD_12, PE_11); +VL53L1X sensor5(PD_13, PD_12, PF_14); VL53L1X sensor6(PD_13, PD_12, PE_13); -VL53L1X sensor7(PD_13, PD_12, D6); +VL53L1X sensor7(PD_13, PD_12, D1); VL53L1X sensor8(PD_13, PD_12, PE_12); VL53L1X sensor9(PD_13, PD_12, PE_10); VL53L1X sensor10(PD_13, PD_12, PE_15); -VL53L1X sensor11(PD_13, PD_12, D6); +VL53L1X sensor11(PD_13, PD_12, D1); 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* 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 +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 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 int main(void) -{ -/* nh.initNode(); - nh.advertise(chatter); - nh.advertise(chatter2); - nh.subscribe(sub);*/ +{ + + + Watchdog dog; // Creates Watchdog object + dog.Configure(watchdogLimit); // Configures timeout for Watchdog + pc.printf("Code initiated/reset"); + + /* 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::compass_thread); // Sets up sampling frequency of the compass_thread + queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::velocity_thread); // Sets up sampling frequency of the velocity_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 emergencyButton_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"); + 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; - int set = 0; while(1) { if( pc.readable()) { set = 1; - char c = pc.getc(); //Read the instruction sent + char c = pc.getc(); // Read the instruction sent if( c == 'w') { - smart.forward(); //Move foward + smart.forward(); // Move foward - } - else if( c == 'a') { - smart.left(); //Turn left + } 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 == 'd') { - smart.right(); //Turn right - } - else if( c == 's') { - smart.backward(); //Turn rackwards - } - - else if( c == 't') { + + else if( c == 't') { smart.pid_twistA(); - } else if(c == 'v'){ + } else if(c == 'v') { smart.showOdom(); - } else if(c == 'o') { //Turns on chair + } 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 + } 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 + + } else if(c == 'k') { // Sends command to go to the kitchen smart.pid_twistV(); - } else if( c == 'm' || manual) { //Turns wheelchair to joystick + } 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 + 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 + 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 { - 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); - -/* t.stop(); - pc.printf("Time elapsed: %f seconds and iteration = %d\n", t.read(), iteration); dog.Service(); // Service the Watchdog so it does not cause a system reset - "Kicking"/"Feeding" the dog - */ } }
--- a/wheelchaircontrol.lib Fri Jun 28 21:18:51 2019 +0000 +++ b/wheelchaircontrol.lib Fri Jun 28 23:54:01 2019 +0000 @@ -1,1 +1,1 @@ -https://os.mbed.com/users/jvfausto/code/wheelchaircontrol1/#da718b990837 +https://os.mbed.com/teams/Affordable-Smart-Wheelchair/code/wheelchaircontrol5/#e01253eb6c6f