Bryce Williams
WiFi Webserver Robot with PID Motor Control
Dependencies: HALLFX_ENCODER MotorDriver PID SDFileSystem mbed
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 /* 00002 Uses the ESP8266 WiFi Chip to set up a WiFi Webserver used to control 00003 the speed of a Sparkfun Redbot Rotbot. USE FIREFOX 00004 Web Browser 00005 00006 NOTES: 00007 1. Webpage Handling in this program is specific to a CUSTOM 00008 WEBPAGE. Program must be modified to handle specfically a new 00009 webpage. A copy of the webpage for this program can be found at 00010 the end of this program page. Simply copy and past text into a 00011 html file and save as the given name. 00012 00013 2. Developed and tested with FireFox 42.0 Web Browser. Does not seem to work 00014 well with Google Chrome or Internet Explorer for some reason... they seem 00015 to generate two post requests which messes with the user input values. 00016 00017 3. There are a bunch of printf statements in the code that can be 00018 uncommented for debugging in a serial terminal progrom. 00019 00020 00021 TODO: ESP8366 has a max packet send size. Make sure we implement 00022 a method to send webpages that exceed this value. The max size is 00023 listed in the official ESP8266 AT Commands Documentation, I think 00024 it is 2048 bytes/chars 00025 00026 TODO: CREATE CONFIG FUNCTION TO SET SSID, PASSWORD, BAUDRATE ETC. 00027 Perhaps have a serial terminal method to take user input, and 00028 put the function call into a #ifdef WiFiConfig statement, so 00029 that the user can enable it to config Wifi module then turn 00030 it off once Wifi module is configed so that this program can 00031 run in a "stand alone" mode. 00032 00033 Reference/ Sources: This code is based on 00034 https://developer.mbed.org/users/4180_1/notebook/using-the-esp8266-with-the-mbed-lpc1768/ 00035 */ 00036 00037 #include "mbed.h" 00038 #include "SDFileSystem.h" 00039 #include "PID.h" 00040 #include "HALLFX_ENCODER.h" 00041 #include "MotorDriver.h" 00042 #include <algorithm> 00043 00044 //#define DEBUG // Uncomment for serial terminal print debugging 00045 // Comment out to turn prints off for normal op 00046 00047 /*********PID CONTROLLER SPECIFIC DECLARATIONS********************************/ 00048 /*****************************************************************************/ 00049 float kp, ki, kd; // Working gain vars 00050 float working_setpoint; // Used for web parsing and updating 00051 float setpoint; // This is used by PID objects, because 00052 // encoders are not quadrature and 00053 // do not have direction information 00054 // we use this to store the absolute 00055 // value of the working_setpoint. 00056 // Specifically setpoint is used only 00057 // for PID objects. Refer to 00058 // pid_callback() function 00059 float feedbackL, outputL; // Should these be volatile? 00060 float feedbackR, outputR; // Should these be volatile? 00061 const float output_lower_limit = 0.0; 00062 const float output_upper_limit = 1.0; 00063 const float FEEDBACK_SCALE = 1.0/384.0; // Scale feedback to 1rev/3000cnts 00064 // this is encoder specific. 00065 enum CONTROL_MODE{PID_OFF = 0, PID_ON = 1}; 00066 int control_mode = PID_ON; 00067 const float Ts = 0.04; // 25Hz Sample Freq (40ms Sample Time) 00068 const float Ts_PID_CALLBACK = Ts/2.0; // Update Motors and sensers twice as 00069 // fast as PID sample rate, ensures 00070 // PID feedback is upto date every 00071 // time PID calculations run 00072 00073 const float kp_init = 0.01; // Good Kp for Speed Control; Start with this 00074 const float ki_init= 0.015; // Good Ki for Speed Control; Start with this 00075 const float kd_init = 0.0001; // Good Kd for Speed Control; Start with this 00076 00077 PID pidL(&setpoint, &feedbackL, &outputL, 00078 output_lower_limit, output_upper_limit, 00079 kp_init, ki_init, kd_init, Ts); 00080 PID pidR(&setpoint, &feedbackR, &outputR, 00081 output_lower_limit, output_upper_limit, 00082 kp_init, ki_init, kd_init, Ts); 00083 MotorDriver mtrR(p20, p19, p25, 10000.0, true); // in1, in2, pwm, pwmFreq, isBrakeable 00084 MotorDriver mtrL(p18, p17, p24, 10000.0, true); // in1, in2, pwm, pwmFreq, isBrakeable 00085 HALLFX_ENCODER encR(p21); 00086 HALLFX_ENCODER encL(p22); 00087 void pid_callback(); // Updates encoder feedback and motor output 00088 Ticker motor; // Interrupt for feedback and motor updates 00089 /*****************************************************************************/ 00090 /*****************************************************************************/ 00091 00092 /**********WEB SERVER SPECIFIC DECLARTATIONS**********************************/ 00093 /*****************************************************************************/ 00094 SDFileSystem sd(p5,p6,p7,p8,"sd"); // MOSI, MISO, SCLK, CS, 00095 // Virtual File System Name 00096 Serial esp(p13, p14); // tx, rx 00097 DigitalOut espRstPin(p26); // ESP Reset 00098 DigitalOut led(LED4); 00099 00100 Timer t1; 00101 Timer t2; 00102 00103 void init(char* buffer, int size); 00104 void getreply(int timeout_ms, char* buffer, int size, int numBytes); 00105 void startserver(char* buffer, int size); 00106 void update_webpage(char* webpage, float working_setpoint, float kp, float ki, float kd); 00107 void parse_input(char* webpage_user_data, int* control_mode, float* working_setpoint, float* kp, float* ki, float* kd); 00108 int port =80; // set server port 00109 int serverTimeout_secs =5; // set server timeout in seconds in case 00110 // link breaks. 00111 /*****************************************************************************/ 00112 /*****************************************************************************/ 00113 00114 // Common Application Declarations 00115 Serial pc(USBTX, USBRX); 00116 float clip(float value, float lower, float upper); 00117 00118 int main() 00119 { 00120 printf("Starting\n"); 00121 00122 /****************** Load Webpage from SD Card***************************************/ 00123 /***********************************************************************************/ 00124 char file[] = "/sd/pid_bot.html"; 00125 00126 // Get file size so we can dynamically allocate buffer size 00127 int num_chars = 0; 00128 FILE *fp = fopen(file, "r"); 00129 while(!feof(fp)){ 00130 fgetc(fp); 00131 num_chars++; 00132 } 00133 rewind(fp); // Go to beginning of file 00134 00135 #ifdef DEBUG 00136 printf("Webpage Data Size: %d byte\r\n", num_chars); 00137 #endif 00138 00139 const int WEBPAGE_SIZE = num_chars; 00140 char webpage[WEBPAGE_SIZE]; 00141 webpage[0] = NULL; // Init our array so that element zero contains a null 00142 // This is important, ensures strings are placed into 00143 // buffer starting at element 0... not some random 00144 // elment 00145 // Read in and buffer file to memory 00146 if(fp == NULL){ 00147 printf("Error: No Such File or something :("); 00148 return 1; 00149 } 00150 else{ 00151 while(!feof(fp)){ 00152 fgets(webpage + strlen(webpage), WEBPAGE_SIZE, fp); // Get a string from stream, add to buffer 00153 } 00154 } 00155 fclose(fp); 00156 printf("Webpage Buffer Size: %d bytes\r\n", sizeof(webpage)); 00157 update_webpage(webpage, working_setpoint, kp_init, ki_init, kd_init); // Update Webpage for 00158 // Position Mode 00159 /***********************************************************************************/ 00160 /***********************************************************************************/ 00161 00162 /***************BRING UP SERVER*****************************************************/ 00163 /***********************************************************************************/ 00164 char buff[5000]; // Working buffer 00165 init(buff, sizeof(buff)); // Init ESP8266 00166 00167 esp.baud(115200); // ESP8266 baudrate. Maximum on KLxx' is 115200, 230400 works on K20 and K22F 00168 00169 startserver(buff, sizeof(buff)); // Configure the ESP8266 and Setup as Server 00170 00171 printf(buff); // If start successful buff contains IP address... 00172 // if not if contains an error. 00173 /***********************************************************************************/ 00174 /***********************************************************************************/ 00175 00176 /************Initialize the PID*****************************************************/ 00177 /***********************************************************************************/ 00178 working_setpoint = 0.0; 00179 encL.reset(); 00180 encR.reset(); 00181 feedbackL = encL.read(); 00182 feedbackR = encR.read(); 00183 00184 // Update sensors and feedback twice as fast as PID sample time 00185 // this makes pid react in real-time avoiding errors due to 00186 // missing counts etc. 00187 motor.attach(&pid_callback, Ts_PID_CALLBACK); 00188 00189 // Start PID sampling 00190 pidL.start(); 00191 pidR.start(); 00192 00193 /***********************************************************************************/ 00194 /***********************************************************************************/ 00195 00196 while(1){ 00197 /**************SERVICE WEBPAGE******************************************************/ 00198 /***********************************************************************************/ 00199 if(esp.readable()){ 00200 getreply(500, buff, sizeof(buff), sizeof(buff) -1); // Get full buff, leave last element for null char 00201 #ifdef DEBUG 00202 printf("\r\n*************WORKING BUFFER******************************\r\n"); 00203 printf(buff); printf("\n"); 00204 printf("\r\n**************END WORKING BUFFER**************************\r\n"); 00205 #endif 00206 // If Recieved Data get ID, Length, and Data 00207 char* rqstPnt = strstr(buff, "+IPD"); 00208 if(rqstPnt != NULL){ 00209 int id, len; 00210 char type[10]; memset(type, '\0', sizeof(type)); // Create and null out data buff 00211 sscanf(rqstPnt, "+IPD,%d,%d:%s ", &id, &len, type); 00212 #ifdef DEBUG 00213 printf("ID: %i\nLen: %i\nType: %s\n", id, len, type); 00214 #endif 00215 // If GET or POST request "type" parse and update user input then send webpage 00216 if(strstr(type, "GET") != NULL || strstr(type, "POST") != NULL){ 00217 #ifdef DEBUG 00218 printf("I got web request\n"); 00219 #endif 00220 /* Read Webpage <Form> data sent using "method=POST"... 00221 Note: Input elements in the <Form> need a set name attribute to 00222 appear in the returned HTML body. Thus to "POST" data ensure: 00223 <Form method="POST"> <input type="xxx" name="xxx" value="xxx"> 00224 <input type="xxx" value="xxx"> </Form> 00225 Only the input with name="xxx" will appear in body of HTML 00226 */ 00227 #ifdef DEBUG 00228 printf("\r\n*************USER INPUT**********************************\r\n"); 00229 #endif 00230 00231 parse_input(buff, &control_mode, &working_setpoint, &kp, &ki, &kd); 00232 working_setpoint = clip(working_setpoint, -90.0, 90.0); // Redbot motors are ~90 RPM; max html field size for setpoint is 7 (to accomodate for a "-" sign 00233 kp = clip(kp, 0.00, 999.99); // 999.99 is max size that can be updated to webpage, i.e. field is 6 digits (see html) 00234 ki = clip(ki, 0.00, 999.99); // 999.99 is max size that can be updated to webpage, i.e. field is 6 digits (see html) 00235 kd = clip(kd, 0.00, 999.99); // 999.99 is max size that can be updated to webpage, i.e. field is 6 digits (see html) 00236 00237 #ifdef DEBUG 00238 printf("User Entered: \ncontrol_mode: %i\nSetpoint: %7.4f\nKp: %6.4f\nKi: %6.4f\nKd: %6.4f\n", 00239 control_mode, working_setpoint, kp, ki, kd); 00240 #endif 00241 00242 pidL.set_parameters(kp, ki, kd, Ts); // Updata PID params 00243 pidR.set_parameters(kp, ki, kd, Ts); // Updata PID params 00244 00245 #ifdef DEBUG 00246 printf("Updated to Kp: %1.4f Ki: %1.4f Kd: %1.4f Ts: %1.4f\r\n", 00247 pidL.getKp(), pidL.getKi(), pidL.getKd(), pidL.getTs()); 00248 printf("Setpoint: %1.4f\r\n", working_setpoint); 00249 printf("\r\n*************END USER INPUT******************************\r\n"); 00250 #endif 00251 00252 // Update Webpage to reflect new values POSTED by client 00253 static bool isFirstRequest = true; 00254 if(!isFirstRequest) update_webpage(webpage, working_setpoint, kp, ki, kd); 00255 else isFirstRequest = false; // First Request just send page with initial values 00256 00257 #ifdef DEBUG 00258 printf(webpage); 00259 #endif 00260 00261 // Command TCP/IP Data Tx 00262 esp.printf("AT+CIPSEND=%d,%d\r\n", id, strlen(webpage)); 00263 getreply(200, buff, sizeof(buff), 15); /*TODO: Wait for "OK\r\n>"*/ 00264 00265 #ifdef DEBUG 00266 printf(buff); printf("\n"); 00267 #endif 00268 00269 // Send webpage 00270 // while(!esp.writeable()); // Wait until esp ready to send data 00271 int idx = 0; 00272 while(webpage[idx] != '\0'){ 00273 esp.putc(webpage[idx]); 00274 idx++; 00275 } 00276 00277 // Check status - Success: close channel and update PID controller, Error: reconnect 00278 bool weberror = true; 00279 t2.reset(); t2.start(); 00280 while(weberror ==1 && t2.read_ms() < 5000){ 00281 getreply(500, buff, sizeof(buff), 24); 00282 if(strstr(buff, "SEND OK") != NULL) weberror = false; 00283 } 00284 if(weberror){ 00285 esp.printf("AT+CIPMUX=1\r\n"); 00286 getreply(500, buff, sizeof(buff), 10); 00287 #ifdef DEBUG 00288 printf(buff); printf("\n"); 00289 #endif 00290 esp.printf("AT+CIPSERVER=1,%d\r\n", port); 00291 getreply(500, buff, sizeof(buff), 10); 00292 #ifdef DEBUG 00293 printf(buff); printf("\n"); 00294 #endif 00295 } 00296 else{ 00297 esp.printf("AT+CIPCLOSE=%d\r\n", id); // Notice id is an int formatted to string 00298 getreply(500, buff, sizeof(buff), 24); 00299 #ifdef DEBUG 00300 printf(buff); printf("\n"); 00301 #endif 00302 } 00303 } 00304 } 00305 } 00306 /*********************************************************************/ 00307 /*********************************************************************/ 00308 } 00309 } 00310 00311 // Initialize ESP8266 00312 void init(char* buffer, int size){ 00313 // Hardware Reset ESP 00314 espRstPin=0; 00315 wait(0.5); 00316 espRstPin=1; 00317 // Get start up junk from ESP8266 00318 getreply(6000, buffer, size, 500); 00319 } 00320 00321 // Get Command and ESP status replies 00322 void getreply(int timeout_ms, char* buffer, int size, int numBytes) 00323 { 00324 memset(buffer, '\0', size); // Null out buffer 00325 t1.reset(); 00326 t1.start(); 00327 int idx = 0; 00328 while(t1.read_ms()< timeout_ms && idx < numBytes) { 00329 if(esp.readable()) { 00330 buffer[idx] = esp.getc(); 00331 idx++; 00332 } 00333 } 00334 t1.stop(); 00335 } 00336 00337 // Starts and restarts webserver if errors detected. 00338 void startserver(char* buffer, int size) 00339 { 00340 esp.printf("AT+RST\r\n"); // BWW: Reset the ESP8266 00341 getreply(8000, buffer, size, 1000); 00342 00343 if (strstr(buffer, "OK") != NULL) { 00344 // BWW: Set ESP8266 for multiple connections 00345 esp.printf("AT+CIPMUX=1\r\n"); 00346 getreply(500, buffer, size, 20); 00347 00348 // BWW: Set ESP8266 as Server on given port 00349 esp.printf("AT+CIPSERVER=1,%d\r\n", port); 00350 getreply(500, buffer, size, 20); // BWW: Wait for reply 00351 00352 // BWW: Set ESP8266 Server Timeout 00353 esp.printf("AT+CIPSTO=%d\r\n", serverTimeout_secs); 00354 getreply(500, buffer, size, 50); // BWW: Wait for reply 00355 00356 // BWW: Request IP Address from router for ESP8266 00357 int weberror = 0; 00358 while(weberror==0) { 00359 esp.printf("AT+CIFSR\r\n"); 00360 getreply(2500, buffer, size, 200); 00361 if(strstr(buffer, "0.0.0.0") == NULL) { 00362 weberror=1; // wait for valid IP 00363 } 00364 } 00365 } 00366 // else ESP8266 did not reply "OK" something is messed up 00367 else { 00368 strcpy(buffer, "ESP8266 Error\n"); 00369 } 00370 } 00371 00372 /* 00373 update_webpage() updates output fields based on webpage user inputs "POSTED" 00374 Preconditions: webpage[] must have the following elements 00375 "kp_output" value="xxx.xx" 00376 "ki_output" value="xxx.xx" 00377 "kp_output" value="xxx.xx" 00378 @param webpage Pointer to webpage char[] 00379 @param kp New kp value posted by user 00380 @param ki New ki value posted by user 00381 @param kd New kd value posted by user 00382 00383 NOTE: THIS IS WEBPAGE SPECIFIC!!!! CHANGE THE CODE IN HERE TO SUITE THE 00384 SPECIFIC APPLICATION WEBPAGE!!! ALSO USED TO REFLECT THE CUSTOM 00385 IMPLEMENTATION OF THE parse_intput() function. MAKE SURE THESE TWO FUNCTIONS 00386 INTEGRATE PROPERLY!!! 00387 */ 00388 void update_webpage(char* webpage, float working_setpoint, float kp, float ki, float kd){ 00389 // Change output value to reflect new control mode, setpoint, kp, ki, kd values 00390 char* begin; 00391 char temp[8]; 00392 int idx; 00393 00394 // Update Control Mode Radio Buttons 00395 memset(temp, '\0', sizeof(temp)); 00396 idx = 0; 00397 begin = strstr(webpage, "name=\"control_mode\" value=\"") + 00398 sizeof("name=\"control_mode\" value=\"0"); // Update Control Mode Position Radio Button 00399 if(control_mode == PID_OFF) sprintf(temp, "%s", "checked");// If PID OFF active "check" it 00400 else sprintf(temp, "%s", " "); // else "clear" it 00401 while(temp[idx] != '\0'){ // Write "checked"/" " to field 00402 begin[idx] = temp[idx]; 00403 idx++; 00404 } 00405 memset(temp, '\0', sizeof(temp)); 00406 idx = 0; 00407 begin = strstr(webpage, "name=\"control_mode\" value=\"") + 00408 sizeof("name=\"control_mode\" value=\"0\""); // Nav to first Control Mode Radio Button (Position) 00409 begin = strstr(begin, "name=\"control_mode\" value=\"") + 00410 sizeof("name=\"control_mode\" value=\"1"); // Nav to second Control Mode Radio Button (Speed) 00411 if(control_mode == PID_ON) sprintf(temp, "%s", "checked"); // If PID ON active "check" it 00412 else sprintf(temp, "%s", " "); // else "clear" it 00413 while(temp[idx] != '\0'){ // Write "checked"/" " to field 00414 begin[idx] = temp[idx]; 00415 idx++; 00416 } 00417 00418 // Update Kp Paramater Field 00419 memset(temp, '\0', sizeof(temp)); 00420 idx = 0; 00421 begin = strstr(webpage, "name=\"kp_input\" value=\"") + 00422 sizeof("name=\"kp_input\" value="); // Points to start of kp_output field 00423 // Determine precision of float such temp string has no empty spaces; 00424 // i.e. each space must have a value or a decimal point, other wise webbrowser may not recognize value 00425 if(kp >= 100) sprintf(temp, "%6.2f", kp); // xxx.00 00426 else if(10 <= kp && kp < 100) sprintf(temp, "%6.3f", kp); // xx.000 00427 else sprintf(temp, "%6.4f", kp); // x.0000 00428 while(temp[idx] != '\0'){ // Overwrite old digits with new digits 00429 begin[idx] = temp[idx]; 00430 idx++; 00431 } 00432 00433 // Update Ki Parameter Field 00434 memset(temp, '\0', sizeof(temp)); 00435 idx = 0; 00436 begin = strstr(webpage, "name=\"ki_input\" value=\"") + 00437 sizeof("name=\"ki_input\" value="); // Points to start of ki_output field 00438 // Determine precision of float such temp string has no empty spaces; 00439 // i.e. each space must have a value or a decimal point, other wise webbrowser may not recognize value 00440 if(ki >= 100) sprintf(temp, "%6.2f", ki); // xxx.00 00441 else if(10 <= ki && ki < 100) sprintf(temp, "%6.3f", ki); // xx.000 00442 else sprintf(temp, "%6.4f", ki); // x.0000 00443 while(temp[idx] != '\0'){ // Overwrite old digits with new digits 00444 begin[idx] = temp[idx]; 00445 idx++; 00446 } 00447 00448 // Update Kd Parameter Field 00449 memset(temp, '\0', sizeof(temp)); 00450 idx = 0; 00451 begin = strstr(webpage, "name=\"kd_input\" value=\"")+ 00452 sizeof("name=\"kd_input\" value="); // Points to start of kd_output field 00453 // Determine precision of float such temp string has no empty spaces; 00454 // i.e. each space must have a value or a decimal point, other wise webbrowser may not recognize value 00455 if(kd >= 100) sprintf(temp, "%6.2f", kd); // xxx.00 00456 else if(10 <= kd && kd < 100) sprintf(temp, "%6.3f", kd); // xx.000 00457 else sprintf(temp, "%6.4f", kd); // x.0000 00458 while(temp[idx] != '\0'){ // Overwrite old digits with new digits 00459 begin[idx] = temp[idx]; 00460 idx++; 00461 } 00462 00463 // Update Setpoint Parameter Field 00464 // Determine precision of float such temp string has no empty spaces; 00465 // i.e. each space must have a value or a decimal point or neg sign, 00466 // other wise webbrowser may not recognize value 00467 memset(temp, '\0', sizeof(temp)); 00468 idx = 0; 00469 begin = strstr(webpage, "name=\"setpoint_input\" value=\"")+ 00470 sizeof("name=\"setpoint_input\" value="); // Points to start of kp_output field 00471 // Determine precision of float such temp string has no empty spaces; 00472 // i.e. each space must have a value or a decimal point, other wise webbrowser may not recognize value 00473 if(working_setpoint >= 0.00){ 00474 if(working_setpoint >= 100) sprintf(temp, "%6.3f", working_setpoint); // xxx.000 00475 else if(10 <= working_setpoint && working_setpoint < 100) sprintf(temp, "%7.4f", working_setpoint); // xx.0000 00476 else sprintf(temp, "%6.5f", working_setpoint); // x.00000 00477 } 00478 else{ 00479 if(working_setpoint <= -100) sprintf(temp, "%6.2f", working_setpoint); // -xxx.00 00480 else if(-100 < working_setpoint && working_setpoint <= -10) sprintf(temp, "%6.3f", working_setpoint); // -xx.000 00481 else sprintf(temp, "%6.4f", working_setpoint); // -x.0000 00482 } 00483 while(temp[idx] != '\0'){ // Overwrite old digits with new digits 00484 begin[idx] = temp[idx]; 00485 idx++; 00486 } 00487 } 00488 00489 /* 00490 parse_input() take a char*, in particular a pointer to Webpage User 00491 Input Data, for example: 00492 char str[] = "+IPD,0,44:kp_input=0.12&ki_input=14.25&kd_input=125.42"; 00493 00494 and parses out the Setpoint Kp, Ki, Kd values that the user entered 00495 and posted in the webpage. Values are converted to floats and 00496 assigned to the given argurments. 00497 00498 NOTE: THIS IS WEBPAGE SPECIFIC!!!! CHANGE THE CODE IN HERE TO SUITE THE 00499 SPECIFIC APPLICATION WEBPAGE!!! THESE EXTRACTED VALUES WILL BE USED IN 00500 THE update_webpage() function. MAKE SURE THESE TWO FUNCTIONS INTEGRATE 00501 PROPERLY!!! 00502 */ 00503 void parse_input(char* webpage_user_data, int* control_mode, float *working_setpoint, float* kp, float* ki, float* kd){ 00504 char keys[] = {'&', '\0'}; 00505 00506 // Parse out user input values 00507 char input_buff[50]; 00508 char* begin; 00509 char* end; 00510 00511 // Parse and Update Control Mode Value 00512 memset(input_buff, '\0', sizeof(input_buff)); // Null out input buff 00513 begin = strstr(webpage_user_data, "control_mode=") + 00514 sizeof("control_mode"); // Points to start of setpoint_input value 00515 end = begin + strcspn(begin, keys); // Points to end of setpoint_input value 00516 for(long i = 0; i < end - begin; i++){ // Parse out the value one char at a time 00517 input_buff[i] = begin[i]; 00518 } 00519 *control_mode = atoi(input_buff); 00520 00521 // Parse and Update Setpoint Value 00522 memset(input_buff, '\0', sizeof(input_buff)); // Null out input buff 00523 begin = strstr(webpage_user_data, "setpoint_input=") + 00524 sizeof("setpoint_input"); // Points to start of setpoint_input value 00525 end = begin + strcspn(begin, keys); // Points to end of setpoint_input value 00526 for(long i = 0; i < end - begin; i++){ // Parse out the value one char at a time 00527 input_buff[i] = begin[i]; 00528 } 00529 *working_setpoint = atof(input_buff); 00530 00531 // Parse and Update Kp Value 00532 memset(input_buff, '\0', sizeof(input_buff)); // Null out input buff 00533 begin = strstr(webpage_user_data, "kp_input=") + 00534 sizeof("kp_input"); // Points to start of kp_input value 00535 end = begin + strcspn(begin, keys); // Points to end of kp_input value 00536 for(long i = 0; i < end - begin; i++){ // Parse out the value one char at a time 00537 input_buff[i] = begin[i]; 00538 } 00539 *kp = atof(input_buff); 00540 00541 // Parse and Update Ki Value 00542 memset(input_buff, '\0', sizeof(input_buff)); // Null out input buff 00543 begin = strstr(webpage_user_data, "ki_input=") + 00544 sizeof("ki_input"); // Points to start of ki_input value 00545 end = begin + strcspn(begin, keys); // Points to end of ki_input value 00546 for(long i = 0; i < end - begin; i++){ // Parse out the value one char at a time 00547 input_buff[i] = begin[i]; 00548 } 00549 *ki = atof(input_buff); 00550 00551 // Parse and Update Kd Value 00552 memset(input_buff, '\0', sizeof(input_buff)); // Null out input buff 00553 begin = strstr(webpage_user_data, "kd_input=") + 00554 sizeof("kd_input"); // Points to start of kd_input value 00555 end = begin + strcspn(begin, keys); // Points to end of kd_input value 00556 for(long i = 0; i < end - begin; i++){ // Parse out the value one char at a time 00557 input_buff[i] = begin[i]; 00558 } 00559 *kd = atof(input_buff); 00560 00561 // Parse for stop button press; we only have to see if "STOP" exists in the input buffer 00562 // because it is just a button and will not be included unless it is pressed!... Makes 00563 // our job easy!... if stop was pressed then set setpoint to zero! 00564 if(strstr(webpage_user_data, "STOP") != NULL) *working_setpoint = 0; 00565 } 00566 00567 void pid_callback(){ 00568 static int lastMode = control_mode; 00569 00570 // If control_mode is PID_OFF turn off PID controllers and run motors in open loop 00571 if(control_mode == PID_OFF){ 00572 // Mode changed; turn off PID controllers 00573 if(lastMode != control_mode){ 00574 pidL.stop(); pidR.stop(); 00575 lastMode = PID_OFF; 00576 } 00577 // Set motor speed in open loop mode 00578 // Motor direction based on working setpoint var 00579 int dirL, dirR; 00580 if(working_setpoint < 0.0){ 00581 dirL = -1; dirR = -1; 00582 } 00583 else{ 00584 dirL = 1; dirR = 1; 00585 } 00586 float speed = abs(working_setpoint) / 90.0; // Normalize based on 90 RPM 00587 mtrL.forceSetSpeed(speed * dirL); 00588 mtrR.forceSetSpeed(speed * dirR); 00589 } 00590 // else control_mode is PID_ON, turn on PID controllers and run motors in closed loop 00591 else{ 00592 // Mode changed; turn on PID controllers 00593 if(lastMode != control_mode){ 00594 pidL.start(); pidR.start(); 00595 lastMode = PID_ON; 00596 } 00597 // Deal with feedback and update motors 00598 // Motor direction based on working setpoint var 00599 int dirL, dirR; 00600 if(working_setpoint < 0.0){ 00601 dirL = -1; dirR = -1; 00602 } 00603 else{ 00604 dirL = 1; dirR = 1; 00605 } 00606 00607 // Setpoint vars used by PID objects are concerned with 00608 // only SPEED not direction. 00609 setpoint = abs(working_setpoint); 00610 00611 float k = Ts_PID_CALLBACK; // Discrete time, (Ts/2 because this callback is called 00612 // at interval of Ts/2... or twice as fast as pid controller) 00613 static int last_count_L = 0; 00614 static int last_count_R = 0; 00615 int countL = encL.read(); 00616 int countR = encR.read(); 00617 00618 // Because encoders are not quadrature we must handle the sign outselves, 00619 // i.e. explicitly make calcs based on the direction we have set the motor 00620 float raw_speed_L = ((countL - last_count_L)*FEEDBACK_SCALE) / k; 00621 float rpm_speed_L = raw_speed_L * 60.0; // Convert speed to RPM 00622 00623 float raw_speed_R = ((countR - last_count_R)*FEEDBACK_SCALE) / k; 00624 float rpm_speed_R = raw_speed_R * 60.0; // Convert speed to RPM 00625 00626 last_count_L = countL; // Save last count 00627 last_count_R = countR; 00628 feedbackL = rpm_speed_L; 00629 feedbackR = rpm_speed_R; 00630 00631 mtrL.forceSetSpeed(outputL * dirL); 00632 mtrR.forceSetSpeed(outputR * dirR); 00633 } 00634 } 00635 00636 /* 00637 Clips value to lower/ uppper 00638 @param value The value to clip 00639 @param lower The mininum allowable value 00640 @param upper The maximum allowable value 00641 @return The resulting clipped value 00642 */ 00643 float clip(float value, float lower, float upper){ 00644 return std::max(lower, std::min(value, upper)); 00645 } 00646 00647 /**************************WEB PAGE TEXT**************************************/ 00648 /***************************************************************************** 00649 Copy and past text below into a html file and save as the given file name to 00650 your SD card. 00651 00652 file name: pid_bot.html 00653 00654 html text: 00655 00656 <!DOCTYPE html> 00657 <html> 00658 <head> 00659 <title>PID RedBot Control</title> 00660 </head> 00661 <body> 00662 <h1>PID Motor Control</h1> 00663 <h2>PID Status</h2> 00664 <form title="User Input" method="post"> 00665 PID Controls: <br> 00666 <input type="radio" name="control_mode" value="0"checked>PID OFF 00667 <br> 00668 <input type="radio" name="control_mode" value="1" >PID ON 00669 <br> 00670 Setpoint:<br> 00671 <input type="number" name="setpoint_input" value="0000.00" step="0.0000001" size="6" /><br> 00672 Proportional Gain: (Good Starting Value: 0.01)<br> 00673 <input type="number" name="kp_input" value="000.01" step="0.0000001" size="6" /><br> 00674 Integral Gain: (Good Starting Value: 0.015)<br> 00675 <input type="number" name="ki_input" value="00.015" step="0.0000001" size="6" /><br> 00676 Derivative Gain: (Good Starting Value: 0.0001)<br> 00677 <input type="number" name="kd_input" value="0.0001" step="0.0000001" size="6" /><br> 00678 <br> 00679 <input type="submit" value="Update" /> 00680 <input type="submit" name="STOP" value="STOP!" /> 00681 </form> 00682 </body> 00683 </html> 00684 00685 *****************************************************************************/ 00686 /*****************************************************************************/
Generated on Fri Jul 15 2022 06:01:43 by
1.7.2