Mark Vandermeulen / FYDP_Final

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

Dependencies:   mbed

bt_shell/shell/motor_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 move_fnt(int argc, char **argv)
majik 0:964eb6a2ef00 2 {
majik 0:964eb6a2ef00 3 if(echo)
majik 0:964eb6a2ef00 4 bt.printf("move command called\r\n");
majik 0:964eb6a2ef00 5
majik 0:964eb6a2ef00 6 if(argc == 1){
majik 0:964eb6a2ef00 7 if(echo){
majik 0:964eb6a2ef00 8 bt.lock();
majik 0:964eb6a2ef00 9 bt.printf("Usage: move [L_speed] [R_speed] [duration_ms]\r\n");
majik 0:964eb6a2ef00 10 bt.unlock();
majik 0:964eb6a2ef00 11 }
majik 0:964eb6a2ef00 12 }else if(argc == 2){
majik 0:964eb6a2ef00 13 if(echo){
majik 0:964eb6a2ef00 14 bt.lock();
majik 0:964eb6a2ef00 15 bt.printf("\r\nInvalid Parameters\r\n");
majik 0:964eb6a2ef00 16 bt.unlock();
majik 0:964eb6a2ef00 17 }
majik 0:964eb6a2ef00 18 }else{
majik 0:964eb6a2ef00 19
majik 0:964eb6a2ef00 20 int param[3];
majik 0:964eb6a2ef00 21
majik 0:964eb6a2ef00 22 for(int i = 0; i < 2; i++)
majik 0:964eb6a2ef00 23 {
majik 0:964eb6a2ef00 24 param[i] = tinysh_atoxi(argv[i+1]);
majik 0:964eb6a2ef00 25
majik 0:964eb6a2ef00 26 if(param[i] > 100)
majik 0:964eb6a2ef00 27 param[i] = 100;
majik 0:964eb6a2ef00 28 else if (param[i] < -100)
majik 0:964eb6a2ef00 29 param[i] = -100;
majik 0:964eb6a2ef00 30 }
majik 0:964eb6a2ef00 31
majik 0:964eb6a2ef00 32 if(echo){
majik 0:964eb6a2ef00 33 bt.lock();
majik 0:964eb6a2ef00 34 bt.printf("Setting motor speeds Left = %d Right = %d\r\n" ,param[0],param[1]);
majik 0:964eb6a2ef00 35 bt.unlock();
majik 0:964eb6a2ef00 36 }
majik 0:964eb6a2ef00 37
majik 0:964eb6a2ef00 38 if(argc == 4)
majik 0:964eb6a2ef00 39 {
majik 0:964eb6a2ef00 40 param[2] = tinysh_atoxi(argv[3]);
majik 0:964eb6a2ef00 41 if( param[2] > 0)
majik 0:964eb6a2ef00 42 {
majik 0:964eb6a2ef00 43 if(echo){
majik 0:964eb6a2ef00 44 bt.lock();
majik 0:964eb6a2ef00 45 bt.printf("moving duration: %sms\r\n", argv[3]);
majik 0:964eb6a2ef00 46 bt.unlock();
majik 0:964eb6a2ef00 47 }
majik 0:964eb6a2ef00 48
majik 0:964eb6a2ef00 49 moveMotors(param[0],param[1],param[2]);
majik 0:964eb6a2ef00 50 }
majik 0:964eb6a2ef00 51 else
majik 0:964eb6a2ef00 52 {
majik 0:964eb6a2ef00 53 if(echo){
majik 0:964eb6a2ef00 54 bt.lock();
majik 0:964eb6a2ef00 55 bt.printf("invalid duration: %sms\r\n", argv[3]);
majik 0:964eb6a2ef00 56 bt.unlock();
majik 0:964eb6a2ef00 57 }
majik 0:964eb6a2ef00 58 }
majik 0:964eb6a2ef00 59 }
majik 0:964eb6a2ef00 60 else
majik 0:964eb6a2ef00 61 {
majik 0:964eb6a2ef00 62 *motors.Left = param[0];
majik 0:964eb6a2ef00 63 *motors.Right = param[1];
majik 0:964eb6a2ef00 64 }
majik 0:964eb6a2ef00 65 }
majik 0:964eb6a2ef00 66 }
majik 0:964eb6a2ef00 67
majik 0:964eb6a2ef00 68 //////////////////////////////////////////////////////////////////////////
majik 0:964eb6a2ef00 69 void stop_motors_fnt(int argc, char **argv)
majik 0:964eb6a2ef00 70 {
majik 0:964eb6a2ef00 71 if(echo){
majik 0:964eb6a2ef00 72 bt.lock();
majik 0:964eb6a2ef00 73 bt.printf("motors stopped\r\n");
majik 0:964eb6a2ef00 74 bt.unlock();
majik 0:964eb6a2ef00 75 }
majik 0:964eb6a2ef00 76 *motors.Left = 0;
majik 0:964eb6a2ef00 77 *motors.Right = 0;
majik 0:964eb6a2ef00 78 }
majik 0:964eb6a2ef00 79
majik 0:964eb6a2ef00 80 ///////////////////////////////////////////////////////////////////////
majik 0:964eb6a2ef00 81 void flip_motors_fnt(int argc, char **argv)
majik 0:964eb6a2ef00 82 {
majik 0:964eb6a2ef00 83 if(echo){
majik 0:964eb6a2ef00 84 bt.lock();
majik 0:964eb6a2ef00 85 bt.printf("L/R motors swapped\r\n");
majik 0:964eb6a2ef00 86 bt.unlock();
majik 0:964eb6a2ef00 87 }
majik 0:964eb6a2ef00 88 motors.flip();
majik 0:964eb6a2ef00 89 }
majik 0:964eb6a2ef00 90
majik 0:964eb6a2ef00 91 void motor_scale_fnt(int argc, char **argv)
majik 0:964eb6a2ef00 92 {
majik 0:964eb6a2ef00 93 if(echo){
majik 0:964eb6a2ef00 94 bt.lock();
majik 0:964eb6a2ef00 95 bt.printf("motor scale command called\r\n");
majik 0:964eb6a2ef00 96 bt.unlock();
majik 0:964eb6a2ef00 97 }
majik 0:964eb6a2ef00 98
majik 0:964eb6a2ef00 99 if(argc == 2){
majik 0:964eb6a2ef00 100 float val = atof(argv[1]);
majik 0:964eb6a2ef00 101
majik 0:964eb6a2ef00 102 motors.Left->scale = val;
majik 0:964eb6a2ef00 103 motors.Right->scale = val;
majik 0:964eb6a2ef00 104
majik 0:964eb6a2ef00 105 if(echo){
majik 0:964eb6a2ef00 106 bt.lock();
majik 0:964eb6a2ef00 107 bt.printf("Scale set to %s\r\n", argv[1]);
majik 0:964eb6a2ef00 108 bt.unlock();
majik 0:964eb6a2ef00 109 }
majik 0:964eb6a2ef00 110 }else{
majik 0:964eb6a2ef00 111 if(echo){
majik 0:964eb6a2ef00 112 bt.lock();
majik 0:964eb6a2ef00 113 bt.printf("Usage: scale [value]\r\n");
majik 0:964eb6a2ef00 114 bt.unlock();
majik 0:964eb6a2ef00 115 }
majik 0:964eb6a2ef00 116 }
majik 0:964eb6a2ef00 117 }