Collated code with down ToF, side ToF, Emergency Button, Watchdog
Dependencies: wheelchaircontrol
Diff: main.cpp
- Revision:
- 17:770a161346ed
- Parent:
- 14:67133c127740
- Child:
- 18:94ae9ca5a709
diff -r 71831ac12177 -r 770a161346ed main.cpp --- a/main.cpp Mon Jul 01 23:36:04 2019 +0000 +++ b/main.cpp Tue Jul 02 16:41:38 2019 +0000 @@ -1,35 +1,36 @@ #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 +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 +//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 +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 x(A0); // Initializes analog axis for the joystick AnalogIn y(A1); -double watchdogLimit = 1; //Should be 0.1 // Set timeout limit for watchdog timer in seconds +//Watchdog limit should be 0.1; Set to 1 for Testing Only +double watchdogLimit = 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 -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(PD_13, PD_12, PC_7); //initializes ToF sensors +VL53L1X sensor1(PD_13, PD_12, PC_7); // Initializes ToF sensors VL53L1X sensor2(PD_13, PD_12, PA_15); VL53L1X sensor3(PD_13, PD_12, PB_5); VL53L1X sensor4(PD_13, PD_12, PF_14); @@ -43,15 +44,15 @@ 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 +&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 ToFSafe; //thread for safety stuff +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 ToFSafe; // Thread for safety stuff int main(void) @@ -61,16 +62,17 @@ 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::ToFSafe_thread); //Sets up sampling frequency of the velosity_thread - //queue.call_every(200, rosCom_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 velosity_thread + queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::ToFSafe_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 - ToFSafe.start(callback(&queue, &EventQueue::dispatch_forever)); //Starts running the velosity thread - //ros_com.start(callback(&queue, &EventQueue::dispatch_forever)); //Starts running the velosity thread + //compass.start(callback(&queue, &EventQueue::dispatch_forever)); // Starts running the compass thread + velocity.start(callback(&queue, &EventQueue::dispatch_forever)); // Starts running the velosity thread + ToFSafe.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 @@ -82,47 +84,47 @@ 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 + smart.left(); // Turn left } else if( c == 'd') { - smart.right(); //Turn right + smart.right(); // Turn right } else if( c == 's') { - smart.backward(); //Turn rackwards + smart.backward(); // Turn backwards } else if( c == 't') { smart.pid_twistA(); } 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; } @@ -131,19 +133,19 @@ } else { pc.printf("none \r\n"); - smart.stop(); //If nothing else is happening stop the chair + smart.stop(); // If nothing else is happening stop the chair } } else { - smart.stop(); //If nothing else is happening stop the chair + 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 + dog.Service(); // Service the Watchdog so it does not cause a system reset - "Kicking"/"Feeding" the dog } }