main.cpp

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