Mark Vandermeulen / FYDP_Final

Code for our FYDP -only one IMU works right now -RTOS is working

Dependencies:   mbed

bt_shell/shell/remotecontrol_fnt.h@0:964eb6a2ef00, 2015-03-18 (annotated)

Committer:
majik
Date:
Wed Mar 18 22:23:48 2015 +0000
Revision:
0:964eb6a2ef00
This is our FYDP code, but only one IMU works with the RTOS.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
majik 0:964eb6a2ef00 1 void remotecontrol_fnt(int argc, char **argv)
majik 0:964eb6a2ef00 2 {
majik 0:964eb6a2ef00 3 int speed = 40;
majik 0:964eb6a2ef00 4
majik 0:964eb6a2ef00 5 bt.lock();
majik 0:964eb6a2ef00 6 bt.printf("Remote Control Mode\r\n");
majik 0:964eb6a2ef00 7 bt.printf("Controls:\r\n");
majik 0:964eb6a2ef00 8 bt.printf(" wasd & qezc to move\r\n");
majik 0:964eb6a2ef00 9 bt.printf(" r/f for speed +/-\r\n");
majik 0:964eb6a2ef00 10 bt.printf(" enter key to exit\r\n");
majik 0:964eb6a2ef00 11 bt.unlock();
majik 0:964eb6a2ef00 12
majik 0:964eb6a2ef00 13 if(argc == 2){
majik 0:964eb6a2ef00 14 speed = tinysh_atoxi(argv[1]);
majik 0:964eb6a2ef00 15 if( speed <= 0 || speed > 100)
majik 0:964eb6a2ef00 16 speed = 100;
majik 0:964eb6a2ef00 17 }
majik 0:964eb6a2ef00 18
majik 0:964eb6a2ef00 19 int Lspeed = 0;
majik 0:964eb6a2ef00 20 int Rspeed = 0;
majik 0:964eb6a2ef00 21
majik 0:964eb6a2ef00 22 char c = 0;
majik 0:964eb6a2ef00 23
majik 0:964eb6a2ef00 24 while(c != '\r' && c != '\n')
majik 0:964eb6a2ef00 25 {
majik 0:964eb6a2ef00 26 bt.lock();
majik 0:964eb6a2ef00 27
majik 0:964eb6a2ef00 28 if(bt.readable())
majik 0:964eb6a2ef00 29 {
majik 0:964eb6a2ef00 30 c = bt.getc();
majik 0:964eb6a2ef00 31 bt.unlock();
majik 0:964eb6a2ef00 32
majik 0:964eb6a2ef00 33 switch ( c ) {
majik 0:964eb6a2ef00 34 case 'w':
majik 0:964eb6a2ef00 35 Lspeed = speed;
majik 0:964eb6a2ef00 36 Rspeed = speed;
majik 0:964eb6a2ef00 37 break;
majik 0:964eb6a2ef00 38 case 'a':
majik 0:964eb6a2ef00 39 Lspeed = -speed;
majik 0:964eb6a2ef00 40 Rspeed = speed;
majik 0:964eb6a2ef00 41 break;
majik 0:964eb6a2ef00 42 case 's':
majik 0:964eb6a2ef00 43 Lspeed = -speed;
majik 0:964eb6a2ef00 44 Rspeed = -speed;
majik 0:964eb6a2ef00 45 break;
majik 0:964eb6a2ef00 46 case 'x':
majik 0:964eb6a2ef00 47 Lspeed = -speed;
majik 0:964eb6a2ef00 48 Rspeed = -speed;
majik 0:964eb6a2ef00 49 break;
majik 0:964eb6a2ef00 50 case 'd':
majik 0:964eb6a2ef00 51 Lspeed = speed;
majik 0:964eb6a2ef00 52 Rspeed = -speed;
majik 0:964eb6a2ef00 53 break;
majik 0:964eb6a2ef00 54 case 'q':
majik 0:964eb6a2ef00 55 Lspeed = 0;
majik 0:964eb6a2ef00 56 Rspeed = speed;
majik 0:964eb6a2ef00 57 break;
majik 0:964eb6a2ef00 58 case 'e':
majik 0:964eb6a2ef00 59 Lspeed = speed;
majik 0:964eb6a2ef00 60 Rspeed = 0;
majik 0:964eb6a2ef00 61 break;
majik 0:964eb6a2ef00 62 case 'c':
majik 0:964eb6a2ef00 63 Lspeed = -speed;
majik 0:964eb6a2ef00 64 Rspeed = 0;
majik 0:964eb6a2ef00 65 break;
majik 0:964eb6a2ef00 66 case 'z':
majik 0:964eb6a2ef00 67 Lspeed = 0;
majik 0:964eb6a2ef00 68 Rspeed = -speed;
majik 0:964eb6a2ef00 69 break;
majik 0:964eb6a2ef00 70 case 'r':
majik 0:964eb6a2ef00 71 speed += 10;
majik 0:964eb6a2ef00 72 if(speed > 100)
majik 0:964eb6a2ef00 73 speed = 100;
majik 0:964eb6a2ef00 74 bt.lock();
majik 0:964eb6a2ef00 75 bt.printf("speed = %d\r\n", speed);
majik 0:964eb6a2ef00 76 bt.unlock();
majik 0:964eb6a2ef00 77 Lspeed = 0;
majik 0:964eb6a2ef00 78 Rspeed = 0;
majik 0:964eb6a2ef00 79 break;
majik 0:964eb6a2ef00 80 case 'f':
majik 0:964eb6a2ef00 81 speed -= 10;
majik 0:964eb6a2ef00 82 if(speed < 0)
majik 0:964eb6a2ef00 83 speed = 0;
majik 0:964eb6a2ef00 84 bt.lock();
majik 0:964eb6a2ef00 85 bt.printf("speed = %d\r\n", speed);
majik 0:964eb6a2ef00 86 bt.unlock();
majik 0:964eb6a2ef00 87 Lspeed = 0;
majik 0:964eb6a2ef00 88 Rspeed = 0;
majik 0:964eb6a2ef00 89 break;
majik 0:964eb6a2ef00 90 default:
majik 0:964eb6a2ef00 91 Lspeed = 0;
majik 0:964eb6a2ef00 92 Rspeed = 0;
majik 0:964eb6a2ef00 93 break;
majik 0:964eb6a2ef00 94 }
majik 0:964eb6a2ef00 95
majik 0:964eb6a2ef00 96 moveMotors(Lspeed,Rspeed,100);
majik 0:964eb6a2ef00 97
majik 0:964eb6a2ef00 98 }else
majik 0:964eb6a2ef00 99 bt.unlock();
majik 0:964eb6a2ef00 100
majik 0:964eb6a2ef00 101 Thread::wait(25);
majik 0:964eb6a2ef00 102 }
majik 0:964eb6a2ef00 103 }