Diff: main.cpp

Revision:

0:6f4cd2c49f65

Child:

1:d69f57dcde02

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Nov 25 22:13:26 2015 +0000
@@ -0,0 +1,653 @@
+/*
+    Uses the ESP8266 WiFi Chip to set up a WiFi Webserver used to control 
+    the position 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)
+
+// 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/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
+            #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.2f\nKp: %6.2f\nKi: %6.2f\nKd: %6.2f\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.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");
+                    #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>
+
+*****************************************************************************/
+/*****************************************************************************/
\ No newline at end of file