File content as of revision 0:11bc7a815367:

/*
    Uses the ESP8266 WiFi Chip to set up a WiFi Webserver used to control 
    the position or speed of a motor using a PID controller. USE FIREFOX 
    Web Browser

    NOTES: 
    1. Webpage Handling in this program is specific to a CUSTOM 
    WEBPAGE. Program must be modified to handle specfically a new 
    webpage. A copy of the webpage for this program can be found at 
    the end of this program page. Simply copy and past text into a
    html file and save as the given name.
    
    2. Developed and tested with FireFox 42.0 Web Browser. Does not seem to work
    well with Google Chrome or Internet Explorer for some reason... they seem 
    to generate two post requests which messes with the user input values. 
    
    3. There are a bunch of printf statements in the code that can be
    uncommented for debugging in a serial terminal progrom. 

    
    TODO: ESP8366 has a max packet send size. Make sure we implement
    a method to send webpages that exceed this value. The max size is
    listed in the official ESP8266 AT Commands Documentation, I think
    it is 2048 bytes/chars
    
    TODO: CREATE CONFIG FUNCTION TO SET SSID, PASSWORD, BAUDRATE ETC.
    Perhaps have a serial terminal method to take user input, and 
    put the function call into a #ifdef WiFiConfig statement, so 
    that the user can enable it to config Wifi module then turn 
    it off once Wifi module is configed so that this program can 
    run in a "stand alone" mode. 
    
    TODO: Implement stop button in webpage
*/


#include "mbed.h"
#include "SDFileSystem.h"
#include "PID.h"
#include "QEI.h"
#include <algorithm>

//#define DEBUG   // Uncomment for serial terminal print debugging
                // Comment out to turn prints off for normal op

/*********PID CONTROLLER SPECIFIC DECLARATIONS********************************/
/*****************************************************************************/

float setpoint, feedback, output;               // Should these be volatile? 
const float output_lower_limit = -1.0;          
const float output_upper_limit = 1.0;
const float FEEDBACK_SCALE = 1.0/3000.0;        // Scale feedback to 1rev/3000cnts
                                                // this is encoder specific.
enum CONTROL_MODE{POSITION = 0, SPEED = 1};
bool control_mode = POSITION;
float kp, ki, kd;           // Gain variables for working
const float Ts = 0.04;                         // 25Hz Sample Freq (40ms Sample Time)
const float Ts_PID_CALLBACK = Ts/2.0;          // Update Motors and sensers twice as 
                                               // fast as PID sample rate, ensures
                                               // PID feedback is upto date every 
                                               // time PID calculations run

// Vars to store gains for Speed and Position when switching Control modes
const float kp_init = 2.5;         // Good Kp for Position Control
const float ki_init = 5.0;         // Good Ki for Position Control
const float kd_init = 0.25;        // Good Kd for Position Control

PID pid(&setpoint, &feedback, &output,
        output_lower_limit, output_upper_limit,
        kp_init, ki_init, kd_init, Ts); // Init for position control
QEI encoder(p15, p16);
PwmOut mtr_pwm(p25);
DigitalOut mtr_dir(p24);
void pid_callback();            // Updates encoder feedback and motor output
Ticker motor;
/*****************************************************************************/
/*****************************************************************************/                

/**********WEB SERVER SPECIFIC DECLARTATIONS**********************************/
/*****************************************************************************/
SDFileSystem sd(p5,p6,p7,p8,"sd");  // MOSI, MISO, SCLK, CS, 
                                    // Virtual File System Name
Serial esp(p13, p14);           // tx, rx
DigitalOut  espRstPin(p26);     // ESP Reset
DigitalOut led(LED4);

Timer t1;
Timer t2;

void init(char* buffer, int size);
void getreply(int timeout_ms, char* buffer, int size, int numBytes);
void startserver(char* buffer, int size);
void update_webpage(char* webpage, float setpoint, float kp, float ki, float kd);
void parse_input(char* webpage_user_data, bool* control_mode, float* setpoint, float* kp, float* ki, float* kd);
int port        =80;         // set server port
int serverTimeout_secs =5;   // set server timeout in seconds in case 
                             // link breaks.
/*****************************************************************************/
/*****************************************************************************/

// Common Application Declarations
Serial pc(USBTX, USBRX);
float clip(float value, float lower, float upper);

int main()
{
    printf("Starting\n");
    
    /****************** Load Webpage from SD Card***************************************/
    /***********************************************************************************/
    char file[] = "/sd/pid_dual.html";
    
    // Get file size so we can dynamically allocate buffer size 
    int num_chars = 0;
    FILE *fp = fopen(file, "r");
    while(!feof(fp)){
        fgetc(fp);
        num_chars++;
    } 
    rewind(fp);                     // Go to beginning of file

    #ifdef DEBUG
    printf("Webpage Data Size: %d byte\r\n", num_chars);
    #endif

    const int WEBPAGE_SIZE = num_chars;
    char webpage[WEBPAGE_SIZE];                   
    webpage[0] = NULL;               // Init our array so that element zero contains a null
                                    // This is important, ensures strings are placed into
                                    // buffer starting at element 0... not some random 
                                    // elment                                           
    // Read in and buffer file to memory
    if(fp == NULL){
         printf("Error: No Such File or something :(");
         return 1;
    }
    else{
        while(!feof(fp)){
            fgets(webpage + strlen(webpage), WEBPAGE_SIZE, fp);  // Get a string from stream, add to buffer
        }
    }
    fclose(fp);
    printf("Webpage Buffer Size: %d bytes\r\n", sizeof(webpage));
    update_webpage(webpage, setpoint, kp_init, ki_init, kd_init);  // Update Webpage for 
                                                                   // Position Mode   
    /***********************************************************************************/
    /***********************************************************************************/
    
    /***************BRING UP SERVER*****************************************************/
    /***********************************************************************************/
    char buff[5000];                // Working buffer
    init(buff, sizeof(buff));       // Init ESP8266
    
    esp.baud(115200);               // ESP8266 baudrate. Maximum on KLxx' is 115200, 230400 works on K20 and K22F
  
    startserver(buff, sizeof(buff));      // Configure the ESP8266 and Setup as Server

    printf(buff);                   // If start successful buff contains IP address...
                                    // if not if contains an error.
    /***********************************************************************************/
    /***********************************************************************************/
    
    /************Initialize the PID*****************************************************/
    /***********************************************************************************/
    setpoint = 0.0;
    encoder.reset();
    feedback = encoder.read();
    
    // Init the motor
    mtr_dir = 0;            // Can be 0 or 1, sets the direction
    mtr_pwm = 0.0;
     
    // Update sensors and feedback twice as fast as PID sample time
    // this makes pid react in real-time avoiding errors due to 
    // missing counts etc. 
    motor.attach(&pid_callback, Ts_PID_CALLBACK);
    
    // Start PID sampling
    pid.start();
    
    /***********************************************************************************/
    /***********************************************************************************/
    
    while(1){
        /**************SERVICE WEBPAGE******************************************************/
        /***********************************************************************************/
        if(esp.readable()){
            getreply(500, buff, sizeof(buff), sizeof(buff) -1); // Get full buff, leave last element for null char
            #ifdef DEBUG
            printf("\r\n*************WORKING BUFFER******************************\r\n");
            printf(buff); printf("\n");
            printf("\r\n**************END WORKING BUFFER**************************\r\n");
            #endif            
            // If Recieved Data get ID, Length, and Data
            char* rqstPnt = strstr(buff, "+IPD");
            if(rqstPnt != NULL){
                int id, len;
                char type[10]; memset(type, '\0', sizeof(type)); // Create and null out data buff
                sscanf(rqstPnt, "+IPD,%d,%d:%s ", &id, &len, type);
                #ifdef DEBUG
                printf("ID: %i\nLen: %i\nType: %s\n", id, len, type);
                #endif                
                // If GET or POST request "type" parse and update user input then send webpage
                if(strstr(type, "GET") != NULL || strstr(type, "POST") != NULL){
                    #ifdef DEBUG
                    printf("I got web request\n");
                    #endif                   
                    /* Read Webpage <Form> data sent using "method=POST"...
                       Note: Input elements in the <Form> need a set name attribute to 
                       appear in the returned HTML body. Thus to "POST" data ensure:
                       <Form method="POST"> <input type="xxx" name="xxx" value="xxx"> 
                       <input type="xxx" value="xxx"> </Form>
                       Only the input with name="xxx" will appear in body of HTML
                    */
                    #ifdef DEBUG
                    printf("\r\n*************USER INPUT**********************************\r\n");
                    #endif
                    
                    parse_input(buff, &control_mode, &setpoint, &kp, &ki, &kd);
                    setpoint = clip(setpoint, -999.99, 999.99);
                    kp = clip(kp, 0.00, 999.99); 
                    ki = clip(ki, 0.00, 999.99); 
                    kd = clip(kd, 0.00, 999.99);
                    
                    #ifdef DEBUG
                    printf("User Entered: \ncontrol_mode: %i\nSetpoint: %7.4f\nKp: %6.4f\nKi: %6.4f\nKd: %6.4f\n", 
                            control_mode, setpoint, kp, ki, kd);
                    #endif                    
                     
                    pid.set_parameters(kp, ki, kd, Ts);    // Updata PID params 
                    
                    #ifdef DEBUG
                    printf("Updated to Kp: %1.4f Ki: %1.4f Kd: %1.4f Ts: %1.4f\r\n",
                            pid.getKp(), pid.getKi(), pid.getKd(), pid.getTs());
                    printf("Setpoint: %1.4f\r\n", setpoint);
                    printf("Output: %1.4f\r\n", output);
                    printf("\r\n*************END USER INPUT******************************\r\n");
                    #endif
                                        
                    // Update Webpage to reflect new values POSTED by client
                    static bool isFirstRequest = true;
                    if(!isFirstRequest) update_webpage(webpage, setpoint, kp, ki, kd);
                    else isFirstRequest = false; // First Request just send page with initial values
                    
                    #ifdef DEBUG
                    printf(webpage); // DEBUGGING ONLY!!! REMOVE FOR RELEASE!!!
                    #endif
                    
                    // Command TCP/IP Data Tx
                    esp.printf("AT+CIPSEND=%d,%d\r\n", id, strlen(webpage));
                    getreply(200, buff, sizeof(buff), 15); /*TODO: Wait for "OK\r\n>"*/
                    
                    #ifdef DEBUG
                    printf(buff); printf("\n");
                    #endif
                    
                    // Send webpage
//                    while(!esp.writeable());         // Wait until esp ready to send data
                    int idx = 0;
                    while(webpage[idx] != '\0'){
                        esp.putc(webpage[idx]);
                        idx++;
                    }
                   
                    // Check status - Success: close channel and update PID controller, Error: reconnect 
                    bool weberror = true;
                    t2.reset(); t2.start();
                    while(weberror ==1 && t2.read_ms() < 5000){
                        getreply(500, buff, sizeof(buff), 24);
                        if(strstr(buff, "SEND OK") != NULL) weberror = false;
                    }
                    if(weberror){
                        esp.printf("AT+CIPMUX=1\r\n");
                        getreply(500, buff, sizeof(buff), 10);
                        #ifdef DEBUG
                        printf(buff); printf("\n");
                        #endif
                        esp.printf("AT+CIPSERVER=1,%d\r\n", port);
                        getreply(500, buff, sizeof(buff), 10);
                        #ifdef DEBUG
                        printf(buff); printf("\n");
                        #endif
                    }
                    else{
                        esp.printf("AT+CIPCLOSE=%d\r\n", id);  // Notice id is an int formatted to string
                        getreply(500, buff, sizeof(buff), 24);
                        #ifdef DEBUG
                        printf(buff); printf("\n");
                        #endif
                    }
                }       
            }
        }
        /*********************************************************************/
        /*********************************************************************/
        
        
    }                      
}

// Initialize ESP8266
void init(char* buffer, int size){
    // Hardware Reset ESP 
    espRstPin=0;
    wait(0.5);
    espRstPin=1;
    // Get start up junk from ESP8266
    getreply(6000, buffer, size, 500);
}

// Get Command and ESP status replies
void getreply(int timeout_ms, char* buffer, int size, int numBytes)
{
    memset(buffer, '\0', size);     // Null out buffer
    t1.reset();
    t1.start();
    int idx = 0;
    while(t1.read_ms()< timeout_ms && idx < numBytes) {
        if(esp.readable()) {
            buffer[idx] = esp.getc();
            idx++;
        }
    }
    t1.stop();
}

// Starts and restarts webserver if errors detected.
void startserver(char* buffer, int size)
{
    esp.printf("AT+RST\r\n");       // BWW: Reset the ESP8266          
    getreply(8000, buffer, size, 1000);                                            

    if (strstr(buffer, "OK") != NULL) {
        // BWW: Set ESP8266 for multiple connections
        esp.printf("AT+CIPMUX=1\r\n");
        getreply(500, buffer, size, 20);
        
        // BWW: Set ESP8266 as Server on given port
        esp.printf("AT+CIPSERVER=1,%d\r\n", port);
        getreply(500, buffer, size, 20);   // BWW: Wait for reply
                
        // BWW: Set ESP8266 Server Timeout
        esp.printf("AT+CIPSTO=%d\r\n", serverTimeout_secs);
        getreply(500, buffer, size, 50);    // BWW: Wait for reply
        
        // BWW: Request IP Address from router for ESP8266 
        int weberror = 0;
        while(weberror==0) {
            esp.printf("AT+CIFSR\r\n");
            getreply(2500, buffer, size, 200);
            if(strstr(buffer, "0.0.0.0") == NULL) {
                weberror=1;   // wait for valid IP
            }
        }
    } 
    // else ESP8266 did not reply "OK" something is messed up
    else {
        strcpy(buffer, "ESP8266 Error\n");
    }
}

/*
    update_webpage() updates output fields based on webpage user inputs "POSTED"
    Preconditions: webpage[] must have the following elements
        "kp_output"  value="xxx.xx"
        "ki_output"  value="xxx.xx"
        "kp_output"  value="xxx.xx"
    @param webpage  Pointer to webpage char[]
    @param kp       New kp value posted by user
    @param ki       New ki value posted by user
    @param kd       New kd value posted by user
    
    NOTE: THIS IS WEBPAGE SPECIFIC!!!! CHANGE THE CODE IN HERE TO SUITE THE 
    SPECIFIC APPLICATION WEBPAGE!!! ALSO USED TO REFLECT THE CUSTOM 
    IMPLEMENTATION OF THE parse_intput() function. MAKE SURE THESE TWO FUNCTIONS 
    INTEGRATE PROPERLY!!!
*/
void update_webpage(char* webpage, float setpoint, float kp, float ki, float kd){
    // Change output value to reflect new control mode, setpoint, kp, ki, kd values
    char* begin; 
    char temp[8];
    int idx;
    
    // Update Control Mode Radio Buttons
    memset(temp, '\0', sizeof(temp));
    idx = 0;
    begin = strstr(webpage, "name=\"control_mode\" value=\"") +     
            sizeof("name=\"control_mode\" value=\"0");        // Update Control Mode Position Radio Button
    if(control_mode == POSITION) sprintf(temp, "%s", "checked");// If Position active "check" it
    else sprintf(temp, "%s", "       ");                         // else "clear" it
    while(temp[idx] !=  '\0'){                                  // Write "checked"/"        " to field
        begin[idx] = temp[idx];
        idx++;
    }
    memset(temp, '\0', sizeof(temp));
    idx = 0;
    begin = strstr(webpage, "name=\"control_mode\" value=\"") +     
            sizeof("name=\"control_mode\" value=\"0\"");        // Nav to first Control Mode Radio Button (Position)
    begin = strstr(begin, "name=\"control_mode\" value=\"") +     
            sizeof("name=\"control_mode\" value=\"1");        // Nav to second Control Mode Radio Button (Speed) 
    if(control_mode == SPEED) sprintf(temp, "%s", "checked"); // If Speed active "check" it
    else sprintf(temp, "%s", "       ");                         // else "clear" it
    while(temp[idx] !=  '\0'){                                  // Write "checked"/"        " to field
        begin[idx] = temp[idx];
        idx++;
    }
    
    // Update Kp Paramater Field
    memset(temp, '\0', sizeof(temp));
    idx = 0;
    begin = strstr(webpage, "name=\"kp_input\" value=\"") + 
            sizeof("name=\"kp_input\" value="); // Points to start of kp_output field
    // Determine precision of float such temp string has no empty spaces; 
    // i.e. each space must have a value or a decimal point, other wise webbrowser may not recognize value
    if(kp >= 100) sprintf(temp, "%6.2f", kp);                             // xxx.00
    else if(10 <= kp && kp < 100) sprintf(temp, "%6.3f", kp);             // xx.000
    else sprintf(temp, "%6.4f", kp);                                      // x.0000
    while(temp[idx] !=  '\0'){                                            // Overwrite old digits with new digits
        begin[idx] = temp[idx];
        idx++;
    }
    
    // Update Ki Parameter Field
    memset(temp, '\0', sizeof(temp));
    idx = 0;
    begin = strstr(webpage, "name=\"ki_input\" value=\"") + 
            sizeof("name=\"ki_input\" value="); // Points to start of ki_output field
    // Determine precision of float such temp string has no empty spaces; 
    // i.e. each space must have a value or a decimal point, other wise webbrowser may not recognize value 
    if(ki >= 100) sprintf(temp, "%6.2f", ki);                             // xxx.00
    else if(10 <= ki && ki < 100) sprintf(temp, "%6.3f", ki);             // xx.000
    else sprintf(temp, "%6.4f", ki);                                      // x.0000     
    while(temp[idx] !=  '\0'){                                            // Overwrite old digits with new digits
        begin[idx] = temp[idx];
        idx++;
    }
    
    // Update Kd Parameter Field
    memset(temp, '\0', sizeof(temp));
    idx = 0;
    begin = strstr(webpage, "name=\"kd_input\" value=\"")+
            sizeof("name=\"kd_input\" value="); // Points to start of kd_output field
    // Determine precision of float such temp string has no empty spaces; 
    // i.e. each space must have a value or a decimal point, other wise webbrowser may not recognize value
    if(kd >= 100) sprintf(temp, "%6.2f", kd);                             // xxx.00
    else if(10 <= kd && kd < 100) sprintf(temp, "%6.3f", kd);             // xx.000
    else sprintf(temp, "%6.4f", kd);                                      // x.0000
    while(temp[idx] !=  '\0'){                                            // Overwrite old digits with new digits
        begin[idx] = temp[idx];
        idx++;
    }
    
    // Update Setpoint Parameter Field
    // Determine precision of float such temp string has no empty spaces; 
    // i.e. each space must have a value or a decimal point or neg sign, 
    // other wise webbrowser may not recognize value
    memset(temp, '\0', sizeof(temp));
    idx = 0;
    begin = strstr(webpage, "name=\"setpoint_input\" value=\"")+
            sizeof("name=\"setpoint_input\" value="); // Points to start of kp_output field
    // Determine precision of float such temp string has no empty spaces; 
    // i.e. each space must have a value or a decimal point, other wise webbrowser may not recognize value
    if(setpoint >= 0.00){
        if(setpoint >= 100) sprintf(temp, "%6.3f", setpoint);                           // xxx.000
        else if(10 <= setpoint && setpoint < 100) sprintf(temp, "%7.4f", setpoint);     // xx.0000
        else sprintf(temp, "%6.5f", setpoint);                                          // x.00000
    }
    else{
        if(setpoint <= -100) sprintf(temp, "%6.2f", setpoint);                          // -xxx.00
        else if(-100 < setpoint && setpoint <= -10) sprintf(temp, "%6.3f", setpoint);   // -xx.000
        else sprintf(temp, "%6.4f", setpoint);                                          // -x.0000
    }
    while(temp[idx] !=  '\0'){                                            // Overwrite old digits with new digits
        begin[idx] = temp[idx];
        idx++;
    }
}

/*
    parse_input() take a char*, in particular a pointer to Webpage User 
    Input Data, for example: 
    char str[] = "+IPD,0,44:kp_input=0.12&ki_input=14.25&kd_input=125.42";
    
    and parses out the Setpoint Kp, Ki, Kd values that the user entered
    and posted in the webpage. Values are converted to floats and 
    assigned to the given argurments.
    
    NOTE: THIS IS WEBPAGE SPECIFIC!!!! CHANGE THE CODE IN HERE TO SUITE THE 
    SPECIFIC APPLICATION WEBPAGE!!! THESE EXTRACTED VALUES WILL BE USED IN 
    THE update_webpage() function. MAKE SURE THESE TWO FUNCTIONS INTEGRATE
    PROPERLY!!!
*/
void parse_input(char* webpage_user_data, bool* control_mode, float *setpoint, float* kp, float* ki, float* kd){
    char keys[] = {'&', '\0'};
      
    // Parse out user input values
    char input_buff[50]; 
    char* begin;
    char* end;
    
    // Parse and Update Control Mode Value
    memset(input_buff, '\0', sizeof(input_buff));           // Null out input buff
    begin = strstr(webpage_user_data, "control_mode=") +
            sizeof("control_mode");                         // Points to start of setpoint_input value
    end = begin + strcspn(begin, keys);                     // Points to end of setpoint_input value
    for(long i = 0; i < end - begin; i++){                  // Parse out the value one char at a time
        input_buff[i] = begin[i];        
    }
    *control_mode = atoi(input_buff);
    
    // Parse and Update Setpoint Value
    memset(input_buff, '\0', sizeof(input_buff));           // Null out input buff
    begin = strstr(webpage_user_data, "setpoint_input=") +
            sizeof("setpoint_input");                       // Points to start of setpoint_input value
    end = begin + strcspn(begin, keys);                     // Points to end of setpoint_input value
    for(long i = 0; i < end - begin; i++){                  // Parse out the value one char at a time
        input_buff[i] = begin[i];        
    }
    *setpoint = atof(input_buff);
    
    // Parse and Update Kp Value
    memset(input_buff, '\0', sizeof(input_buff));           // Null out input buff
    begin = strstr(webpage_user_data, "kp_input=") + 
            sizeof("kp_input");                             // Points to start of kp_input value
    end = begin + strcspn(begin, keys);                     // Points to end of kp_input value
    for(long i = 0; i < end - begin; i++){                  // Parse out the value one char at a time
        input_buff[i] = begin[i];
    }
    *kp = atof(input_buff);
    
    // Parse and Update Ki Value
    memset(input_buff, '\0', sizeof(input_buff));           // Null out input buff
    begin = strstr(webpage_user_data, "ki_input=") + 
            sizeof("ki_input");                             // Points to start of ki_input value
    end = begin + strcspn(begin, keys);                     // Points to end of ki_input value
    for(long i = 0; i < end - begin; i++){                  // Parse out the value one char at a time
        input_buff[i] = begin[i];
    }
    *ki = atof(input_buff);
    
    // Parse and Update Kd Value
    memset(input_buff, '\0', sizeof(input_buff));           // Null out input buff
    begin = strstr(webpage_user_data, "kd_input=") + 
            sizeof("kd_input");                             // Points to start of kd_input value
    end = begin + strcspn(begin, keys);                     // Points to end of kd_input value
    for(long i = 0; i < end - begin; i++){                  // Parse out the value one char at a time
        input_buff[i] = begin[i];  
    }
    *kd = atof(input_buff);
}

void pid_callback(){
    // If control_mode is POSITION run position pid
    if(control_mode == POSITION){
        // Update motor
        if(output >= 0.0) mtr_dir = 1;       // Set direction to sign of output 
        else mtr_dir = 0;
        mtr_pwm = abs(output);               // Apply motor output
        
        // Update feedback
        feedback = encoder.read()*FEEDBACK_SCALE;// Scale feedback to num wheel revs  
    }
    // else control_mode must be SPEED, run speed pid
    else{
        if(setpoint >= 0.0) mtr_dir = 1;       // Set motor direction based on setpoint
        else mtr_dir = 0;
        if(-0.001 < setpoint && setpoint < 0.001){ 
            /* Setpoint = 0 is a special case, we allow output to control speed AND 
            direction to fight intertia and/or downhill roll. */
            if(output >= 0.0) mtr_dir = 1;
            else mtr_dir = 0;
            mtr_pwm = abs(output);
        }
        else{    
            if(mtr_dir == 1){                      // If CW then apply positive outputs
                if(output >= 0.0) mtr_pwm = output;
                else mtr_pwm = 0.0;
            }
            else{                                // If CCW then apply negative outputs
                if(output <= 0.0) mtr_pwm = abs(output);
                else mtr_pwm = 0.0;
            }                          
        } 
        float k = Ts/2.0;                        // Discrete time, (Ts/2 because this callback is called
                                                 // at interval of Ts/2... or twice as fast as pid controller)
        
        /* TODO: Implement a "rolling"/"moving" average */
        static int last_count = 0;
        int count = encoder.read();
        float raw_speed = ((count - last_count)*FEEDBACK_SCALE) / k; 
        float rpm_speed = raw_speed * 60.0;     // Convert speed to RPM
        
        last_count = count;                     // Save last count
        feedback = rpm_speed; 
    }
}

/*
    Clips value to lower/ uppper
    @param value    The value to clip
    @param lower    The mininum allowable value
    @param upper    The maximum allowable value
    @return         The resulting clipped value
*/
float clip(float value, float lower, float upper){
    return std::max(lower, std::min(value, upper));
}

/**************************WEB PAGE TEXT**************************************/
/*****************************************************************************
Copy and past text below into a html file and save as the given file name to 
your SD card.

file name: pid_dual.html

html text:

<!DOCTYPE html>
<html>
<head>
<title>PID Motor Control</title>
</head>
<body>
<h1>PID Motor Control</h1>
<h2>Motor Status</h2>
<p>
<form title="Motor Status">
<input type="text" value="Some user information" size="25" readonly /><br>
Current Setpoint:
<input type="number" name="current_setpoint" value="0000.00" readonly /><br>
Current Position:
<input type="number" name="current_position" value="0000.00" readonly /><br>
</form>
</p>
<h2>PID Status</h2>
<form title="User Input" method="post">
PID Controls: <br>
<input type="radio" name="control_mode" value="0"checked>Position(#Revolutions)
<br>
<input type="radio" name="control_mode" value="1"       >Speed(RPM)
<br>
Setpoint: 
<input type="number" name="setpoint_input" value="0000.00" step="0.0000001" size="6"  /><br>
Proportional Gain: (Good Starting Values: Position = 2.50   Speed = 0.01)<br>
<input type="number" name="kp_input" value="002.50" step="0.0000001" size="6"  /><br>
Integral Gain: (Good Starting Values: Position = 5.0    Speed = 0.015)<br>
<input type="number" name="ki_input" value="005.00" step="0.0000001" size="6"  /><br>
Derivative Gain: (Good Starting Values: Position = 0.25    Speed = 0.0001)<br>
<input type="number" name="kd_input" value="000.25" step="0.0000001" size="6"  /><br>
<br>
<input type="submit" value="Update" />
</form>
</body>
</html>

*****************************************************************************/
/*****************************************************************************/