![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Fully integrated ToF/IMU codes
Dependencies: QEI2 PID Watchdog VL53L1X_Filter ros_lib_kinetic
Diff: main.cpp
- Revision:
- 9:ca11e4db63a7
- Parent:
- 8:db780b392bae
- Child:
- 10:b4d68db3ddbd
--- a/main.cpp Fri Aug 31 20:01:55 2018 +0000 +++ b/main.cpp Fri Apr 19 23:08:27 2019 +0000 @@ -1,160 +1,139 @@ #include "wheelchair.h" -QEI wheel(D0, D1, NC, 1200); -DigitalIn pt3(D1, PullUp); -DigitalIn pt4(D0, PullUp); +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 wheel2 (D3, D6, NC, 1200); -DigitalIn pt1(D3, PullUp); -DigitalIn pt2(D6, 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); -AnalogIn x(A0); +int max_velocity; +//Timer testAccT; + +AnalogIn x(A0); //Initializes analog axis for the joystick AnalogIn y(A1); -DigitalOut up(D2); -DigitalOut down(D3); -DigitalOut on(D12); -DigitalOut off(D11); -bool manual = false; +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); +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); -Timer t; -EventQueue queue; +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; -//MPU9250 imu(D14, D15); -Wheelchair smart(xDir,yDir, &pc, &t, &wheel); -Thread thread; +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 int main(void) -{ - queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::compass_thread); +{ +/* 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(); - thread.start(callback(&queue, &EventQueue::dispatch_forever)); + 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 + // int emerg_button = e_button; + + int set = 0; while(1) { if( pc.readable()) { - char c = pc.getc(); + set = 1; + char c = pc.getc(); //Read the instruction sent if( c == 'w') { - //pc.printf("up \r\n"); - smart.forward(); - pc.printf("%f\r\n", wheel.getDistance(53.975)); + smart.forward(); //Move foward + } else if( c == 'a') { - //pc.printf("left \r\n"); - smart.left(); - } - - else if( c == 'd') { - //pc.printf("right \r\n"); - smart.right(); + smart.left(); //Turn left } - - else if( c == 's') { - //pc.printf("down \r\n"); - smart.backward(); - // wheel.reset(); - } - - else if( c == 'r') { - smart.turn_right(90); - } - - else if( c == 'l') { - smart.turn_left(90); + else if( c == 'd') { + smart.right(); //Turn right } - - else if( c == 't') { - - char buffer[256]; - pc.printf ("Enter a long number: "); - char d = pc.getc(); - int angle = 0; - //fgets (buffer, 256, stdin); - if(d == 'r') - { - angle = 90;//atoi (buffer); - } - if(d == 'l') - { - angle = -90;//atoi (buffer); - } - if(angle == 0) { - pc.printf("invalid input try again\r\n"); - } else { - smart.pid_turn(angle); - } - - } else if(c == 'o') { + 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') { + } else if(c == 'f') { //Turns off chair pc.printf("turning off\r\n"); off = 1; wait(1); off = 0; - } else if(c == 'k'){ - pc.printf("kitchen\r\n"); - smart.kitchen(); - } else if(c == 'e'){ - pc.printf("desk\r\n"); - smart.desk(); - } else if(c == 'x'){ - pc.printf("desk to kitchen\r\n"); - smart.desk_to_kitchen(); - } else if(c == 'u') { - pc.printf ("Enter a long number: "); - char d = pc.getc(); - double displacement = 0; - if(d == '1') - displacement = 5461; - if(d == '2') - displacement = 3658; - if(d == '3') - displacement = 305; - if(d == '4') - displacement = 1000; - if(displacement > 0) - { - smart.pid_forward(displacement); - } - else if(displacement < 0) - { - smart.pid_reverse(displacement); - } - } else if( c == 'm' || manual) { + + } 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); + smart.move(x,y); //Reads from joystick and moves if( pc.readable()) { char d = pc.getc(); - if( d == 'm') { + 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(); + 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 { - // pc.printf("Nothing pressed \r\n"); - smart.stop(); - } wait(process); } - } - - - -