Isaac Lee / Mbed 2 deprecated 4180_lab4_project

Bluetooth app controlled robot

Dependencies:   ESP8266 HALLFX_ENCODER LSM9DS1_Library_cal MotorDriver PID mbed

Fork of ESP8266_pid_redbot_webserver by Bryce Williams

main.cpp@1:d4a95e3a8aeb, 2015-12-08 (annotated)

Committer:
electromotivated
Date:
Tue Dec 08 00:17:04 2015 +0000
Revision:
1:d4a95e3a8aeb
Parent:
0:11bc7a815367
Child:
2:3466e9e16c99
Upload;

Who changed what in which revision?

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