File content as of revision 1:d4a95e3a8aeb:

/*
    Uses the ESP8266 WiFi Chip to set up a WiFi Webserver used to control 
    the speed of a Sparkfun Redbot Rotbot. 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. 
    
    Reference/ Sources: This code is based on 
    https://developer.mbed.org/users/4180_1/notebook/using-the-esp8266-with-the-mbed-lpc1768/
*/

#include "mbed.h"
#include "SDFileSystem.h"
#include "PID.h"
#include "HALLFX_ENCODER.h"
#include "MotorDriver.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 kp, ki, kd;                               // Working gain vars
float working_setpoint;                         // Used for web parsing and updating
float setpoint;                                 // This is used by PID objects, because 
                                                // encoders are not quadrature and 
                                                // do not have direction information
                                                // we use this to store the absolute
                                                // value of the working_setpoint.
                                                // Specifically setpoint is used only
                                                // for PID objects. Refer to 
                                                // pid_callback() function
float feedbackL, outputL;                       // Should these be volatile?
float feedbackR, outputR;                       // Should these be volatile? 
const float output_lower_limit = 0.0;          
const float output_upper_limit = 1.0;
const float FEEDBACK_SCALE = 1.0/384.0;         // Scale feedback to 1rev/3000cnts
                                                // this is encoder specific.
enum CONTROL_MODE{PID_OFF = 0, PID_ON = 1};
int control_mode = PID_ON;
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

const float kp_init = 0.01;        // Good Kp for Speed Control; Start with this
const float ki_init= 0.015;        // Good Ki for Speed Control; Start with this
const float kd_init = 0.0001;      // Good Kd for Speed Control; Start with this

PID pidL(&setpoint, &feedbackL, &outputL,
        output_lower_limit, output_upper_limit,
        kp_init, ki_init, kd_init, Ts); 
PID pidR(&setpoint, &feedbackR, &outputR,
        output_lower_limit, output_upper_limit, 
        kp_init, ki_init, kd_init, Ts);
MotorDriver mtrR(p20, p19, p25, 10000.0, true); // in1, in2, pwm, pwmFreq, isBrakeable
MotorDriver mtrL(p18, p17, p24, 10000.0, true); // in1, in2, pwm, pwmFreq, isBrakeable
HALLFX_ENCODER encR(p21);
HALLFX_ENCODER encL(p22);
void pid_callback();            // Updates encoder feedback and motor output
Ticker motor;                   // Interrupt for feedback and motor updates
/*****************************************************************************/
/*****************************************************************************/                

/**********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 working_setpoint, float kp, float ki, float kd);
void parse_input(char* webpage_user_data, int* control_mode, float* working_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_bot.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, working_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*****************************************************/
    /***********************************************************************************/
    working_setpoint = 0.0;
    encL.reset();
    encR.reset();
    feedbackL = encL.read();
    feedbackR = encR.read();
     
    // 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
    pidL.start();
    pidR.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, &working_setpoint, &kp, &ki, &kd);
                    working_setpoint = clip(working_setpoint, -90.0, 90.0); // Redbot motors are ~90 RPM; max html field size for setpoint is 7 (to accomodate for a "-" sign
                    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)
                    
                    #ifdef DEBUG
                    printf("User Entered: \ncontrol_mode: %i\nSetpoint: %7.4f\nKp: %6.4f\nKi: %6.4f\nKd: %6.4f\n", 
                            control_mode, working_setpoint, kp, ki, kd);
                    #endif                    
                     
                    pidL.set_parameters(kp, ki, kd, Ts);    // Updata PID params 
                    pidR.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",
                            pidL.getKp(), pidL.getKi(), pidL.getKd(), pidL.getTs());
                    printf("Setpoint: %1.4f\r\n", working_setpoint);
                    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, working_setpoint, kp, ki, kd);
                    else isFirstRequest = false; // First Request just send page with initial values
                    
                    #ifdef DEBUG
                    printf(webpage); 
                    #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 working_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 == PID_OFF) sprintf(temp, "%s", "checked");// If PID OFF 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 == PID_ON) sprintf(temp, "%s", "checked"); // If PID ON 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(working_setpoint >= 0.00){
        if(working_setpoint >= 100) sprintf(temp, "%6.3f", working_setpoint);                           // xxx.000
        else if(10 <= working_setpoint && working_setpoint < 100) sprintf(temp, "%7.4f", working_setpoint);     // xx.0000
        else sprintf(temp, "%6.5f", working_setpoint);                                          // x.00000
    }
    else{
        if(working_setpoint <= -100) sprintf(temp, "%6.2f", working_setpoint);                          // -xxx.00
        else if(-100 < working_setpoint && working_setpoint <= -10) sprintf(temp, "%6.3f", working_setpoint);   // -xx.000
        else sprintf(temp, "%6.4f", working_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, int* control_mode, float *working_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];        
    }
    *working_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);
    
    // Parse for stop button press; we only have to see if "STOP" exists in the input buffer 
    // because it is just a button and will not be included unless it is pressed!... Makes 
    // our job easy!... if stop was pressed then set setpoint to zero!
    if(strstr(webpage_user_data, "STOP") != NULL) *working_setpoint = 0;
}

void pid_callback(){
    static int lastMode = control_mode;
    
    // If control_mode is PID_OFF turn off PID controllers and run motors in open loop
    if(control_mode == PID_OFF){
        // Mode changed; turn off PID controllers
        if(lastMode != control_mode){
            pidL.stop(); pidR.stop();
            lastMode = PID_OFF;
        }
        // Set motor speed in open loop mode
        // Motor direction based on working setpoint var
        int dirL, dirR;
        if(working_setpoint < 0.0){
             dirL = -1; dirR = -1;
        }
        else{
            dirL = 1; dirR = 1;
        }
        float speed = abs(working_setpoint) / 90.0; // Normalize based on 90 RPM
        mtrL.forceSetSpeed(speed * dirL);
        mtrR.forceSetSpeed(speed * dirR);
    }
    // else control_mode is PID_ON, turn on PID controllers and run motors in closed loop
    else{
        // Mode changed; turn on PID controllers
        if(lastMode != control_mode){
            pidL.start(); pidR.start();
            lastMode = PID_ON;
        }
        // Deal with feedback and update motors
        // Motor direction based on working setpoint var
        int dirL, dirR;
        if(working_setpoint < 0.0){
             dirL = -1; dirR = -1;
        }
        else{
            dirL = 1; dirR = 1;
        }
        
        // Setpoint vars used by PID objects are concerned with 
        // only SPEED not direction. 
        setpoint = abs(working_setpoint);
    
        float k = Ts_PID_CALLBACK;    // Discrete time, (Ts/2 because this callback is called
                                      // at interval of Ts/2... or twice as fast as pid controller)
        static int last_count_L = 0;
        static int last_count_R = 0;
        int countL = encL.read();
        int countR = encR.read();
        
        // Because encoders are not quadrature we must handle the sign outselves, 
        // i.e. explicitly make calcs based on the direction we have set the motor
        float raw_speed_L = ((countL - last_count_L)*FEEDBACK_SCALE) / k; 
        float rpm_speed_L = raw_speed_L * 60.0;     // Convert speed to RPM
        
        float raw_speed_R = ((countR - last_count_R)*FEEDBACK_SCALE) / k; 
        float rpm_speed_R = raw_speed_R * 60.0;     // Convert speed to RPM
        
        last_count_L = countL;                      // Save last count
        last_count_R = countR;
        feedbackL = rpm_speed_L;
        feedbackR = rpm_speed_R;
        
        mtrL.forceSetSpeed(outputL * dirL);
        mtrR.forceSetSpeed(outputR * dirR);
    }
}

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

html text:

<!DOCTYPE html>
<html>
<head>
<title>PID RedBot Control</title>
</head>
<body>
<h1>PID Motor Control</h1>
<h2>PID Status</h2>
<form title="User Input" method="post">
PID Controls: <br>
<input type="radio" name="control_mode" value="0"checked>PID OFF
<br>
<input type="radio" name="control_mode" value="1"       >PID ON
<br>
Setpoint:<br> 
<input type="number" name="setpoint_input" value="0000.00" step="0.0000001" size="6"  /><br>
Proportional Gain: (Good Starting Value: 0.01)<br>
<input type="number" name="kp_input" value="000.01" step="0.0000001" size="6"  /><br>
Integral Gain: (Good Starting Value: 0.015)<br>
<input type="number" name="ki_input" value="00.015" step="0.0000001" size="6"  /><br>
Derivative Gain: (Good Starting Value: 0.0001)<br>
<input type="number" name="kd_input" value="0.0001" step="0.0000001" size="6"  /><br>
<br>
<input type="submit" value="Update" />
<input type="submit" name="STOP" value="STOP!" />
</form>
</body>
</html>

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