Bryce Williams
PID Motor Speed & Position Control Over WiFi using ESP8266 WiFi module, US Digital E4P-100-079 Quadrature Encoder, HN-GH12-1634T 30:1 200 RPM DC Motor, and LMD18200 H-Bridge Breakout
Dependencies: 4DGL-uLCD-SE PID QEI 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 position or speed of a motor using a PID controller. 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 TODO: Implement stop button in webpage 00034 */ 00035 00036 00037 #include "mbed.h" 00038 #include "SDFileSystem.h" 00039 #include "PID.h" 00040 #include "QEI.h" 00041 #include <algorithm> 00042 00043 #define DEBUG // Uncomment for serial terminal print debugging 00044 // Comment out to turn prints off for normal op 00045 00046 /*********PID CONTROLLER SPECIFIC DECLARATIONS********************************/ 00047 /*****************************************************************************/ 00048 00049 float setpoint, feedback, output; // Should these be volatile? 00050 const float output_lower_limit = -1.0; 00051 const float output_upper_limit = 1.0; 00052 const float FEEDBACK_SCALE = 1.0/3000.0; // Scale feedback to 1rev/3000cnts 00053 // this is encoder specific. 00054 enum CONTROL_MODE{POSITION = 0, SPEED = 1}; 00055 bool control_mode = POSITION; 00056 float kp, ki, kd; // Gain variables for working 00057 const float Ts = 0.04; // 25Hz Sample Freq (40ms Sample Time) 00058 const float Ts_PID_CALLBACK = Ts/2.0; // Update Motors and sensers twice as 00059 // fast as PID sample rate, ensures 00060 // PID feedback is upto date every 00061 // time PID calculations run 00062 00063 const float kp_init = 2.5; // Good Kp for Position Control 00064 const float ki_init = 5.0; // Good Ki for Position Control 00065 const float kd_init = 0.25; // Good Kd for Position Control 00066 00067 PID pid(&setpoint, &feedback, &output, 00068 output_lower_limit, output_upper_limit, 00069 kp_init, ki_init, kd_init, Ts); // Init for position control 00070 QEI encoder(p15, p16); 00071 PwmOut mtr_pwm(p25); 00072 DigitalOut mtr_dir(p24); 00073 void pid_callback(); // Updates encoder feedback and motor output 00074 Ticker motor; 00075 /*****************************************************************************/ 00076 /*****************************************************************************/ 00077 00078 /**********WEB SERVER SPECIFIC DECLARTATIONS**********************************/ 00079 /*****************************************************************************/ 00080 SDFileSystem sd(p5,p6,p7,p8,"sd"); // MOSI, MISO, SCLK, CS, 00081 // Virtual File System Name 00082 Serial esp(p13, p14); // tx, rx 00083 DigitalOut espRstPin(p26); // ESP Reset 00084 DigitalOut led(LED4); 00085 00086 Timer t1; 00087 Timer t2; 00088 00089 void init(char* buffer, int size); 00090 void getreply(int timeout_ms, char* buffer, int size, int numBytes); 00091 void startserver(char* buffer, int size); 00092 void update_webpage(char* webpage, float setpoint, float kp, float ki, float kd); 00093 void parse_input(char* webpage_user_data, bool* control_mode, float* setpoint, float* kp, float* ki, float* kd); 00094 int port =80; // set server port 00095 int serverTimeout_secs =5; // set server timeout in seconds in case 00096 // link breaks. 00097 /*****************************************************************************/ 00098 /*****************************************************************************/ 00099 00100 // Common Application Declarations 00101 Serial pc(USBTX, USBRX); 00102 float clip(float value, float lower, float upper); 00103 00104 int main() 00105 { 00106 printf("Starting\n"); 00107 00108 /****************** Load Webpage from SD Card***************************************/ 00109 /***********************************************************************************/ 00110 char file[] = "/sd/pid_dual.html"; 00111 00112 // Get file size so we can dynamically allocate buffer size 00113 int num_chars = 0; 00114 FILE *fp = fopen(file, "r"); 00115 while(!feof(fp)){ 00116 fgetc(fp); 00117 num_chars++; 00118 } 00119 rewind(fp); // Go to beginning of file 00120 00121 #ifdef DEBUG 00122 printf("Webpage Data Size: %d byte\r\n", num_chars); 00123 #endif 00124 00125 const int WEBPAGE_SIZE = num_chars; 00126 char webpage[WEBPAGE_SIZE]; 00127 webpage[0] = NULL; // Init our array so that element zero contains a null 00128 // This is important, ensures strings are placed into 00129 // buffer starting at element 0... not some random 00130 // elment 00131 // Read in and buffer file to memory 00132 if(fp == NULL){ 00133 printf("Error: No Such File or something :("); 00134 return 1; 00135 } 00136 else{ 00137 while(!feof(fp)){ 00138 fgets(webpage + strlen(webpage), WEBPAGE_SIZE, fp); // Get a string from stream, add to buffer 00139 } 00140 } 00141 fclose(fp); 00142 printf("Webpage Buffer Size: %d bytes\r\n", sizeof(webpage)); 00143 update_webpage(webpage, setpoint, kp_init, ki_init, kd_init); // Update Webpage for 00144 // Position Mode 00145 /***********************************************************************************/ 00146 /***********************************************************************************/ 00147 00148 /***************BRING UP SERVER*****************************************************/ 00149 /***********************************************************************************/ 00150 char buff[5000]; // Working buffer 00151 init(buff, sizeof(buff)); // Init ESP8266 00152 00153 esp.baud(115200); // ESP8266 baudrate. Maximum on KLxx' is 115200, 230400 works on K20 and K22F 00154 00155 startserver(buff, sizeof(buff)); // Configure the ESP8266 and Setup as Server 00156 00157 printf(buff); // If start successful buff contains IP address... 00158 // if not if contains an error. 00159 /***********************************************************************************/ 00160 /***********************************************************************************/ 00161 00162 /************Initialize the PID*****************************************************/ 00163 /***********************************************************************************/ 00164 setpoint = 0.0; 00165 encoder.reset(); 00166 feedback = encoder.read(); 00167 00168 // Init the motor 00169 mtr_dir = 0; // Can be 0 or 1, sets the direction 00170 mtr_pwm = 0.0; 00171 00172 // Update sensors and feedback twice as fast as PID sample time 00173 // this makes pid react in real-time avoiding errors due to 00174 // missing counts etc. 00175 motor.attach(&pid_callback, Ts_PID_CALLBACK); 00176 00177 // Start PID sampling 00178 pid.start(); 00179 00180 /***********************************************************************************/ 00181 /***********************************************************************************/ 00182 00183 while(1){ 00184 /**************SERVICE WEBPAGE******************************************************/ 00185 /***********************************************************************************/ 00186 if(esp.readable()){ 00187 getreply(500, buff, sizeof(buff), sizeof(buff) -1); // Get full buff, leave last element for null char 00188 #ifdef DEBUG 00189 printf("\r\n*************WORKING BUFFER******************************\r\n"); 00190 printf(buff); printf("\n"); 00191 printf("\r\n**************END WORKING BUFFER**************************\r\n"); 00192 #endif 00193 // If Recieved Data get ID, Length, and Data 00194 char* rqstPnt = strstr(buff, "+IPD"); 00195 if(rqstPnt != NULL){ 00196 int id, len; 00197 char type[10]; memset(type, '\0', sizeof(type)); // Create and null out data buff 00198 sscanf(rqstPnt, "+IPD,%d,%d:%s ", &id, &len, type); 00199 #ifdef DEBUG 00200 printf("ID: %i\nLen: %i\nType: %s\n", id, len, type); 00201 #endif 00202 // If GET or POST request "type" parse and update user input then send webpage 00203 if(strstr(type, "GET") != NULL || strstr(type, "POST") != NULL){ 00204 #ifdef DEBUG 00205 printf("I got web request\n"); 00206 #endif 00207 /* Read Webpage <Form> data sent using "method=POST"... 00208 Note: Input elements in the <Form> need a set name attribute to 00209 appear in the returned HTML body. Thus to "POST" data ensure: 00210 <Form method="POST"> <input type="xxx" name="xxx" value="xxx"> 00211 <input type="xxx" value="xxx"> </Form> 00212 Only the input with name="xxx" will appear in body of HTML 00213 */ 00214 #ifdef DEBUG 00215 printf("\r\n*************USER INPUT**********************************\r\n"); 00216 #endif 00217 00218 parse_input(buff, &control_mode, &setpoint, &kp, &ki, &kd); 00219 setpoint = clip(setpoint, -999.99, 999.99); // -999.99 is max size that can be updated to webpage, i.e. field is 7 digits (see html) 00220 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) 00221 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) 00222 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) 00223 00224 #ifdef DEBUG 00225 printf("User Entered: \ncontrol_mode: %i\nSetpoint: %7.4f\nKp: %6.4f\nKi: %6.4f\nKd: %6.4f\n", 00226 control_mode, setpoint, kp, ki, kd); 00227 #endif 00228 00229 pid.set_parameters(kp, ki, kd, Ts); // Updata PID params 00230 00231 #ifdef DEBUG 00232 printf("Updated to Kp: %1.4f Ki: %1.4f Kd: %1.4f Ts: %1.4f\r\n", 00233 pid.getKp(), pid.getKi(), pid.getKd(), pid.getTs()); 00234 printf("Setpoint: %1.4f\r\n", setpoint); 00235 printf("Output: %1.4f\r\n", output); 00236 printf("\r\n*************END USER INPUT******************************\r\n"); 00237 #endif 00238 00239 // Update Webpage to reflect new values POSTED by client 00240 static bool isFirstRequest = true; 00241 if(!isFirstRequest) update_webpage(webpage, setpoint, kp, ki, kd); 00242 else isFirstRequest = false; // First Request just send page with initial values 00243 00244 #ifdef DEBUG 00245 printf(webpage); // DEBUGGING ONLY!!! REMOVE FOR RELEASE!!! 00246 #endif 00247 00248 // Command TCP/IP Data Tx 00249 esp.printf("AT+CIPSEND=%d,%d\r\n", id, strlen(webpage)); 00250 getreply(200, buff, sizeof(buff), 15); /*TODO: Wait for "OK\r\n>"*/ 00251 00252 #ifdef DEBUG 00253 printf(buff); printf("\n"); 00254 #endif 00255 00256 // Send webpage 00257 // while(!esp.writeable()); // Wait until esp ready to send data 00258 int idx = 0; 00259 while(webpage[idx] != '\0'){ 00260 esp.putc(webpage[idx]); 00261 idx++; 00262 } 00263 00264 // Check status - Success: close channel and update PID controller, Error: reconnect 00265 bool weberror = true; 00266 t2.reset(); t2.start(); 00267 while(weberror ==1 && t2.read_ms() < 5000){ 00268 getreply(500, buff, sizeof(buff), 24); 00269 if(strstr(buff, "SEND OK") != NULL) weberror = false; 00270 } 00271 if(weberror){ 00272 esp.printf("AT+CIPMUX=1\r\n"); 00273 getreply(500, buff, sizeof(buff), 10); 00274 #ifdef DEBUG 00275 printf(buff); printf("\n"); 00276 #endif 00277 esp.printf("AT+CIPSERVER=1,%d\r\n", port); 00278 getreply(500, buff, sizeof(buff), 10); 00279 #ifdef DEBUG 00280 printf(buff); printf("\n"); 00281 #endif 00282 } 00283 else{ 00284 esp.printf("AT+CIPCLOSE=%d\r\n", id); // Notice id is an int formatted to string 00285 getreply(500, buff, sizeof(buff), 24); 00286 #ifdef DEBUG 00287 printf(buff); printf("\n"); 00288 #endif 00289 } 00290 } 00291 } 00292 } 00293 /*********************************************************************/ 00294 /*********************************************************************/ 00295 00296 00297 } 00298 } 00299 00300 // Initialize ESP8266 00301 void init(char* buffer, int size){ 00302 // Hardware Reset ESP 00303 espRstPin=0; 00304 wait(0.5); 00305 espRstPin=1; 00306 // Get start up junk from ESP8266 00307 getreply(6000, buffer, size, 500); 00308 } 00309 00310 // Get Command and ESP status replies 00311 void getreply(int timeout_ms, char* buffer, int size, int numBytes) 00312 { 00313 memset(buffer, '\0', size); // Null out buffer 00314 t1.reset(); 00315 t1.start(); 00316 int idx = 0; 00317 while(t1.read_ms()< timeout_ms && idx < numBytes) { 00318 if(esp.readable()) { 00319 buffer[idx] = esp.getc(); 00320 idx++; 00321 } 00322 } 00323 t1.stop(); 00324 } 00325 00326 // Starts and restarts webserver if errors detected. 00327 void startserver(char* buffer, int size) 00328 { 00329 esp.printf("AT+RST\r\n"); // BWW: Reset the ESP8266 00330 getreply(8000, buffer, size, 1000); 00331 00332 if (strstr(buffer, "OK") != NULL) { 00333 // BWW: Set ESP8266 for multiple connections 00334 esp.printf("AT+CIPMUX=1\r\n"); 00335 getreply(500, buffer, size, 20); 00336 00337 // BWW: Set ESP8266 as Server on given port 00338 esp.printf("AT+CIPSERVER=1,%d\r\n", port); 00339 getreply(500, buffer, size, 20); // BWW: Wait for reply 00340 00341 // BWW: Set ESP8266 Server Timeout 00342 esp.printf("AT+CIPSTO=%d\r\n", serverTimeout_secs); 00343 getreply(500, buffer, size, 50); // BWW: Wait for reply 00344 00345 // BWW: Request IP Address from router for ESP8266 00346 int weberror = 0; 00347 while(weberror==0) { 00348 esp.printf("AT+CIFSR\r\n"); 00349 getreply(2500, buffer, size, 200); 00350 if(strstr(buffer, "0.0.0.0") == NULL) { 00351 weberror=1; // wait for valid IP 00352 } 00353 } 00354 } 00355 // else ESP8266 did not reply "OK" something is messed up 00356 else { 00357 strcpy(buffer, "ESP8266 Error\n"); 00358 } 00359 } 00360 00361 /* 00362 update_webpage() updates output fields based on webpage user inputs "POSTED" 00363 Preconditions: webpage[] must have the following elements 00364 "kp_output" value="xxx.xx" 00365 "ki_output" value="xxx.xx" 00366 "kp_output" value="xxx.xx" 00367 @param webpage Pointer to webpage char[] 00368 @param kp New kp value posted by user 00369 @param ki New ki value posted by user 00370 @param kd New kd value posted by user 00371 00372 NOTE: THIS IS WEBPAGE SPECIFIC!!!! CHANGE THE CODE IN HERE TO SUITE THE 00373 SPECIFIC APPLICATION WEBPAGE!!! ALSO USED TO REFLECT THE CUSTOM 00374 IMPLEMENTATION OF THE parse_intput() function. MAKE SURE THESE TWO FUNCTIONS 00375 INTEGRATE PROPERLY!!! 00376 */ 00377 void update_webpage(char* webpage, float setpoint, float kp, float ki, float kd){ 00378 // Change output value to reflect new control mode, setpoint, kp, ki, kd values 00379 char* begin; 00380 char temp[8]; 00381 int idx; 00382 00383 // Update Control Mode Radio Buttons 00384 memset(temp, '\0', sizeof(temp)); 00385 idx = 0; 00386 begin = strstr(webpage, "name=\"control_mode\" value=\"") + 00387 sizeof("name=\"control_mode\" value=\"0"); // Update Control Mode Position Radio Button 00388 if(control_mode == POSITION) sprintf(temp, "%s", "checked");// If Position active "check" it 00389 else sprintf(temp, "%s", " "); // else "clear" it 00390 while(temp[idx] != '\0'){ // Write "checked"/" " to field 00391 begin[idx] = temp[idx]; 00392 idx++; 00393 } 00394 memset(temp, '\0', sizeof(temp)); 00395 idx = 0; 00396 begin = strstr(webpage, "name=\"control_mode\" value=\"") + 00397 sizeof("name=\"control_mode\" value=\"0\""); // Nav to first Control Mode Radio Button (Position) 00398 begin = strstr(begin, "name=\"control_mode\" value=\"") + 00399 sizeof("name=\"control_mode\" value=\"1"); // Nav to second Control Mode Radio Button (Speed) 00400 if(control_mode == SPEED) sprintf(temp, "%s", "checked"); // If Speed active "check" it 00401 else sprintf(temp, "%s", " "); // else "clear" it 00402 while(temp[idx] != '\0'){ // Write "checked"/" " to field 00403 begin[idx] = temp[idx]; 00404 idx++; 00405 } 00406 00407 // Update Kp Paramater Field 00408 memset(temp, '\0', sizeof(temp)); 00409 idx = 0; 00410 begin = strstr(webpage, "name=\"kp_input\" value=\"") + 00411 sizeof("name=\"kp_input\" value="); // Points to start of kp_output field 00412 // Determine precision of float such temp string has no empty spaces; 00413 // i.e. each space must have a value or a decimal point, other wise webbrowser may not recognize value 00414 if(kp >= 100) sprintf(temp, "%6.2f", kp); // xxx.00 00415 else if(10 <= kp && kp < 100) sprintf(temp, "%6.3f", kp); // xx.000 00416 else sprintf(temp, "%6.4f", kp); // x.0000 00417 while(temp[idx] != '\0'){ // Overwrite old digits with new digits 00418 begin[idx] = temp[idx]; 00419 idx++; 00420 } 00421 00422 // Update Ki Parameter Field 00423 memset(temp, '\0', sizeof(temp)); 00424 idx = 0; 00425 begin = strstr(webpage, "name=\"ki_input\" value=\"") + 00426 sizeof("name=\"ki_input\" value="); // Points to start of ki_output field 00427 // Determine precision of float such temp string has no empty spaces; 00428 // i.e. each space must have a value or a decimal point, other wise webbrowser may not recognize value 00429 if(ki >= 100) sprintf(temp, "%6.2f", ki); // xxx.00 00430 else if(10 <= ki && ki < 100) sprintf(temp, "%6.3f", ki); // xx.000 00431 else sprintf(temp, "%6.4f", ki); // x.0000 00432 while(temp[idx] != '\0'){ // Overwrite old digits with new digits 00433 begin[idx] = temp[idx]; 00434 idx++; 00435 } 00436 00437 // Update Kd Parameter Field 00438 memset(temp, '\0', sizeof(temp)); 00439 idx = 0; 00440 begin = strstr(webpage, "name=\"kd_input\" value=\"")+ 00441 sizeof("name=\"kd_input\" value="); // Points to start of kd_output field 00442 // Determine precision of float such temp string has no empty spaces; 00443 // i.e. each space must have a value or a decimal point, other wise webbrowser may not recognize value 00444 if(kd >= 100) sprintf(temp, "%6.2f", kd); // xxx.00 00445 else if(10 <= kd && kd < 100) sprintf(temp, "%6.3f", kd); // xx.000 00446 else sprintf(temp, "%6.4f", kd); // x.0000 00447 while(temp[idx] != '\0'){ // Overwrite old digits with new digits 00448 begin[idx] = temp[idx]; 00449 idx++; 00450 } 00451 00452 // Update Setpoint Parameter 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 or neg sign, 00455 // other wise webbrowser may not recognize value 00456 memset(temp, '\0', sizeof(temp)); 00457 idx = 0; 00458 begin = strstr(webpage, "name=\"setpoint_input\" value=\"")+ 00459 sizeof("name=\"setpoint_input\" value="); // Points to start of kp_output field 00460 // Determine precision of float such temp string has no empty spaces; 00461 // i.e. each space must have a value or a decimal point, other wise webbrowser may not recognize value 00462 if(setpoint >= 0.00){ 00463 if(setpoint >= 100) sprintf(temp, "%6.3f", setpoint); // xxx.000 00464 else if(10 <= setpoint && setpoint < 100) sprintf(temp, "%7.4f", setpoint); // xx.0000 00465 else sprintf(temp, "%6.5f", setpoint); // x.00000 00466 } 00467 else{ 00468 if(setpoint <= -100) sprintf(temp, "%6.2f", setpoint); // -xxx.00 00469 else if(-100 < setpoint && setpoint <= -10) sprintf(temp, "%6.3f", setpoint); // -xx.000 00470 else sprintf(temp, "%6.4f", setpoint); // -x.0000 00471 } 00472 while(temp[idx] != '\0'){ // Overwrite old digits with new digits 00473 begin[idx] = temp[idx]; 00474 idx++; 00475 } 00476 } 00477 00478 /* 00479 parse_input() take a char*, in particular a pointer to Webpage User 00480 Input Data, for example: 00481 char str[] = "+IPD,0,44:kp_input=0.12&ki_input=14.25&kd_input=125.42"; 00482 00483 and parses out the Setpoint Kp, Ki, Kd values that the user entered 00484 and posted in the webpage. Values are converted to floats and 00485 assigned to the given argurments. 00486 00487 NOTE: THIS IS WEBPAGE SPECIFIC!!!! CHANGE THE CODE IN HERE TO SUITE THE 00488 SPECIFIC APPLICATION WEBPAGE!!! THESE EXTRACTED VALUES WILL BE USED IN 00489 THE update_webpage() function. MAKE SURE THESE TWO FUNCTIONS INTEGRATE 00490 PROPERLY!!! 00491 */ 00492 void parse_input(char* webpage_user_data, bool* control_mode, float *setpoint, float* kp, float* ki, float* kd){ 00493 char keys[] = {'&', '\0'}; 00494 00495 // Parse out user input values 00496 char input_buff[50]; 00497 char* begin; 00498 char* end; 00499 00500 // Parse and Update Control Mode Value 00501 memset(input_buff, '\0', sizeof(input_buff)); // Null out input buff 00502 begin = strstr(webpage_user_data, "control_mode=") + 00503 sizeof("control_mode"); // Points to start of setpoint_input value 00504 end = begin + strcspn(begin, keys); // Points to end of setpoint_input value 00505 for(long i = 0; i < end - begin; i++){ // Parse out the value one char at a time 00506 input_buff[i] = begin[i]; 00507 } 00508 *control_mode = atoi(input_buff); 00509 00510 // Parse and Update Setpoint Value 00511 memset(input_buff, '\0', sizeof(input_buff)); // Null out input buff 00512 begin = strstr(webpage_user_data, "setpoint_input=") + 00513 sizeof("setpoint_input"); // Points to start of setpoint_input value 00514 end = begin + strcspn(begin, keys); // Points to end of setpoint_input value 00515 for(long i = 0; i < end - begin; i++){ // Parse out the value one char at a time 00516 input_buff[i] = begin[i]; 00517 } 00518 *setpoint = atof(input_buff); 00519 00520 // Parse and Update Kp Value 00521 memset(input_buff, '\0', sizeof(input_buff)); // Null out input buff 00522 begin = strstr(webpage_user_data, "kp_input=") + 00523 sizeof("kp_input"); // Points to start of kp_input value 00524 end = begin + strcspn(begin, keys); // Points to end of kp_input value 00525 for(long i = 0; i < end - begin; i++){ // Parse out the value one char at a time 00526 input_buff[i] = begin[i]; 00527 } 00528 *kp = atof(input_buff); 00529 00530 // Parse and Update Ki Value 00531 memset(input_buff, '\0', sizeof(input_buff)); // Null out input buff 00532 begin = strstr(webpage_user_data, "ki_input=") + 00533 sizeof("ki_input"); // Points to start of ki_input value 00534 end = begin + strcspn(begin, keys); // Points to end of ki_input value 00535 for(long i = 0; i < end - begin; i++){ // Parse out the value one char at a time 00536 input_buff[i] = begin[i]; 00537 } 00538 *ki = atof(input_buff); 00539 00540 // Parse and Update Kd Value 00541 memset(input_buff, '\0', sizeof(input_buff)); // Null out input buff 00542 begin = strstr(webpage_user_data, "kd_input=") + 00543 sizeof("kd_input"); // Points to start of kd_input value 00544 end = begin + strcspn(begin, keys); // Points to end of kd_input value 00545 for(long i = 0; i < end - begin; i++){ // Parse out the value one char at a time 00546 input_buff[i] = begin[i]; 00547 } 00548 *kd = atof(input_buff); 00549 } 00550 00551 void pid_callback(){ 00552 // If control_mode is POSITION run position pid 00553 if(control_mode == POSITION){ 00554 // Update motor 00555 if(output >= 0.0) mtr_dir = 1; // Set direction to sign of output 00556 else mtr_dir = 0; 00557 mtr_pwm = abs(output); // Apply motor output 00558 00559 // Update feedback 00560 feedback = encoder.read()*FEEDBACK_SCALE;// Scale feedback to num wheel revs 00561 } 00562 // else control_mode must be SPEED, run speed pid 00563 else{ 00564 if(setpoint >= 0.0) mtr_dir = 1; // Set motor direction based on setpoint 00565 else mtr_dir = 0; 00566 if(-0.001 < setpoint && setpoint < 0.001){ 00567 /* Setpoint = 0 is a special case, we allow output to control speed AND 00568 direction to fight intertia and/or downhill roll. */ 00569 if(output >= 0.0) mtr_dir = 1; 00570 else mtr_dir = 0; 00571 mtr_pwm = abs(output); 00572 } 00573 else{ 00574 if(mtr_dir == 1){ // If CW then apply positive outputs 00575 if(output >= 0.0) mtr_pwm = output; 00576 else mtr_pwm = 0.0; 00577 } 00578 else{ // If CCW then apply negative outputs 00579 if(output <= 0.0) mtr_pwm = abs(output); 00580 else mtr_pwm = 0.0; 00581 } 00582 } 00583 float k = Ts/2.0; // Discrete time, (Ts/2 because this callback is called 00584 // at interval of Ts/2... or twice as fast as pid controller) 00585 00586 /* TODO: Implement a "rolling"/"moving" average */ 00587 static int last_count = 0; 00588 int count = encoder.read(); 00589 float raw_speed = ((count - last_count)*FEEDBACK_SCALE) / k; 00590 float rpm_speed = raw_speed * 60.0; // Convert speed to RPM 00591 00592 last_count = count; // Save last count 00593 feedback = rpm_speed; 00594 } 00595 } 00596 00597 /* 00598 Clips value to lower/ uppper 00599 @param value The value to clip 00600 @param lower The mininum allowable value 00601 @param upper The maximum allowable value 00602 @return The resulting clipped value 00603 */ 00604 float clip(float value, float lower, float upper){ 00605 return std::max(lower, std::min(value, upper)); 00606 } 00607 00608 /**************************WEB PAGE TEXT**************************************/ 00609 /***************************************************************************** 00610 Copy and past text below into a html file and save as the given file name to 00611 your SD card. 00612 00613 file name: pid_dual.html 00614 00615 html text: 00616 00617 <!DOCTYPE html> 00618 <html> 00619 <head> 00620 <title>PID Motor Control</title> 00621 </head> 00622 <body> 00623 <h1>PID Motor Control</h1> 00624 <h2>Motor Status</h2> 00625 <p> 00626 <form title="Motor Status"> 00627 <input type="text" value="Some user information" size="25" readonly /><br> 00628 Current Setpoint: 00629 <input type="number" name="current_setpoint" value="0000.00" readonly /><br> 00630 Current Position: 00631 <input type="number" name="current_position" value="0000.00" readonly /><br> 00632 </form> 00633 </p> 00634 <h2>PID Status</h2> 00635 <form title="User Input" method="post"> 00636 PID Controls: <br> 00637 <input type="radio" name="control_mode" value="0"checked>Position(#Revolutions) 00638 <br> 00639 <input type="radio" name="control_mode" value="1" >Speed(RPM) 00640 <br> 00641 Setpoint: 00642 <input type="number" name="setpoint_input" value="0000.00" step="0.0000001" size="6" /><br> 00643 Proportional Gain: (Good Starting Values: Position = 2.50 Speed = 0.01)<br> 00644 <input type="number" name="kp_input" value="002.50" step="0.0000001" size="6" /><br> 00645 Integral Gain: (Good Starting Values: Position = 5.0 Speed = 0.015)<br> 00646 <input type="number" name="ki_input" value="005.00" step="0.0000001" size="6" /><br> 00647 Derivative Gain: (Good Starting Values: Position = 0.25 Speed = 0.0001)<br> 00648 <input type="number" name="kd_input" value="000.25" step="0.0000001" size="6" /><br> 00649 <br> 00650 <input type="submit" value="Update" /> 00651 </form> 00652 </body> 00653 </html> 00654 00655 *****************************************************************************/ 00656 /*****************************************************************************/
Generated on Thu Jul 14 2022 22:54:06 by
1.7.2