Fully integrated ToF/IMU codes
Dependencies: QEI2 PID Watchdog VL53L1X_Filter ros_lib_kinetic
Diff: main.cpp
- Revision:
- 0:7e6b349182bc
- Child:
- 3:ef063fd4234e
- Child:
- 5:90bf5f0d86e9
diff -r 000000000000 -r 7e6b349182bc main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu Aug 09 16:37:08 2018 +0000 @@ -0,0 +1,102 @@ +#include "wheelchair.h" + +AnalogIn x(A0); +AnalogIn y(A1); + +DigitalOut off(D0); +DigitalOut on(D1); +DigitalOut up(D2); +DigitalOut down(D3); + +bool manual = false; + +Serial pc(USBTX, USBRX, 57600); +Timer t; +EventQueue queue; + +//MPU9250 imu(D14, D15); +Wheelchair smart(xDir,yDir, &pc, &t); +Thread thread; + +int main(void) +{ + queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::compass_thread); + t.reset(); + thread.start(callback(&queue, &EventQueue::dispatch_forever)); + while(1) { + if( pc.readable()) { + char c = pc.getc(); + + if( c == 'w') { + pc.printf("up \n"); + smart.forward(); + } + + else if( c == 'a') { + //pc.printf("left \n"); + smart.left(); + } + + else if( c == 'd') { + //pc.printf("right \n"); + smart.right(); + } + + else if( c == 's') { + pc.printf("down \n"); + smart.backward(); + } + + else if( c == 'r') { + smart.turn_right(90); + } + + else if( c == 'l') { + smart.turn_left(90); + } + + else if( c == 't') { + char buffer[256]; + pc.printf ("Enter a long number: "); + fgets (buffer, 256, stdin); + int angle = atoi (buffer); + + if(angle == 0) { + pc.printf("invalid input try again\n"); + } else { + smart.pid_turn(angle); + } + + } + + else if( c == 'm' || manual) { + pc.printf("turning on joystick\n"); + manual = true; + t.reset(); + while(manual) { + smart.move(x,y); + if( pc.readable()) { + char d = pc.getc(); + if( d == 'm') { + pc.printf("turning off joystick\n"); + manual = false; + } + } + } + } + + else { + pc.printf("none \n"); + smart.stop(); + } + } + + else { + // pc.printf("Nothing pressed \n"); + smart.stop(); + } + wait(process); + } + +} +