control_motors.cpp

00001 #include "control_motors.h"
00002 
00003 bool parse_request(char *request){
00004     
00005     bool okFlag = false;
00006     
00007     char req[1024];
00008     strcpy(req, request + 1);
00009 
00010     char* str = strtok(req, "/");
00011     
00012     if(strcmp(str, "api") == 0){
00013         str = strtok(NULL,"/");
00014 
00015         if(strcmp(str, "motor") == 0){
00016             str = strtok(NULL,"?");
00017 
00018             if(strcmp(str, "right") == 0){
00019 
00020                 str = strtok(NULL,"="); //"speed"
00021                 str = strtok(NULL,""); //            
00022                 
00023                 int speed = atoi(str);
00024 
00025                 // motor right
00026                 move(MOTOR_RIGHT, speed);
00027                 okFlag = true;
00028 
00029             }else if(strcmp(str, "left") == 0){
00030                 
00031                 str = strtok(NULL,"="); //"speed"
00032                 str = strtok(NULL,""); //            
00033                 
00034                 int speed = atoi(str);
00035 
00036                 // motor left
00037                 move(MOTOR_LEFT, speed);
00038                 okFlag = true;
00039 
00040             }
00041             
00042         }else if(strcmp(str, "tail") == 0){
00043             str = strtok(NULL,"/");
00044             if(strcmp(str, "swing") == 0){
00045                 str = strtok(NULL,"/");
00046                 if(strcmp(str, "start") == 0){
00047                     
00048                     // tail start
00049                     start_shaking_tail();
00050                     
00051                     okFlag = true;
00052                     
00053                 }else if(strcmp(str, "end") == 0){
00054 
00055                     // tail end
00056                     stop_shaking_tail();
00057                     
00058                     okFlag = true;
00059                     
00060                 }
00061             }
00062         }
00063     }
00064     
00065     if(!okFlag){
00066         printf("error: request=%s\n", request);
00067         return false;
00068     }
00069     
00070     return true;
00071 }
00072 
00073 
00074 void move(int motor_id, int speed){
00075     if(motor_id == MOTOR_RIGHT){
00076         printf("motor right speed = %d\n", speed);
00077         
00078         // モーター制御の処理
00079             if(speed > 0){
00080             mPwmMotorR = (float)speed/255;
00081             mDoutMotorR1 = 1;
00082             mDoutMotorR2 = 0;
00083         }else if (speed < 0){
00084             mPwmMotorR = -(float)speed/255;
00085             mDoutMotorR1 = 0;
00086             mDoutMotorR2 = 1;
00087         }else{
00088             mPwmMotorR = 0;
00089             mDoutMotorR1 = 0;
00090             mDoutMotorR2 = 0;
00091         }
00092 
00093     }else if(motor_id == MOTOR_LEFT){
00094         printf("motor left speed = %d\n", speed);
00095         
00096         // モーター制御の処理
00097        if(speed > 0){
00098             mPwmMotorL = (float)speed/255;
00099             mDoutMotorL1 = 1;
00100             mDoutMotorL2 = 0;
00101         }else if (speed < 0){
00102             mPwmMotorL = -(float)speed/255;
00103             mDoutMotorL1 = 0;
00104             mDoutMotorL2 = 1;
00105         }else{
00106             mPwmMotorL = 0.0;
00107             mDoutMotorL1 = 0;
00108             mDoutMotorL2 = 0;
00109         }
00110 
00111     }
00112 }
00113 
00114 
00115 void start_shaking_tail(){
00116     printf("start_shaking_tail\n");
00117 
00118     // モーター制御の処理
00119     mPwmMotorTail = 0.8;
00120     mDoutMotorTail1 = 1;
00121     mDoutMotorTail2 = 0;
00122 
00123 }
00124 
00125     
00126 void stop_shaking_tail(){
00127     printf("stop_shaking_tail\n");
00128 
00129     // モーター制御の処理
00130     mPwmMotorTail = 0.0;
00131     mDoutMotorTail1 = 0;
00132     mDoutMotorTail2 = 0;
00133 
00134 }