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             else
00050             {
00051                 if(echo){
00052                     bt.lock();                
00053                     bt.printf("invalid duration: %sms\r\n", argv[3]);    
00054                     bt.unlock();
00055                 }                
00056             }
00057         }
00058         else
00059         {   
00060             *motors.Left = param[0];
00061             *motors.Right = param[1];
00062         }
00063     }
00064 }
00065 
00066 //////////////////////////////////////////////////////////////////////////
00067 void stop_motors_fnt(int argc, char **argv)
00068 {
00069     if(echo){
00070         bt.lock();
00071         bt.printf("motors stopped\r\n");
00072         bt.unlock();
00073     }
00074     *motors.Left = 0;
00075     *motors.Right = 0;
00076 }
00077 
00078 ///////////////////////////////////////////////////////////////////////
00079 void flip_motors_fnt(int argc, char **argv)
00080 {
00081     if(echo){
00082         bt.lock();
00083         bt.printf("L/R motors swapped\r\n");
00084         bt.unlock();
00085     }
00086     motors.flip();    
00087 }
00088 
00089 void motor_scale_fnt(int argc, char **argv)
00090 {
00091     if(echo){
00092         bt.lock();
00093         bt.printf("motor scale command called\r\n");
00094         bt.unlock();
00095     }
00096     
00097     if(argc == 2){
00098         float val = atof(argv[1]);
00099         
00100         motors.Left->scale = val;
00101         motors.Right->scale = val;                 
00102         
00103         if(echo){
00104             bt.lock();
00105             bt.printf("Scale set to %s\r\n", argv[1]); 
00106             bt.unlock();
00107         }
00108     }else{
00109         if(echo){
00110             bt.lock();
00111             bt.printf("Usage: scale [value]\r\n");
00112             bt.unlock();
00113         }
00114     }
00115 }