finished commenting on the .cpp file and changed the statistics library name
Dependencies: wheelchaircontrol1
Diff: main.cpp
- Revision:
- 20:bd257ffdef24
- Parent:
- 19:7ead04523f89
- Child:
- 21:b438961f85d5
diff -r 7ead04523f89 -r bd257ffdef24 main.cpp --- a/main.cpp Tue Jul 02 17:53:42 2019 +0000 +++ b/main.cpp Tue Jul 02 18:53:53 2019 +0000 @@ -1,95 +1,118 @@ -/************************** -* Version 1.0 * -**************************/ +/************************************************************************* + * Version 1.0 * + *************************************************************************/ +#include "wheelchair.h" +Serial pc(USBTX, USBRX, 57600); // Serial Monitor -#include "wheelchair.h" - +/************************************************************************** + * Encoder Pins & Variables * + **************************************************************************/ + 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); -DigitalIn e_button(D6, PullDown); // Change to PullUp if testing without Emergency Button Connected - 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; -AnalogIn x(A0); // Initializes analog axis for the joystick -AnalogIn y(A1); - +/************************************************************************** + * Variables and Pins for Watchdog and Emergency Button * + **************************************************************************/ + +DigitalIn e_button(D6, PullDown); // Change to PullUp if testing without Emergency Button Connected +PwmOut on(PE_6); // Turn Wheelchair On +PwmOut off(PE_5); // Turn Wheelchair Off //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; +/************************************************************************** + * Joystick Pins & Variables * + **************************************************************************/ + +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 - -PwmOut on(PE_6); // Turn Wheelchair On -PwmOut off(PE_5); // Turn Wheelchair Off - bool manual = false; // Turns chair joystic to automatic and viceverza -Serial pc(USBTX, USBRX, 57600); // Serial Monitor +/************************************************************************** + * ToF Sensor Pin Assignments * + **************************************************************************/ -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); // Block 1 +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); // Block 2 +VL53L1X sensor5(PD_13, PD_12, PF_14); VL53L1X sensor6(PD_13, PD_12, PE_13); -VL53L1X sensor7(PD_13, PD_12, D6); -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 sensor12(PB_11, PB_10, D11); + +VL53L1X sensor7(PD_13, PD_12, PG_15); // Block 3 +VL53L1X sensor8(PD_13, PD_12, PG_14); +VL53L1X sensor9(PD_13, PD_12, PG_9); + +VL53L1X sensor10(PD_13, PD_12, PE_10); // Block 4 +VL53L1X sensor11(PD_13, PD_12, PE_12); +VL53L1X sensor12(PD_13, PD_12, PE_14); 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 -Thread emergencyButton; // Thread to check button state and reset device +/************************************************************************** + * Thread Definitions * + **************************************************************************/ +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 velocity +Thread ToFSafe; // Thread for safety stuff +Thread emergencyButton; // Thread to check button state and reset device +/************************************************************************** + * MAIN CODE * + **************************************************************************/ int main(void) -{ -/* nh.initNode(); +{ + +/* nh.initNode(); nh.advertise(chatter); nh.advertise(chatter2); - nh.subscribe(sub);*/ + nh.subscribe(sub); */ + + pc.printf("Before Starting\r\n"); - 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 velocity thread - queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::ToFSafe_thread); // Sets up sampling frequency of the ToF safety thread - //queue.call_every(200, rosCom_thread); // Sets up sampling frequency of the ROS com thread - queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::emergencyButton_thread); // Sets up sampling frequency of the emergency button 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::ToFSafe_thread); // Sets up sampling frequency of the ToF safety thread + //queue.call_every(200, rosCom_thread); // Sets up sampling frequency of the ROS com thread + queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::emergencyButton_thread); // Sets up sampling frequency of the emergency button 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 velocity thread - ToFSafe.start(callback(&queue, &EventQueue::dispatch_forever)); // Starts running the ROS com thread - //ros_com.start(callback(&queue, &EventQueue::dispatch_forever)); // Starts running the ROS com thread - emergencyButton.start(callback(&queue, &EventQueue::dispatch_forever)); // Starts running the emergency button thread + //compass.start(callback(&queue, &EventQueue::dispatch_forever)); // Starts running the compass thread + velocity.start(callback(&queue, &EventQueue::dispatch_forever)); // Starts running the velocity thread + ToFSafe.start(callback(&queue, &EventQueue::dispatch_forever)); // Starts running the ROS com thread + //ros_com.start(callback(&queue, &EventQueue::dispatch_forever)); // Starts running the ROS com thread + emergencyButton.start(callback(&queue, &EventQueue::dispatch_forever)); // Starts running the emergency button thread - pc.printf("After starting\r\n"); + pc.printf("After Starting\r\n"); //added - // int emerg_button = e_button; + //int emerg_button = e_button; Watchdog dog; // Creates Watchdog object dog.Configure(watchdogLimit); // Configures timeout for Watchdog pc.printf("Code initiated\n"); int set = 0; + while(1) { if( pc.readable()) { set = 1; @@ -153,7 +176,7 @@ wait(process); t.stop(); - //pc.printf("Time elapsed: %f seconds and iteration = %d\n", t.read(), iteration); + //pc.printf("Time elapsed: %f seconds, Iteration = %d\n", t.read(), iteration); dog.Service(); // Service the Watchdog so it does not cause a system reset - "Kicking"/"Feeding" the dog }