This is for our FYDP project. 2 MPU6050s are used
bt_shell/shell/remotecontrol_fnt.h
- Committer:
- majik
- Date:
- 2015-03-22
- Revision:
- 4:05484073a641
- Parent:
- 0:21019d94ad33
File content as of revision 4:05484073a641:
void remotecontrol_fnt(int argc, char **argv) { int speed = 40; bt.lock(); bt.printf("Remote Control Mode\r\n"); bt.printf("Controls:\r\n"); bt.printf(" wasd & qezc to move\r\n"); bt.printf(" r/f for speed +/-\r\n"); bt.printf(" enter key to exit\r\n"); bt.unlock(); if(argc == 2){ speed = tinysh_atoxi(argv[1]); if( speed <= 0 || speed > 100) speed = 100; } int Lspeed = 0; int Rspeed = 0; char c = 0; while(c != '\r' && c != '\n') { bt.lock(); if(bt.readable()) { c = bt.getc(); bt.unlock(); switch ( c ) { case 'w': Lspeed = speed; Rspeed = speed; break; case 'a': Lspeed = -speed; Rspeed = speed; break; case 's': Lspeed = -speed; Rspeed = -speed; break; case 'x': Lspeed = -speed; Rspeed = -speed; break; case 'd': Lspeed = speed; Rspeed = -speed; break; case 'q': Lspeed = 0; Rspeed = speed; break; case 'e': Lspeed = speed; Rspeed = 0; break; case 'c': Lspeed = -speed; Rspeed = 0; break; case 'z': Lspeed = 0; Rspeed = -speed; break; case 'r': speed += 10; if(speed > 100) speed = 100; bt.lock(); bt.printf("speed = %d\r\n", speed); bt.unlock(); Lspeed = 0; Rspeed = 0; break; case 'f': speed -= 10; if(speed < 0) speed = 0; bt.lock(); bt.printf("speed = %d\r\n", speed); bt.unlock(); Lspeed = 0; Rspeed = 0; break; default: Lspeed = 0; Rspeed = 0; break; } moveMotors(Lspeed,Rspeed,100); }else bt.unlock(); Thread::wait(25); } }