motor_fnt.h

00001 void move_fnt(int argc, char **argv)
00002 {
00003     if(echo)
00004         bt.printf("move command called\r\n");
00005     
00006     if(argc == 1){
00007         if(echo){
00008             bt.lock();
00009             bt.printf("Usage: move [L_speed] [R_speed] [duration_ms]\r\n");
00010             bt.unlock();
00011         }
00012     }else if(argc == 2){
00013         if(echo){
00014             bt.lock();
00015             bt.printf("\r\nInvalid Parameters\r\n");
00016             bt.unlock();
00017         }
00018     }else{
00019         
00020         int param[3];
00021             
00022         for(int i = 0; i < 2; i++)
00023         {
00024             param[i] = tinysh_atoxi(argv[i+1]);
00025     
00026             if(param[i] > 100)
00027                 param[i] = 100;
00028             else if (param[i] < -100)
00029                 param[i] = -100;
00030         }
00031         
00032         if(echo){
00033             bt.lock();
00034             bt.printf("Setting motor speeds Left = %d  Right = %d\r\n" ,param[0],param[1]);
00035             bt.unlock();
00036         }
00037             
00038         if(argc == 4)
00039         {
00040             param[2] = tinysh_atoxi(argv[3]);
00041             if( param[2] > 0)
00042             {
00043                 if(echo){
00044                     bt.lock();
00045                     bt.printf("moving duration: %sms\r\n", argv[3]);
00046                     bt.unlock();
00047                 }
00048 
00049                 moveMotors(param[0],param[1],param[2]);
00050             }
00051             else
00052             {
00053                 if(echo){
00054                     bt.lock();                
00055                     bt.printf("invalid duration: %sms\r\n", argv[3]);    
00056                     bt.unlock();
00057                 }                
00058             }
00059         }
00060         else
00061         {   
00062             *motors.Left = param[0];
00063             *motors.Right = param[1];
00064         }
00065     }
00066 }
00067 
00068 //////////////////////////////////////////////////////////////////////////
00069 void stop_motors_fnt(int argc, char **argv)
00070 {
00071     if(echo){
00072         bt.lock();
00073         bt.printf("motors stopped\r\n");
00074         bt.unlock();
00075     }
00076     *motors.Left = 0;
00077     *motors.Right = 0;
00078 }
00079 
00080 ///////////////////////////////////////////////////////////////////////
00081 void flip_motors_fnt(int argc, char **argv)
00082 {
00083     if(echo){
00084         bt.lock();
00085         bt.printf("L/R motors swapped\r\n");
00086         bt.unlock();
00087     }
00088     motors.flip();    
00089 }
00090 
00091 void motor_scale_fnt(int argc, char **argv)
00092 {
00093     if(echo){
00094         bt.lock();
00095         bt.printf("motor scale command called\r\n");
00096         bt.unlock();
00097     }
00098     
00099     if(argc == 2){
00100         float val = atof(argv[1]);
00101         
00102         motors.Left->scale = val;
00103         motors.Right->scale = val;                 
00104         
00105         if(echo){
00106             bt.lock();
00107             bt.printf("Scale set to %s\r\n", argv[1]); 
00108             bt.unlock();
00109         }
00110     }else{
00111         if(echo){
00112             bt.lock();
00113             bt.printf("Usage: scale [value]\r\n");
00114             bt.unlock();
00115         }
00116     }
00117 }