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