Bryce Williams / Mbed 2 deprecated ESP8266_pid_spd_and_pos_webserver_SDcard

PID Motor Speed & Position Control Over WiFi using ESP8266 WiFi module, US Digital E4P-100-079 Quadrature Encoder, HN-GH12-1634T 30:1 200 RPM DC Motor, and LMD18200 H-Bridge Breakout

Dependencies:   4DGL-uLCD-SE PID QEI SDFileSystem mbed

main.cpp@4:9f51c7de9ebd, 2015-11-28 (annotated)

Committer:
electromotivated
Date:
Sat Nov 28 03:16:31 2015 +0000
Revision:
4:9f51c7de9ebd
Parent:
3:7fb9c4c7e24b 
Added comments;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
electromotivated 0:6f4cd2c49f65 1 /*
electromotivated 0:6f4cd2c49f65 2 Uses the ESP8266 WiFi Chip to set up a WiFi Webserver used to control
electromotivated 1:d69f57dcde02 3 the position or speed of a motor using a PID controller. USE FIREFOX
electromotivated 1:d69f57dcde02 4 Web Browser
electromotivated 0:6f4cd2c49f65 5
electromotivated 0:6f4cd2c49f65 6 NOTES:
electromotivated 0:6f4cd2c49f65 7 1. Webpage Handling in this program is specific to a CUSTOM
electromotivated 0:6f4cd2c49f65 8 WEBPAGE. Program must be modified to handle specfically a new
electromotivated 0:6f4cd2c49f65 9 webpage. A copy of the webpage for this program can be found at
electromotivated 0:6f4cd2c49f65 10 the end of this program page. Simply copy and past text into a
electromotivated 0:6f4cd2c49f65 11 html file and save as the given name.
electromotivated 0:6f4cd2c49f65 12
electromotivated 0:6f4cd2c49f65 13 2. Developed and tested with FireFox 42.0 Web Browser. Does not seem to work
electromotivated 0:6f4cd2c49f65 14 well with Google Chrome or Internet Explorer for some reason... they seem
electromotivated 0:6f4cd2c49f65 15 to generate two post requests which messes with the user input values.
electromotivated 0:6f4cd2c49f65 16
electromotivated 0:6f4cd2c49f65 17 3. There are a bunch of printf statements in the code that can be
electromotivated 0:6f4cd2c49f65 18 uncommented for debugging in a serial terminal progrom.
electromotivated 0:6f4cd2c49f65 19
electromotivated 0:6f4cd2c49f65 20
electromotivated 0:6f4cd2c49f65 21 TODO: ESP8366 has a max packet send size. Make sure we implement
electromotivated 0:6f4cd2c49f65 22 a method to send webpages that exceed this value. The max size is
electromotivated 0:6f4cd2c49f65 23 listed in the official ESP8266 AT Commands Documentation, I think
electromotivated 0:6f4cd2c49f65 24 it is 2048 bytes/chars
electromotivated 0:6f4cd2c49f65 25
electromotivated 0:6f4cd2c49f65 26 TODO: CREATE CONFIG FUNCTION TO SET SSID, PASSWORD, BAUDRATE ETC.
electromotivated 0:6f4cd2c49f65 27 Perhaps have a serial terminal method to take user input, and
electromotivated 0:6f4cd2c49f65 28 put the function call into a #ifdef WiFiConfig statement, so
electromotivated 0:6f4cd2c49f65 29 that the user can enable it to config Wifi module then turn
electromotivated 0:6f4cd2c49f65 30 it off once Wifi module is configed so that this program can
electromotivated 0:6f4cd2c49f65 31 run in a "stand alone" mode.
electromotivated 0:6f4cd2c49f65 32
electromotivated 0:6f4cd2c49f65 33 TODO: Implement stop button in webpage
electromotivated 0:6f4cd2c49f65 34 */
electromotivated 0:6f4cd2c49f65 35
electromotivated 0:6f4cd2c49f65 36
electromotivated 0:6f4cd2c49f65 37 #include "mbed.h"
electromotivated 0:6f4cd2c49f65 38 #include "SDFileSystem.h"
electromotivated 0:6f4cd2c49f65 39 #include "PID.h"
electromotivated 0:6f4cd2c49f65 40 #include "QEI.h"
electromotivated 0:6f4cd2c49f65 41 #include <algorithm>
electromotivated 0:6f4cd2c49f65 42
electromotivated 0:6f4cd2c49f65 43 #define DEBUG // Uncomment for serial terminal print debugging
electromotivated 0:6f4cd2c49f65 44 // Comment out to turn prints off for normal op
electromotivated 0:6f4cd2c49f65 45
electromotivated 0:6f4cd2c49f65 46 /*********PID CONTROLLER SPECIFIC DECLARATIONS********************************/
electromotivated 0:6f4cd2c49f65 47 /*****************************************************************************/
electromotivated 0:6f4cd2c49f65 48
electromotivated 0:6f4cd2c49f65 49 float setpoint, feedback, output; // Should these be volatile?
electromotivated 0:6f4cd2c49f65 50 const float output_lower_limit = -1.0;
electromotivated 0:6f4cd2c49f65 51 const float output_upper_limit = 1.0;
electromotivated 0:6f4cd2c49f65 52 const float FEEDBACK_SCALE = 1.0/3000.0; // Scale feedback to 1rev/3000cnts
electromotivated 0:6f4cd2c49f65 53 // this is encoder specific.
electromotivated 0:6f4cd2c49f65 54 enum CONTROL_MODE{POSITION = 0, SPEED = 1};
electromotivated 0:6f4cd2c49f65 55 bool control_mode = POSITION;
electromotivated 2:07a3107e7664 56 float kp, ki, kd; // Gain variables for working
electromotivated 0:6f4cd2c49f65 57 const float Ts = 0.04; // 25Hz Sample Freq (40ms Sample Time)
electromotivated 2:07a3107e7664 58 const float Ts_PID_CALLBACK = Ts/2.0; // Update Motors and sensers twice as
electromotivated 2:07a3107e7664 59 // fast as PID sample rate, ensures
electromotivated 2:07a3107e7664 60 // PID feedback is upto date every
electromotivated 2:07a3107e7664 61 // time PID calculations run
electromotivated 0:6f4cd2c49f65 62
electromotivated 0:6f4cd2c49f65 63 const float kp_init = 2.5; // Good Kp for Position Control
electromotivated 0:6f4cd2c49f65 64 const float ki_init = 5.0; // Good Ki for Position Control
electromotivated 0:6f4cd2c49f65 65 const float kd_init = 0.25; // Good Kd for Position Control
electromotivated 0:6f4cd2c49f65 66
electromotivated 0:6f4cd2c49f65 67 PID pid(&setpoint, &feedback, &output,
electromotivated 0:6f4cd2c49f65 68 output_lower_limit, output_upper_limit,
electromotivated 0:6f4cd2c49f65 69 kp_init, ki_init, kd_init, Ts); // Init for position control
electromotivated 0:6f4cd2c49f65 70 QEI encoder(p15, p16);
electromotivated 0:6f4cd2c49f65 71 PwmOut mtr_pwm(p25);
electromotivated 0:6f4cd2c49f65 72 DigitalOut mtr_dir(p24);
electromotivated 0:6f4cd2c49f65 73 void pid_callback(); // Updates encoder feedback and motor output
electromotivated 0:6f4cd2c49f65 74 Ticker motor;
electromotivated 0:6f4cd2c49f65 75 /*****************************************************************************/
electromotivated 0:6f4cd2c49f65 76 /*****************************************************************************/
electromotivated 0:6f4cd2c49f65 77
electromotivated 0:6f4cd2c49f65 78 /**********WEB SERVER SPECIFIC DECLARTATIONS**********************************/
electromotivated 0:6f4cd2c49f65 79 /*****************************************************************************/
electromotivated 0:6f4cd2c49f65 80 SDFileSystem sd(p5,p6,p7,p8,"sd"); // MOSI, MISO, SCLK, CS,
electromotivated 0:6f4cd2c49f65 81 // Virtual File System Name
electromotivated 0:6f4cd2c49f65 82 Serial esp(p13, p14); // tx, rx
electromotivated 0:6f4cd2c49f65 83 DigitalOut espRstPin(p26); // ESP Reset
electromotivated 0:6f4cd2c49f65 84 DigitalOut led(LED4);
electromotivated 0:6f4cd2c49f65 85
electromotivated 0:6f4cd2c49f65 86 Timer t1;
electromotivated 0:6f4cd2c49f65 87 Timer t2;
electromotivated 0:6f4cd2c49f65 88
electromotivated 0:6f4cd2c49f65 89 void init(char* buffer, int size);
electromotivated 0:6f4cd2c49f65 90 void getreply(int timeout_ms, char* buffer, int size, int numBytes);
electromotivated 0:6f4cd2c49f65 91 void startserver(char* buffer, int size);
electromotivated 0:6f4cd2c49f65 92 void update_webpage(char* webpage, float setpoint, float kp, float ki, float kd);
electromotivated 0:6f4cd2c49f65 93 void parse_input(char* webpage_user_data, bool* control_mode, float* setpoint, float* kp, float* ki, float* kd);
electromotivated 0:6f4cd2c49f65 94 int port =80; // set server port
electromotivated 0:6f4cd2c49f65 95 int serverTimeout_secs =5; // set server timeout in seconds in case
electromotivated 0:6f4cd2c49f65 96 // link breaks.
electromotivated 0:6f4cd2c49f65 97 /*****************************************************************************/
electromotivated 0:6f4cd2c49f65 98 /*****************************************************************************/
electromotivated 0:6f4cd2c49f65 99
electromotivated 0:6f4cd2c49f65 100 // Common Application Declarations
electromotivated 0:6f4cd2c49f65 101 Serial pc(USBTX, USBRX);
electromotivated 0:6f4cd2c49f65 102 float clip(float value, float lower, float upper);
electromotivated 0:6f4cd2c49f65 103
electromotivated 0:6f4cd2c49f65 104 int main()
electromotivated 0:6f4cd2c49f65 105 {
electromotivated 0:6f4cd2c49f65 106 printf("Starting\n");
electromotivated 0:6f4cd2c49f65 107
electromotivated 0:6f4cd2c49f65 108 /****************** Load Webpage from SD Card***************************************/
electromotivated 0:6f4cd2c49f65 109 /***********************************************************************************/
electromotivated 0:6f4cd2c49f65 110 char file[] = "/sd/pid_dual.html";
electromotivated 0:6f4cd2c49f65 111
electromotivated 0:6f4cd2c49f65 112 // Get file size so we can dynamically allocate buffer size
electromotivated 0:6f4cd2c49f65 113 int num_chars = 0;
electromotivated 0:6f4cd2c49f65 114 FILE *fp = fopen(file, "r");
electromotivated 0:6f4cd2c49f65 115 while(!feof(fp)){
electromotivated 0:6f4cd2c49f65 116 fgetc(fp);
electromotivated 0:6f4cd2c49f65 117 num_chars++;
electromotivated 0:6f4cd2c49f65 118 }
electromotivated 0:6f4cd2c49f65 119 rewind(fp); // Go to beginning of file
electromotivated 0:6f4cd2c49f65 120
electromotivated 0:6f4cd2c49f65 121 #ifdef DEBUG
electromotivated 0:6f4cd2c49f65 122 printf("Webpage Data Size: %d byte\r\n", num_chars);
electromotivated 0:6f4cd2c49f65 123 #endif
electromotivated 0:6f4cd2c49f65 124
electromotivated 0:6f4cd2c49f65 125 const int WEBPAGE_SIZE = num_chars;
electromotivated 0:6f4cd2c49f65 126 char webpage[WEBPAGE_SIZE];
electromotivated 0:6f4cd2c49f65 127 webpage[0] = NULL; // Init our array so that element zero contains a null
electromotivated 0:6f4cd2c49f65 128 // This is important, ensures strings are placed into
electromotivated 0:6f4cd2c49f65 129 // buffer starting at element 0... not some random
electromotivated 0:6f4cd2c49f65 130 // elment
electromotivated 0:6f4cd2c49f65 131 // Read in and buffer file to memory
electromotivated 0:6f4cd2c49f65 132 if(fp == NULL){
electromotivated 0:6f4cd2c49f65 133 printf("Error: No Such File or something :(");
electromotivated 0:6f4cd2c49f65 134 return 1;
electromotivated 0:6f4cd2c49f65 135 }
electromotivated 0:6f4cd2c49f65 136 else{
electromotivated 0:6f4cd2c49f65 137 while(!feof(fp)){
electromotivated 0:6f4cd2c49f65 138 fgets(webpage + strlen(webpage), WEBPAGE_SIZE, fp); // Get a string from stream, add to buffer
electromotivated 0:6f4cd2c49f65 139 }
electromotivated 0:6f4cd2c49f65 140 }
electromotivated 0:6f4cd2c49f65 141 fclose(fp);
electromotivated 0:6f4cd2c49f65 142 printf("Webpage Buffer Size: %d bytes\r\n", sizeof(webpage));
electromotivated 0:6f4cd2c49f65 143 update_webpage(webpage, setpoint, kp_init, ki_init, kd_init); // Update Webpage for
electromotivated 0:6f4cd2c49f65 144 // Position Mode
electromotivated 0:6f4cd2c49f65 145 /***********************************************************************************/
electromotivated 0:6f4cd2c49f65 146 /***********************************************************************************/
electromotivated 0:6f4cd2c49f65 147
electromotivated 0:6f4cd2c49f65 148 /***************BRING UP SERVER*****************************************************/
electromotivated 0:6f4cd2c49f65 149 /***********************************************************************************/
electromotivated 0:6f4cd2c49f65 150 char buff[5000]; // Working buffer
electromotivated 0:6f4cd2c49f65 151 init(buff, sizeof(buff)); // Init ESP8266
electromotivated 0:6f4cd2c49f65 152
electromotivated 0:6f4cd2c49f65 153 esp.baud(115200); // ESP8266 baudrate. Maximum on KLxx' is 115200, 230400 works on K20 and K22F
electromotivated 0:6f4cd2c49f65 154
electromotivated 0:6f4cd2c49f65 155 startserver(buff, sizeof(buff)); // Configure the ESP8266 and Setup as Server
electromotivated 0:6f4cd2c49f65 156
electromotivated 0:6f4cd2c49f65 157 printf(buff); // If start successful buff contains IP address...
electromotivated 0:6f4cd2c49f65 158 // if not if contains an error.
electromotivated 0:6f4cd2c49f65 159 /***********************************************************************************/
electromotivated 0:6f4cd2c49f65 160 /***********************************************************************************/
electromotivated 0:6f4cd2c49f65 161
electromotivated 0:6f4cd2c49f65 162 /************Initialize the PID*****************************************************/
electromotivated 0:6f4cd2c49f65 163 /***********************************************************************************/
electromotivated 0:6f4cd2c49f65 164 setpoint = 0.0;
electromotivated 0:6f4cd2c49f65 165 encoder.reset();
electromotivated 0:6f4cd2c49f65 166 feedback = encoder.read();
electromotivated 0:6f4cd2c49f65 167
electromotivated 0:6f4cd2c49f65 168 // Init the motor
electromotivated 0:6f4cd2c49f65 169 mtr_dir = 0; // Can be 0 or 1, sets the direction
electromotivated 0:6f4cd2c49f65 170 mtr_pwm = 0.0;
electromotivated 0:6f4cd2c49f65 171
electromotivated 0:6f4cd2c49f65 172 // Update sensors and feedback twice as fast as PID sample time
electromotivated 0:6f4cd2c49f65 173 // this makes pid react in real-time avoiding errors due to
electromotivated 0:6f4cd2c49f65 174 // missing counts etc.
electromotivated 2:07a3107e7664 175 motor.attach(&pid_callback, Ts_PID_CALLBACK);
electromotivated 0:6f4cd2c49f65 176
electromotivated 0:6f4cd2c49f65 177 // Start PID sampling
electromotivated 0:6f4cd2c49f65 178 pid.start();
electromotivated 0:6f4cd2c49f65 179
electromotivated 0:6f4cd2c49f65 180 /***********************************************************************************/
electromotivated 0:6f4cd2c49f65 181 /***********************************************************************************/
electromotivated 0:6f4cd2c49f65 182
electromotivated 0:6f4cd2c49f65 183 while(1){
electromotivated 0:6f4cd2c49f65 184 /**************SERVICE WEBPAGE******************************************************/
electromotivated 0:6f4cd2c49f65 185 /***********************************************************************************/
electromotivated 0:6f4cd2c49f65 186 if(esp.readable()){
electromotivated 0:6f4cd2c49f65 187 getreply(500, buff, sizeof(buff), sizeof(buff) -1); // Get full buff, leave last element for null char
electromotivated 0:6f4cd2c49f65 188 #ifdef DEBUG
electromotivated 0:6f4cd2c49f65 189 printf("\r\n*************WORKING BUFFER******************************\r\n");
electromotivated 0:6f4cd2c49f65 190 printf(buff); printf("\n");
electromotivated 0:6f4cd2c49f65 191 printf("\r\n**************END WORKING BUFFER**************************\r\n");
electromotivated 0:6f4cd2c49f65 192 #endif
electromotivated 0:6f4cd2c49f65 193 // If Recieved Data get ID, Length, and Data
electromotivated 0:6f4cd2c49f65 194 char* rqstPnt = strstr(buff, "+IPD");
electromotivated 0:6f4cd2c49f65 195 if(rqstPnt != NULL){
electromotivated 0:6f4cd2c49f65 196 int id, len;
electromotivated 0:6f4cd2c49f65 197 char type[10]; memset(type, '\0', sizeof(type)); // Create and null out data buff
electromotivated 0:6f4cd2c49f65 198 sscanf(rqstPnt, "+IPD,%d,%d:%s ", &id, &len, type);
electromotivated 0:6f4cd2c49f65 199 #ifdef DEBUG
electromotivated 0:6f4cd2c49f65 200 printf("ID: %i\nLen: %i\nType: %s\n", id, len, type);
electromotivated 0:6f4cd2c49f65 201 #endif
electromotivated 0:6f4cd2c49f65 202 // If GET or POST request "type" parse and update user input then send webpage
electromotivated 0:6f4cd2c49f65 203 if(strstr(type, "GET") != NULL || strstr(type, "POST") != NULL){
electromotivated 0:6f4cd2c49f65 204 #ifdef DEBUG
electromotivated 0:6f4cd2c49f65 205 printf("I got web request\n");
electromotivated 0:6f4cd2c49f65 206 #endif
electromotivated 0:6f4cd2c49f65 207 /* Read Webpage <Form> data sent using "method=POST"...
electromotivated 0:6f4cd2c49f65 208 Note: Input elements in the <Form> need a set name attribute to
electromotivated 0:6f4cd2c49f65 209 appear in the returned HTML body. Thus to "POST" data ensure:
electromotivated 0:6f4cd2c49f65 210 <Form method="POST"> <input type="xxx" name="xxx" value="xxx">
electromotivated 0:6f4cd2c49f65 211 <input type="xxx" value="xxx"> </Form>
electromotivated 0:6f4cd2c49f65 212 Only the input with name="xxx" will appear in body of HTML
electromotivated 0:6f4cd2c49f65 213 */
electromotivated 0:6f4cd2c49f65 214 #ifdef DEBUG
electromotivated 0:6f4cd2c49f65 215 printf("\r\n*************USER INPUT**********************************\r\n");
electromotivated 0:6f4cd2c49f65 216 #endif
electromotivated 0:6f4cd2c49f65 217
electromotivated 0:6f4cd2c49f65 218 parse_input(buff, &control_mode, &setpoint, &kp, &ki, &kd);
electromotivated 4:9f51c7de9ebd 219 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)
electromotivated 4:9f51c7de9ebd 220 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)
electromotivated 4:9f51c7de9ebd 221 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)
electromotivated 4:9f51c7de9ebd 222 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)
electromotivated 0:6f4cd2c49f65 223
electromotivated 0:6f4cd2c49f65 224 #ifdef DEBUG
electromotivated 1:d69f57dcde02 225 printf("User Entered: \ncontrol_mode: %i\nSetpoint: %7.4f\nKp: %6.4f\nKi: %6.4f\nKd: %6.4f\n",
electromotivated 0:6f4cd2c49f65 226 control_mode, setpoint, kp, ki, kd);
electromotivated 0:6f4cd2c49f65 227 #endif
electromotivated 0:6f4cd2c49f65 228
electromotivated 0:6f4cd2c49f65 229 pid.set_parameters(kp, ki, kd, Ts); // Updata PID params
electromotivated 0:6f4cd2c49f65 230
electromotivated 0:6f4cd2c49f65 231 #ifdef DEBUG
electromotivated 1:d69f57dcde02 232 printf("Updated to Kp: %1.4f Ki: %1.4f Kd: %1.4f Ts: %1.4f\r\n",
electromotivated 0:6f4cd2c49f65 233 pid.getKp(), pid.getKi(), pid.getKd(), pid.getTs());
electromotivated 1:d69f57dcde02 234 printf("Setpoint: %1.4f\r\n", setpoint);
electromotivated 1:d69f57dcde02 235 printf("Output: %1.4f\r\n", output);
electromotivated 0:6f4cd2c49f65 236 printf("\r\n*************END USER INPUT******************************\r\n");
electromotivated 0:6f4cd2c49f65 237 #endif
electromotivated 0:6f4cd2c49f65 238
electromotivated 0:6f4cd2c49f65 239 // Update Webpage to reflect new values POSTED by client
electromotivated 0:6f4cd2c49f65 240 static bool isFirstRequest = true;
electromotivated 0:6f4cd2c49f65 241 if(!isFirstRequest) update_webpage(webpage, setpoint, kp, ki, kd);
electromotivated 0:6f4cd2c49f65 242 else isFirstRequest = false; // First Request just send page with initial values
electromotivated 0:6f4cd2c49f65 243
electromotivated 0:6f4cd2c49f65 244 #ifdef DEBUG
electromotivated 0:6f4cd2c49f65 245 printf(webpage); // DEBUGGING ONLY!!! REMOVE FOR RELEASE!!!
electromotivated 0:6f4cd2c49f65 246 #endif
electromotivated 0:6f4cd2c49f65 247
electromotivated 0:6f4cd2c49f65 248 // Command TCP/IP Data Tx
electromotivated 0:6f4cd2c49f65 249 esp.printf("AT+CIPSEND=%d,%d\r\n", id, strlen(webpage));
electromotivated 0:6f4cd2c49f65 250 getreply(200, buff, sizeof(buff), 15); /*TODO: Wait for "OK\r\n>"*/
electromotivated 0:6f4cd2c49f65 251
electromotivated 0:6f4cd2c49f65 252 #ifdef DEBUG
electromotivated 0:6f4cd2c49f65 253 printf(buff); printf("\n");
electromotivated 0:6f4cd2c49f65 254 #endif
electromotivated 0:6f4cd2c49f65 255
electromotivated 0:6f4cd2c49f65 256 // Send webpage
electromotivated 0:6f4cd2c49f65 257 // while(!esp.writeable()); // Wait until esp ready to send data
electromotivated 0:6f4cd2c49f65 258 int idx = 0;
electromotivated 0:6f4cd2c49f65 259 while(webpage[idx] != '\0'){
electromotivated 0:6f4cd2c49f65 260 esp.putc(webpage[idx]);
electromotivated 0:6f4cd2c49f65 261 idx++;
electromotivated 0:6f4cd2c49f65 262 }
electromotivated 0:6f4cd2c49f65 263
electromotivated 0:6f4cd2c49f65 264 // Check status - Success: close channel and update PID controller, Error: reconnect
electromotivated 0:6f4cd2c49f65 265 bool weberror = true;
electromotivated 0:6f4cd2c49f65 266 t2.reset(); t2.start();
electromotivated 0:6f4cd2c49f65 267 while(weberror ==1 && t2.read_ms() < 5000){
electromotivated 0:6f4cd2c49f65 268 getreply(500, buff, sizeof(buff), 24);
electromotivated 0:6f4cd2c49f65 269 if(strstr(buff, "SEND OK") != NULL) weberror = false;
electromotivated 0:6f4cd2c49f65 270 }
electromotivated 0:6f4cd2c49f65 271 if(weberror){
electromotivated 0:6f4cd2c49f65 272 esp.printf("AT+CIPMUX=1\r\n");
electromotivated 0:6f4cd2c49f65 273 getreply(500, buff, sizeof(buff), 10);
electromotivated 0:6f4cd2c49f65 274 #ifdef DEBUG
electromotivated 0:6f4cd2c49f65 275 printf(buff); printf("\n");
electromotivated 0:6f4cd2c49f65 276 #endif
electromotivated 0:6f4cd2c49f65 277 esp.printf("AT+CIPSERVER=1,%d\r\n", port);
electromotivated 0:6f4cd2c49f65 278 getreply(500, buff, sizeof(buff), 10);
electromotivated 0:6f4cd2c49f65 279 #ifdef DEBUG
electromotivated 0:6f4cd2c49f65 280 printf(buff); printf("\n");
electromotivated 0:6f4cd2c49f65 281 #endif
electromotivated 0:6f4cd2c49f65 282 }
electromotivated 0:6f4cd2c49f65 283 else{
electromotivated 0:6f4cd2c49f65 284 esp.printf("AT+CIPCLOSE=%d\r\n", id); // Notice id is an int formatted to string
electromotivated 0:6f4cd2c49f65 285 getreply(500, buff, sizeof(buff), 24);
electromotivated 0:6f4cd2c49f65 286 #ifdef DEBUG
electromotivated 0:6f4cd2c49f65 287 printf(buff); printf("\n");
electromotivated 0:6f4cd2c49f65 288 #endif
electromotivated 0:6f4cd2c49f65 289 }
electromotivated 0:6f4cd2c49f65 290 }
electromotivated 0:6f4cd2c49f65 291 }
electromotivated 0:6f4cd2c49f65 292 }
electromotivated 0:6f4cd2c49f65 293 /*********************************************************************/
electromotivated 0:6f4cd2c49f65 294 /*********************************************************************/
electromotivated 0:6f4cd2c49f65 295
electromotivated 0:6f4cd2c49f65 296
electromotivated 0:6f4cd2c49f65 297 }
electromotivated 0:6f4cd2c49f65 298 }
electromotivated 0:6f4cd2c49f65 299
electromotivated 0:6f4cd2c49f65 300 // Initialize ESP8266
electromotivated 0:6f4cd2c49f65 301 void init(char* buffer, int size){
electromotivated 0:6f4cd2c49f65 302 // Hardware Reset ESP
electromotivated 0:6f4cd2c49f65 303 espRstPin=0;
electromotivated 0:6f4cd2c49f65 304 wait(0.5);
electromotivated 0:6f4cd2c49f65 305 espRstPin=1;
electromotivated 0:6f4cd2c49f65 306 // Get start up junk from ESP8266
electromotivated 0:6f4cd2c49f65 307 getreply(6000, buffer, size, 500);
electromotivated 0:6f4cd2c49f65 308 }
electromotivated 0:6f4cd2c49f65 309
electromotivated 0:6f4cd2c49f65 310 // Get Command and ESP status replies
electromotivated 0:6f4cd2c49f65 311 void getreply(int timeout_ms, char* buffer, int size, int numBytes)
electromotivated 0:6f4cd2c49f65 312 {
electromotivated 0:6f4cd2c49f65 313 memset(buffer, '\0', size); // Null out buffer
electromotivated 0:6f4cd2c49f65 314 t1.reset();
electromotivated 0:6f4cd2c49f65 315 t1.start();
electromotivated 0:6f4cd2c49f65 316 int idx = 0;
electromotivated 0:6f4cd2c49f65 317 while(t1.read_ms()< timeout_ms && idx < numBytes) {
electromotivated 0:6f4cd2c49f65 318 if(esp.readable()) {
electromotivated 0:6f4cd2c49f65 319 buffer[idx] = esp.getc();
electromotivated 0:6f4cd2c49f65 320 idx++;
electromotivated 0:6f4cd2c49f65 321 }
electromotivated 0:6f4cd2c49f65 322 }
electromotivated 0:6f4cd2c49f65 323 t1.stop();
electromotivated 0:6f4cd2c49f65 324 }
electromotivated 0:6f4cd2c49f65 325
electromotivated 0:6f4cd2c49f65 326 // Starts and restarts webserver if errors detected.
electromotivated 0:6f4cd2c49f65 327 void startserver(char* buffer, int size)
electromotivated 0:6f4cd2c49f65 328 {
electromotivated 0:6f4cd2c49f65 329 esp.printf("AT+RST\r\n"); // BWW: Reset the ESP8266
electromotivated 0:6f4cd2c49f65 330 getreply(8000, buffer, size, 1000);
electromotivated 0:6f4cd2c49f65 331
electromotivated 0:6f4cd2c49f65 332 if (strstr(buffer, "OK") != NULL) {
electromotivated 0:6f4cd2c49f65 333 // BWW: Set ESP8266 for multiple connections
electromotivated 0:6f4cd2c49f65 334 esp.printf("AT+CIPMUX=1\r\n");
electromotivated 0:6f4cd2c49f65 335 getreply(500, buffer, size, 20);
electromotivated 0:6f4cd2c49f65 336
electromotivated 0:6f4cd2c49f65 337 // BWW: Set ESP8266 as Server on given port
electromotivated 0:6f4cd2c49f65 338 esp.printf("AT+CIPSERVER=1,%d\r\n", port);
electromotivated 0:6f4cd2c49f65 339 getreply(500, buffer, size, 20); // BWW: Wait for reply
electromotivated 0:6f4cd2c49f65 340
electromotivated 0:6f4cd2c49f65 341 // BWW: Set ESP8266 Server Timeout
electromotivated 0:6f4cd2c49f65 342 esp.printf("AT+CIPSTO=%d\r\n", serverTimeout_secs);
electromotivated 0:6f4cd2c49f65 343 getreply(500, buffer, size, 50); // BWW: Wait for reply
electromotivated 0:6f4cd2c49f65 344
electromotivated 0:6f4cd2c49f65 345 // BWW: Request IP Address from router for ESP8266
electromotivated 0:6f4cd2c49f65 346 int weberror = 0;
electromotivated 0:6f4cd2c49f65 347 while(weberror==0) {
electromotivated 0:6f4cd2c49f65 348 esp.printf("AT+CIFSR\r\n");
electromotivated 0:6f4cd2c49f65 349 getreply(2500, buffer, size, 200);
electromotivated 0:6f4cd2c49f65 350 if(strstr(buffer, "0.0.0.0") == NULL) {
electromotivated 0:6f4cd2c49f65 351 weberror=1; // wait for valid IP
electromotivated 0:6f4cd2c49f65 352 }
electromotivated 0:6f4cd2c49f65 353 }
electromotivated 0:6f4cd2c49f65 354 }
electromotivated 0:6f4cd2c49f65 355 // else ESP8266 did not reply "OK" something is messed up
electromotivated 0:6f4cd2c49f65 356 else {
electromotivated 0:6f4cd2c49f65 357 strcpy(buffer, "ESP8266 Error\n");
electromotivated 0:6f4cd2c49f65 358 }
electromotivated 0:6f4cd2c49f65 359 }
electromotivated 0:6f4cd2c49f65 360
electromotivated 0:6f4cd2c49f65 361 /*
electromotivated 0:6f4cd2c49f65 362 update_webpage() updates output fields based on webpage user inputs "POSTED"
electromotivated 0:6f4cd2c49f65 363 Preconditions: webpage[] must have the following elements
electromotivated 0:6f4cd2c49f65 364 "kp_output" value="xxx.xx"
electromotivated 0:6f4cd2c49f65 365 "ki_output" value="xxx.xx"
electromotivated 0:6f4cd2c49f65 366 "kp_output" value="xxx.xx"
electromotivated 0:6f4cd2c49f65 367 @param webpage Pointer to webpage char[]
electromotivated 0:6f4cd2c49f65 368 @param kp New kp value posted by user
electromotivated 0:6f4cd2c49f65 369 @param ki New ki value posted by user
electromotivated 0:6f4cd2c49f65 370 @param kd New kd value posted by user
electromotivated 0:6f4cd2c49f65 371
electromotivated 0:6f4cd2c49f65 372 NOTE: THIS IS WEBPAGE SPECIFIC!!!! CHANGE THE CODE IN HERE TO SUITE THE
electromotivated 0:6f4cd2c49f65 373 SPECIFIC APPLICATION WEBPAGE!!! ALSO USED TO REFLECT THE CUSTOM
electromotivated 0:6f4cd2c49f65 374 IMPLEMENTATION OF THE parse_intput() function. MAKE SURE THESE TWO FUNCTIONS
electromotivated 0:6f4cd2c49f65 375 INTEGRATE PROPERLY!!!
electromotivated 0:6f4cd2c49f65 376 */
electromotivated 0:6f4cd2c49f65 377 void update_webpage(char* webpage, float setpoint, float kp, float ki, float kd){
electromotivated 0:6f4cd2c49f65 378 // Change output value to reflect new control mode, setpoint, kp, ki, kd values
electromotivated 0:6f4cd2c49f65 379 char* begin;
electromotivated 0:6f4cd2c49f65 380 char temp[8];
electromotivated 0:6f4cd2c49f65 381 int idx;
electromotivated 0:6f4cd2c49f65 382
electromotivated 0:6f4cd2c49f65 383 // Update Control Mode Radio Buttons
electromotivated 0:6f4cd2c49f65 384 memset(temp, '\0', sizeof(temp));
electromotivated 0:6f4cd2c49f65 385 idx = 0;
electromotivated 0:6f4cd2c49f65 386 begin = strstr(webpage, "name=\"control_mode\" value=\"") +
electromotivated 0:6f4cd2c49f65 387 sizeof("name=\"control_mode\" value=\"0"); // Update Control Mode Position Radio Button
electromotivated 0:6f4cd2c49f65 388 if(control_mode == POSITION) sprintf(temp, "%s", "checked");// If Position active "check" it
electromotivated 0:6f4cd2c49f65 389 else sprintf(temp, "%s", " "); // else "clear" it
electromotivated 0:6f4cd2c49f65 390 while(temp[idx] != '\0'){ // Write "checked"/" " to field
electromotivated 0:6f4cd2c49f65 391 begin[idx] = temp[idx];
electromotivated 0:6f4cd2c49f65 392 idx++;
electromotivated 0:6f4cd2c49f65 393 }
electromotivated 0:6f4cd2c49f65 394 memset(temp, '\0', sizeof(temp));
electromotivated 0:6f4cd2c49f65 395 idx = 0;
electromotivated 0:6f4cd2c49f65 396 begin = strstr(webpage, "name=\"control_mode\" value=\"") +
electromotivated 0:6f4cd2c49f65 397 sizeof("name=\"control_mode\" value=\"0\""); // Nav to first Control Mode Radio Button (Position)
electromotivated 0:6f4cd2c49f65 398 begin = strstr(begin, "name=\"control_mode\" value=\"") +
electromotivated 0:6f4cd2c49f65 399 sizeof("name=\"control_mode\" value=\"1"); // Nav to second Control Mode Radio Button (Speed)
electromotivated 0:6f4cd2c49f65 400 if(control_mode == SPEED) sprintf(temp, "%s", "checked"); // If Speed active "check" it
electromotivated 0:6f4cd2c49f65 401 else sprintf(temp, "%s", " "); // else "clear" it
electromotivated 0:6f4cd2c49f65 402 while(temp[idx] != '\0'){ // Write "checked"/" " to field
electromotivated 0:6f4cd2c49f65 403 begin[idx] = temp[idx];
electromotivated 0:6f4cd2c49f65 404 idx++;
electromotivated 0:6f4cd2c49f65 405 }
electromotivated 0:6f4cd2c49f65 406
electromotivated 0:6f4cd2c49f65 407 // Update Kp Paramater Field
electromotivated 0:6f4cd2c49f65 408 memset(temp, '\0', sizeof(temp));
electromotivated 0:6f4cd2c49f65 409 idx = 0;
electromotivated 0:6f4cd2c49f65 410 begin = strstr(webpage, "name=\"kp_input\" value=\"") +
electromotivated 0:6f4cd2c49f65 411 sizeof("name=\"kp_input\" value="); // Points to start of kp_output field
electromotivated 0:6f4cd2c49f65 412 // Determine precision of float such temp string has no empty spaces;
electromotivated 0:6f4cd2c49f65 413 // i.e. each space must have a value or a decimal point, other wise webbrowser may not recognize value
electromotivated 0:6f4cd2c49f65 414 if(kp >= 100) sprintf(temp, "%6.2f", kp); // xxx.00
electromotivated 0:6f4cd2c49f65 415 else if(10 <= kp && kp < 100) sprintf(temp, "%6.3f", kp); // xx.000
electromotivated 0:6f4cd2c49f65 416 else sprintf(temp, "%6.4f", kp); // x.0000
electromotivated 0:6f4cd2c49f65 417 while(temp[idx] != '\0'){ // Overwrite old digits with new digits
electromotivated 0:6f4cd2c49f65 418 begin[idx] = temp[idx];
electromotivated 0:6f4cd2c49f65 419 idx++;
electromotivated 0:6f4cd2c49f65 420 }
electromotivated 0:6f4cd2c49f65 421
electromotivated 0:6f4cd2c49f65 422 // Update Ki Parameter Field
electromotivated 0:6f4cd2c49f65 423 memset(temp, '\0', sizeof(temp));
electromotivated 0:6f4cd2c49f65 424 idx = 0;
electromotivated 0:6f4cd2c49f65 425 begin = strstr(webpage, "name=\"ki_input\" value=\"") +
electromotivated 0:6f4cd2c49f65 426 sizeof("name=\"ki_input\" value="); // Points to start of ki_output field
electromotivated 0:6f4cd2c49f65 427 // Determine precision of float such temp string has no empty spaces;
electromotivated 0:6f4cd2c49f65 428 // i.e. each space must have a value or a decimal point, other wise webbrowser may not recognize value
electromotivated 0:6f4cd2c49f65 429 if(ki >= 100) sprintf(temp, "%6.2f", ki); // xxx.00
electromotivated 0:6f4cd2c49f65 430 else if(10 <= ki && ki < 100) sprintf(temp, "%6.3f", ki); // xx.000
electromotivated 0:6f4cd2c49f65 431 else sprintf(temp, "%6.4f", ki); // x.0000
electromotivated 0:6f4cd2c49f65 432 while(temp[idx] != '\0'){ // Overwrite old digits with new digits
electromotivated 0:6f4cd2c49f65 433 begin[idx] = temp[idx];
electromotivated 0:6f4cd2c49f65 434 idx++;
electromotivated 0:6f4cd2c49f65 435 }
electromotivated 0:6f4cd2c49f65 436
electromotivated 0:6f4cd2c49f65 437 // Update Kd Parameter Field
electromotivated 0:6f4cd2c49f65 438 memset(temp, '\0', sizeof(temp));
electromotivated 0:6f4cd2c49f65 439 idx = 0;
electromotivated 0:6f4cd2c49f65 440 begin = strstr(webpage, "name=\"kd_input\" value=\"")+
electromotivated 0:6f4cd2c49f65 441 sizeof("name=\"kd_input\" value="); // Points to start of kd_output field
electromotivated 0:6f4cd2c49f65 442 // Determine precision of float such temp string has no empty spaces;
electromotivated 0:6f4cd2c49f65 443 // i.e. each space must have a value or a decimal point, other wise webbrowser may not recognize value
electromotivated 0:6f4cd2c49f65 444 if(kd >= 100) sprintf(temp, "%6.2f", kd); // xxx.00
electromotivated 0:6f4cd2c49f65 445 else if(10 <= kd && kd < 100) sprintf(temp, "%6.3f", kd); // xx.000
electromotivated 0:6f4cd2c49f65 446 else sprintf(temp, "%6.4f", kd); // x.0000
electromotivated 0:6f4cd2c49f65 447 while(temp[idx] != '\0'){ // Overwrite old digits with new digits
electromotivated 0:6f4cd2c49f65 448 begin[idx] = temp[idx];
electromotivated 0:6f4cd2c49f65 449 idx++;
electromotivated 0:6f4cd2c49f65 450 }
electromotivated 0:6f4cd2c49f65 451
electromotivated 0:6f4cd2c49f65 452 // Update Setpoint Parameter Field
electromotivated 0:6f4cd2c49f65 453 // Determine precision of float such temp string has no empty spaces;
electromotivated 0:6f4cd2c49f65 454 // i.e. each space must have a value or a decimal point or neg sign,
electromotivated 0:6f4cd2c49f65 455 // other wise webbrowser may not recognize value
electromotivated 0:6f4cd2c49f65 456 memset(temp, '\0', sizeof(temp));
electromotivated 0:6f4cd2c49f65 457 idx = 0;
electromotivated 0:6f4cd2c49f65 458 begin = strstr(webpage, "name=\"setpoint_input\" value=\"")+
electromotivated 0:6f4cd2c49f65 459 sizeof("name=\"setpoint_input\" value="); // Points to start of kp_output field
electromotivated 0:6f4cd2c49f65 460 // Determine precision of float such temp string has no empty spaces;
electromotivated 0:6f4cd2c49f65 461 // i.e. each space must have a value or a decimal point, other wise webbrowser may not recognize value
electromotivated 0:6f4cd2c49f65 462 if(setpoint >= 0.00){
electromotivated 0:6f4cd2c49f65 463 if(setpoint >= 100) sprintf(temp, "%6.3f", setpoint); // xxx.000
electromotivated 0:6f4cd2c49f65 464 else if(10 <= setpoint && setpoint < 100) sprintf(temp, "%7.4f", setpoint); // xx.0000
electromotivated 0:6f4cd2c49f65 465 else sprintf(temp, "%6.5f", setpoint); // x.00000
electromotivated 0:6f4cd2c49f65 466 }
electromotivated 0:6f4cd2c49f65 467 else{
electromotivated 0:6f4cd2c49f65 468 if(setpoint <= -100) sprintf(temp, "%6.2f", setpoint); // -xxx.00
electromotivated 0:6f4cd2c49f65 469 else if(-100 < setpoint && setpoint <= -10) sprintf(temp, "%6.3f", setpoint); // -xx.000
electromotivated 0:6f4cd2c49f65 470 else sprintf(temp, "%6.4f", setpoint); // -x.0000
electromotivated 0:6f4cd2c49f65 471 }
electromotivated 0:6f4cd2c49f65 472 while(temp[idx] != '\0'){ // Overwrite old digits with new digits
electromotivated 0:6f4cd2c49f65 473 begin[idx] = temp[idx];
electromotivated 0:6f4cd2c49f65 474 idx++;
electromotivated 0:6f4cd2c49f65 475 }
electromotivated 0:6f4cd2c49f65 476 }
electromotivated 0:6f4cd2c49f65 477
electromotivated 0:6f4cd2c49f65 478 /*
electromotivated 0:6f4cd2c49f65 479 parse_input() take a char*, in particular a pointer to Webpage User
electromotivated 0:6f4cd2c49f65 480 Input Data, for example:
electromotivated 0:6f4cd2c49f65 481 char str[] = "+IPD,0,44:kp_input=0.12&ki_input=14.25&kd_input=125.42";
electromotivated 0:6f4cd2c49f65 482
electromotivated 0:6f4cd2c49f65 483 and parses out the Setpoint Kp, Ki, Kd values that the user entered
electromotivated 0:6f4cd2c49f65 484 and posted in the webpage. Values are converted to floats and
electromotivated 0:6f4cd2c49f65 485 assigned to the given argurments.
electromotivated 0:6f4cd2c49f65 486
electromotivated 0:6f4cd2c49f65 487 NOTE: THIS IS WEBPAGE SPECIFIC!!!! CHANGE THE CODE IN HERE TO SUITE THE
electromotivated 0:6f4cd2c49f65 488 SPECIFIC APPLICATION WEBPAGE!!! THESE EXTRACTED VALUES WILL BE USED IN
electromotivated 0:6f4cd2c49f65 489 THE update_webpage() function. MAKE SURE THESE TWO FUNCTIONS INTEGRATE
electromotivated 0:6f4cd2c49f65 490 PROPERLY!!!
electromotivated 0:6f4cd2c49f65 491 */
electromotivated 0:6f4cd2c49f65 492 void parse_input(char* webpage_user_data, bool* control_mode, float *setpoint, float* kp, float* ki, float* kd){
electromotivated 0:6f4cd2c49f65 493 char keys[] = {'&', '\0'};
electromotivated 0:6f4cd2c49f65 494
electromotivated 0:6f4cd2c49f65 495 // Parse out user input values
electromotivated 0:6f4cd2c49f65 496 char input_buff[50];
electromotivated 0:6f4cd2c49f65 497 char* begin;
electromotivated 0:6f4cd2c49f65 498 char* end;
electromotivated 0:6f4cd2c49f65 499
electromotivated 0:6f4cd2c49f65 500 // Parse and Update Control Mode Value
electromotivated 0:6f4cd2c49f65 501 memset(input_buff, '\0', sizeof(input_buff)); // Null out input buff
electromotivated 0:6f4cd2c49f65 502 begin = strstr(webpage_user_data, "control_mode=") +
electromotivated 0:6f4cd2c49f65 503 sizeof("control_mode"); // Points to start of setpoint_input value
electromotivated 0:6f4cd2c49f65 504 end = begin + strcspn(begin, keys); // Points to end of setpoint_input value
electromotivated 0:6f4cd2c49f65 505 for(long i = 0; i < end - begin; i++){ // Parse out the value one char at a time
electromotivated 0:6f4cd2c49f65 506 input_buff[i] = begin[i];
electromotivated 0:6f4cd2c49f65 507 }
electromotivated 0:6f4cd2c49f65 508 *control_mode = atoi(input_buff);
electromotivated 0:6f4cd2c49f65 509
electromotivated 0:6f4cd2c49f65 510 // Parse and Update Setpoint Value
electromotivated 0:6f4cd2c49f65 511 memset(input_buff, '\0', sizeof(input_buff)); // Null out input buff
electromotivated 0:6f4cd2c49f65 512 begin = strstr(webpage_user_data, "setpoint_input=") +
electromotivated 0:6f4cd2c49f65 513 sizeof("setpoint_input"); // Points to start of setpoint_input value
electromotivated 0:6f4cd2c49f65 514 end = begin + strcspn(begin, keys); // Points to end of setpoint_input value
electromotivated 0:6f4cd2c49f65 515 for(long i = 0; i < end - begin; i++){ // Parse out the value one char at a time
electromotivated 0:6f4cd2c49f65 516 input_buff[i] = begin[i];
electromotivated 0:6f4cd2c49f65 517 }
electromotivated 0:6f4cd2c49f65 518 *setpoint = atof(input_buff);
electromotivated 0:6f4cd2c49f65 519
electromotivated 0:6f4cd2c49f65 520 // Parse and Update Kp Value
electromotivated 0:6f4cd2c49f65 521 memset(input_buff, '\0', sizeof(input_buff)); // Null out input buff
electromotivated 0:6f4cd2c49f65 522 begin = strstr(webpage_user_data, "kp_input=") +
electromotivated 0:6f4cd2c49f65 523 sizeof("kp_input"); // Points to start of kp_input value
electromotivated 0:6f4cd2c49f65 524 end = begin + strcspn(begin, keys); // Points to end of kp_input value
electromotivated 0:6f4cd2c49f65 525 for(long i = 0; i < end - begin; i++){ // Parse out the value one char at a time
electromotivated 0:6f4cd2c49f65 526 input_buff[i] = begin[i];
electromotivated 0:6f4cd2c49f65 527 }
electromotivated 0:6f4cd2c49f65 528 *kp = atof(input_buff);
electromotivated 0:6f4cd2c49f65 529
electromotivated 0:6f4cd2c49f65 530 // Parse and Update Ki Value
electromotivated 0:6f4cd2c49f65 531 memset(input_buff, '\0', sizeof(input_buff)); // Null out input buff
electromotivated 0:6f4cd2c49f65 532 begin = strstr(webpage_user_data, "ki_input=") +
electromotivated 0:6f4cd2c49f65 533 sizeof("ki_input"); // Points to start of ki_input value
electromotivated 0:6f4cd2c49f65 534 end = begin + strcspn(begin, keys); // Points to end of ki_input value
electromotivated 0:6f4cd2c49f65 535 for(long i = 0; i < end - begin; i++){ // Parse out the value one char at a time
electromotivated 0:6f4cd2c49f65 536 input_buff[i] = begin[i];
electromotivated 0:6f4cd2c49f65 537 }
electromotivated 0:6f4cd2c49f65 538 *ki = atof(input_buff);
electromotivated 0:6f4cd2c49f65 539
electromotivated 0:6f4cd2c49f65 540 // Parse and Update Kd Value
electromotivated 0:6f4cd2c49f65 541 memset(input_buff, '\0', sizeof(input_buff)); // Null out input buff
electromotivated 0:6f4cd2c49f65 542 begin = strstr(webpage_user_data, "kd_input=") +
electromotivated 0:6f4cd2c49f65 543 sizeof("kd_input"); // Points to start of kd_input value
electromotivated 0:6f4cd2c49f65 544 end = begin + strcspn(begin, keys); // Points to end of kd_input value
electromotivated 0:6f4cd2c49f65 545 for(long i = 0; i < end - begin; i++){ // Parse out the value one char at a time
electromotivated 0:6f4cd2c49f65 546 input_buff[i] = begin[i];
electromotivated 0:6f4cd2c49f65 547 }
electromotivated 0:6f4cd2c49f65 548 *kd = atof(input_buff);
electromotivated 0:6f4cd2c49f65 549 }
electromotivated 0:6f4cd2c49f65 550
electromotivated 0:6f4cd2c49f65 551 void pid_callback(){
electromotivated 0:6f4cd2c49f65 552 // If control_mode is POSITION run position pid
electromotivated 0:6f4cd2c49f65 553 if(control_mode == POSITION){
electromotivated 0:6f4cd2c49f65 554 // Update motor
electromotivated 0:6f4cd2c49f65 555 if(output >= 0.0) mtr_dir = 1; // Set direction to sign of output
electromotivated 0:6f4cd2c49f65 556 else mtr_dir = 0;
electromotivated 0:6f4cd2c49f65 557 mtr_pwm = abs(output); // Apply motor output
electromotivated 0:6f4cd2c49f65 558
electromotivated 0:6f4cd2c49f65 559 // Update feedback
electromotivated 0:6f4cd2c49f65 560 feedback = encoder.read()*FEEDBACK_SCALE;// Scale feedback to num wheel revs
electromotivated 0:6f4cd2c49f65 561 }
electromotivated 0:6f4cd2c49f65 562 // else control_mode must be SPEED, run speed pid
electromotivated 0:6f4cd2c49f65 563 else{
electromotivated 0:6f4cd2c49f65 564 if(setpoint >= 0.0) mtr_dir = 1; // Set motor direction based on setpoint
electromotivated 0:6f4cd2c49f65 565 else mtr_dir = 0;
electromotivated 0:6f4cd2c49f65 566 if(-0.001 < setpoint && setpoint < 0.001){
electromotivated 0:6f4cd2c49f65 567 /* Setpoint = 0 is a special case, we allow output to control speed AND
electromotivated 0:6f4cd2c49f65 568 direction to fight intertia and/or downhill roll. */
electromotivated 0:6f4cd2c49f65 569 if(output >= 0.0) mtr_dir = 1;
electromotivated 0:6f4cd2c49f65 570 else mtr_dir = 0;
electromotivated 0:6f4cd2c49f65 571 mtr_pwm = abs(output);
electromotivated 0:6f4cd2c49f65 572 }
electromotivated 0:6f4cd2c49f65 573 else{
electromotivated 0:6f4cd2c49f65 574 if(mtr_dir == 1){ // If CW then apply positive outputs
electromotivated 0:6f4cd2c49f65 575 if(output >= 0.0) mtr_pwm = output;
electromotivated 0:6f4cd2c49f65 576 else mtr_pwm = 0.0;
electromotivated 0:6f4cd2c49f65 577 }
electromotivated 0:6f4cd2c49f65 578 else{ // If CCW then apply negative outputs
electromotivated 0:6f4cd2c49f65 579 if(output <= 0.0) mtr_pwm = abs(output);
electromotivated 0:6f4cd2c49f65 580 else mtr_pwm = 0.0;
electromotivated 0:6f4cd2c49f65 581 }
electromotivated 0:6f4cd2c49f65 582 }
electromotivated 0:6f4cd2c49f65 583 float k = Ts/2.0; // Discrete time, (Ts/2 because this callback is called
electromotivated 0:6f4cd2c49f65 584 // at interval of Ts/2... or twice as fast as pid controller)
electromotivated 0:6f4cd2c49f65 585
electromotivated 0:6f4cd2c49f65 586 /* TODO: Implement a "rolling"/"moving" average */
electromotivated 0:6f4cd2c49f65 587 static int last_count = 0;
electromotivated 0:6f4cd2c49f65 588 int count = encoder.read();
electromotivated 0:6f4cd2c49f65 589 float raw_speed = ((count - last_count)*FEEDBACK_SCALE) / k;
electromotivated 0:6f4cd2c49f65 590 float rpm_speed = raw_speed * 60.0; // Convert speed to RPM
electromotivated 0:6f4cd2c49f65 591
electromotivated 0:6f4cd2c49f65 592 last_count = count; // Save last count
electromotivated 0:6f4cd2c49f65 593 feedback = rpm_speed;
electromotivated 0:6f4cd2c49f65 594 }
electromotivated 0:6f4cd2c49f65 595 }
electromotivated 0:6f4cd2c49f65 596
electromotivated 0:6f4cd2c49f65 597 /*
electromotivated 0:6f4cd2c49f65 598 Clips value to lower/ uppper
electromotivated 0:6f4cd2c49f65 599 @param value The value to clip
electromotivated 0:6f4cd2c49f65 600 @param lower The mininum allowable value
electromotivated 0:6f4cd2c49f65 601 @param upper The maximum allowable value
electromotivated 0:6f4cd2c49f65 602 @return The resulting clipped value
electromotivated 0:6f4cd2c49f65 603 */
electromotivated 0:6f4cd2c49f65 604 float clip(float value, float lower, float upper){
electromotivated 0:6f4cd2c49f65 605 return std::max(lower, std::min(value, upper));
electromotivated 0:6f4cd2c49f65 606 }
electromotivated 0:6f4cd2c49f65 607
electromotivated 0:6f4cd2c49f65 608 /**************************WEB PAGE TEXT**************************************/
electromotivated 0:6f4cd2c49f65 609 /*****************************************************************************
electromotivated 0:6f4cd2c49f65 610 Copy and past text below into a html file and save as the given file name to
electromotivated 0:6f4cd2c49f65 611 your SD card.
electromotivated 0:6f4cd2c49f65 612
electromotivated 0:6f4cd2c49f65 613 file name: pid_dual.html
electromotivated 0:6f4cd2c49f65 614
electromotivated 0:6f4cd2c49f65 615 html text:
electromotivated 0:6f4cd2c49f65 616
electromotivated 0:6f4cd2c49f65 617 <!DOCTYPE html>
electromotivated 0:6f4cd2c49f65 618 <html>
electromotivated 0:6f4cd2c49f65 619 <head>
electromotivated 0:6f4cd2c49f65 620 <title>PID Motor Control</title>
electromotivated 0:6f4cd2c49f65 621 </head>
electromotivated 0:6f4cd2c49f65 622 <body>
electromotivated 0:6f4cd2c49f65 623 <h1>PID Motor Control</h1>
electromotivated 0:6f4cd2c49f65 624 <h2>Motor Status</h2>
electromotivated 0:6f4cd2c49f65 625 <p>
electromotivated 0:6f4cd2c49f65 626 <form title="Motor Status">
electromotivated 0:6f4cd2c49f65 627 <input type="text" value="Some user information" size="25" readonly /><br>
electromotivated 0:6f4cd2c49f65 628 Current Setpoint:
electromotivated 0:6f4cd2c49f65 629 <input type="number" name="current_setpoint" value="0000.00" readonly /><br>
electromotivated 0:6f4cd2c49f65 630 Current Position:
electromotivated 0:6f4cd2c49f65 631 <input type="number" name="current_position" value="0000.00" readonly /><br>
electromotivated 0:6f4cd2c49f65 632 </form>
electromotivated 0:6f4cd2c49f65 633 </p>
electromotivated 0:6f4cd2c49f65 634 <h2>PID Status</h2>
electromotivated 0:6f4cd2c49f65 635 <form title="User Input" method="post">
electromotivated 0:6f4cd2c49f65 636 PID Controls: <br>
electromotivated 0:6f4cd2c49f65 637 <input type="radio" name="control_mode" value="0"checked>Position(#Revolutions)
electromotivated 0:6f4cd2c49f65 638 <br>
electromotivated 0:6f4cd2c49f65 639 <input type="radio" name="control_mode" value="1" >Speed(RPM)
electromotivated 0:6f4cd2c49f65 640 <br>
electromotivated 0:6f4cd2c49f65 641 Setpoint:
electromotivated 0:6f4cd2c49f65 642 <input type="number" name="setpoint_input" value="0000.00" step="0.0000001" size="6" /><br>
electromotivated 0:6f4cd2c49f65 643 Proportional Gain: (Good Starting Values: Position = 2.50 Speed = 0.01)<br>
electromotivated 0:6f4cd2c49f65 644 <input type="number" name="kp_input" value="002.50" step="0.0000001" size="6" /><br>
electromotivated 0:6f4cd2c49f65 645 Integral Gain: (Good Starting Values: Position = 5.0 Speed = 0.015)<br>
electromotivated 0:6f4cd2c49f65 646 <input type="number" name="ki_input" value="005.00" step="0.0000001" size="6" /><br>
electromotivated 0:6f4cd2c49f65 647 Derivative Gain: (Good Starting Values: Position = 0.25 Speed = 0.0001)<br>
electromotivated 0:6f4cd2c49f65 648 <input type="number" name="kd_input" value="000.25" step="0.0000001" size="6" /><br>
electromotivated 0:6f4cd2c49f65 649 <br>
electromotivated 0:6f4cd2c49f65 650 <input type="submit" value="Update" />
electromotivated 0:6f4cd2c49f65 651 </form>
electromotivated 0:6f4cd2c49f65 652 </body>
electromotivated 0:6f4cd2c49f65 653 </html>
electromotivated 0:6f4cd2c49f65 654
electromotivated 0:6f4cd2c49f65 655 *****************************************************************************/
electromotivated 0:6f4cd2c49f65 656 /*****************************************************************************/