File content as of revision 1:26a13ee574e9:

/*
    Uses the ESP8266 WiFi Chip to set up a WiFi Webserver used to control 
    the position of a motor using a PID controller.

    NOTE: 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.
    
    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. 

    TODO: CREATE FUCNTIONS FOR WIFI AND SERVER!!! GET RID OF "MAGIC NUMBER"
    MAKE THIS AWESOME!!!!
    
    TODO: COMMENT AND DOCUMENT THE HELL OUT OF THIS PROGRAM!!!
*/


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

/**********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, 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.
/*****************************************************************************/
/*****************************************************************************/

/*********PID CONTROLLER SPECIFIC DECLARATIONS********************************/
/*****************************************************************************/
static float setpoint, feedback, output;
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.
const float setpoint_init = 0.0;
const float kp_init = 2.5;
const float ki_init = 5.0;
const float kd_init = 0.5;
const float Ts_init = 0.04;                   // 25Hz Sample Freq (40ms Sample Time)

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

// 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_pos.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

//    printf("Webpage Data Size: %d byte\r\n", num_chars);

    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);   
    /***********************************************************************************/
    /***********************************************************************************/
    
    /***************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.
    led =1;
    /***********************************************************************************/
    /***********************************************************************************/
    
    /************Initialize the PID*****************************************************/
    /***********************************************************************************/
    // Working paramater variables
    setpoint = setpoint_init;
    encoder.reset();
    feedback = encoder.read();
    float kp = kp_init;
    float ki = ki_init;
    float kd = kd_init;
    pid.set_parameters(kp, ki, kd, Ts_init);
    
    // Init the motor
    mtr_dir = 0;            // Can be 0 or 1, sets the direction
    mtr_pwm = 0.0;
     
    // Clear encoder count
    encoder.reset();
    
    // 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_init/2.0);
    
    // 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
//            printf("\r\n*************WORKING BUFFER******************************\r\n");
            printf(buff); printf("\n");
//            printf("\r\n**************END WORKING BUFFER**************************\r\n");
            
            // 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);
//                printf("ID: %i\nLen: %i\nType: %s\n", id, len, type);
                
                // If GET or POST request "type" parse and update user input then send webpage
                if(strstr(type, "GET") != NULL || strstr(type, "POST") != NULL){
//                    printf("I got web request\n");
                   
                    /* 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
                    */
                    printf("\r\n*************USER INPUT**********************************\r\n");
                    parse_input(buff, &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);
                    printf("User Entered: \nSetpoint: %7.2f\nKp: %6.2f\nKi: %6.2f\nKd: %6.2f\n", setpoint, kp, ki, kd);
                    pid.set_parameters(kp, ki, kd, Ts_init);    // Updata PID params 
                    printf("Updated to Kp: %1.2f Ki: %1.2f Kd: %1.2f Ts: %1.2f\r\n",
                            pid.getKp(), pid.getKi(), pid.getKd(), pid.getTs());
                    printf("Setpoint: %1.2f\r\n", setpoint);
                    printf("Output: %1.2f\r\n", output);
                    printf("\r\n*************END USER INPUT******************************\r\n");
                    
                    // 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
                    printf(webpage); // DEBUGGING ONLY!!! REMOVE FOR RELEASE!!!
                    
                    // 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>"*/
//                    printf(buff); printf("\n");
                   
                    // 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);
  //                      printf(buff); printf("\n");
                        
                        esp.printf("AT+CIPSERVER=1,%d\r\n", port);
                        getreply(500, buff, sizeof(buff), 10);
 //                       printf(buff); printf("\n");
                    }
                    else{
                        esp.printf("AT+CIPCLOSE=%d\r\n", id);  // Notice id is an int formatted to string
                        getreply(500, buff, sizeof(buff), 24);
//                        printf(buff); printf("\n");
                    }
                }       
            }
        }
        /*********************************************************************/
        /*********************************************************************/
        
        
    }                      
}

// 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
        
//        wait(1);
        
        // BWW: Set ESP8266 Server Timeout
        esp.printf("AT+CIPSTO=%d\r\n", serverTimeout_secs);
        getreply(500, buffer, size, 50);    // BWW: Wait for reply
        
//        wait(5);
        
        // 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 setpoint kp, ki, kd values
    char* begin; 
//    char* end;
    char temp[8];
    int idx;
    
    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
//    end = begin + 5;                                                      // Points to end of kp_output value 
    // 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++;
    }
    
    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
//    end = begin + 5;                                                      // Points to end of ki_output 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++;
    }
    
    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
//    end = begin + 5;                                                      // Points to end of kd_output 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++;
    }
    
    // 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
//    end = begin + 6;                                                      // Points to end of kp_output value. +6 to accomadate negative sign 
    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,float *setpoint, float* kp, float* ki, float* kd){
    char keys[] = {'&', '\0'};
      
    // Parse out user input values
    char input_buff[50]; 
    char* begin;
    char* end;
    printf("**************Parsing**************\n");
    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];        
    }
    printf("Setpoint Parsed Data: %s\n", input_buff);
    *setpoint = atof(input_buff);
    
    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];
    }
    printf("Kp Parsed Data: %s\n", input_buff);
    *kp = atof(input_buff);
    
    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];
    }
    printf("Ki Parsed Data: %s\n", input_buff); 
    *ki = atof(input_buff);
    
    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];  
    }
    printf("Kd Parsed Data: %s\n", input_buff);
    *kd = atof(input_buff);
    printf("**********End Parsing***************\n");
}

void pid_callback(){
    // 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  
}

/*
    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_pos.html

html text:

<!DOCTYPE html>
<html>
<head>
<title>PID Motor Position Control</title>
</head>
<body>
<h1>PID Motor Position 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>
Setpoint: 
<input type="number" name="setpoint_input" value="0000.00" step="0.01" size="6"  /><br>
Proportional Gain:
<input type="number" name="kp_input" value="002.50" step="0.01" size="6"  /><br>
Integral Gain:
<input type="number" name="ki_input" value="005.00" step="0.01" size="6"  /><br>
Derivative Gain:
<input type="number" name="kd_input" value="000.25" step="0.01" size="6"  /><br>
<br>
<input type="submit" value="Submit" />
<input type="submit" name="update" value="Update">
<input type="submit" name="estop" value="STOP">
</form>
</body>
</html>


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