File content as of revision 4:05e4ebafbcaf:

/*
    Uses the ESP8266 WiFi Chip to set up a WiFi Webserver used to control 
    the 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: Move debugging printf statements inside of #ifdef DEBUG
    statements, so that serial terminal print statements can be 
    turned on and off easily for debugging.
    
    TODO: Implement stop button in webpage
*/


#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********************************/
/*****************************************************************************/
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.
// Init Webpage to these values                                                
const float setpoint_init = 0.0;
const float kp_init = 0.01;
const float ki_init = 0.015;
const float kd_init = 0.0001;
const float Ts_init = 0.04;                    // 25Hz Sample Freq (40ms Sample Time)
const float Ts_PID_CALLBACK = Ts_init/2.0;     // Update Motors and sensers twice as 
                                               // fast as PID sample rate, ensures
                                               // PID feedback is upto date every 
                                               // time PID calculations run

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_spd.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_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
//            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); // -999.99 is max size that can be updated to webpage, i.e. field is 7 digits (see html)
                    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)
                    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)
                    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)
//                    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(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_init/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_spd.html

html text:

<!DOCTYPE html>
<html>
<head>
<title>PID Motor Speed Control</title>
</head>
<body>
<h1>PID Motor Speed 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 (RPM): 
<input type="number" name="setpoint_input" value="0000.00" step="0.01" size="6"  /><br>
Proportional Gain:
<input type="number" name="kp_input" value="000.01" step="0.0001" size="6"  /><br>
Integral Gain:
<input type="number" name="ki_input" value="000.01" step="0.0001" size="6"  /><br>
Derivative Gain:
<input type="number" name="kd_input" value="000.00" step="0.0001" 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>


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