Dave Lu / FYDP_Final2

Only imu output

Dependencies:   Servo mbed

Fork of FYDP_Final2 by Mark Vandermeulen

bt_shell/shell/motor_fnt.h@3:47461d37adfb, 2015-03-22 (annotated)

Committer:
majik
Date:
Sun Mar 22 00:43:42 2015 +0000
Revision:
3:47461d37adfb
Parent:
0:21019d94ad33
Child:
5:d2e955a94940
1 IMU, Servo added; -PTD0 is used by something mysterious, so I had to move the servo control line to PTA12

Who changed what in which revision?

UserRevisionLine numberNew 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 3:47461d37adfb 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 }