![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
WiFi Webserver Robot with PID Motor Control
Dependencies: HALLFX_ENCODER MotorDriver PID SDFileSystem mbed
Diff: main.cpp
- Revision:
- 1:d4a95e3a8aeb
- Parent:
- 0:11bc7a815367
--- a/main.cpp Fri Nov 27 15:30:19 2015 +0000 +++ b/main.cpp Tue Dec 08 00:17:04 2015 +0000 @@ -1,6 +1,6 @@ /* Uses the ESP8266 WiFi Chip to set up a WiFi Webserver used to control - the position or speed of a motor using a PID controller. USE FIREFOX + the speed of a Sparkfun Redbot Rotbot. USE FIREFOX Web Browser NOTES: @@ -30,14 +30,15 @@ it off once Wifi module is configed so that this program can run in a "stand alone" mode. - TODO: Implement stop button in webpage + Reference/ Sources: This code is based on + https://developer.mbed.org/users/4180_1/notebook/using-the-esp8266-with-the-mbed-lpc1768/ */ - #include "mbed.h" #include "SDFileSystem.h" #include "PID.h" -#include "QEI.h" +#include "HALLFX_ENCODER.h" +#include "MotorDriver.h" #include <algorithm> //#define DEBUG // Uncomment for serial terminal print debugging @@ -45,34 +46,46 @@ /*********PID CONTROLLER SPECIFIC DECLARATIONS********************************/ /*****************************************************************************/ - -float setpoint, feedback, output; // Should these be volatile? -const float output_lower_limit = -1.0; +float kp, ki, kd; // Working gain vars +float working_setpoint; // Used for web parsing and updating +float setpoint; // This is used by PID objects, because + // encoders are not quadrature and + // do not have direction information + // we use this to store the absolute + // value of the working_setpoint. + // Specifically setpoint is used only + // for PID objects. Refer to + // pid_callback() function +float feedbackL, outputL; // Should these be volatile? +float feedbackR, outputR; // Should these be volatile? +const float output_lower_limit = 0.0; const float output_upper_limit = 1.0; -const float FEEDBACK_SCALE = 1.0/3000.0; // Scale feedback to 1rev/3000cnts +const float FEEDBACK_SCALE = 1.0/384.0; // Scale feedback to 1rev/3000cnts // this is encoder specific. -enum CONTROL_MODE{POSITION = 0, SPEED = 1}; -bool control_mode = POSITION; -float kp, ki, kd; // Gain variables for working +enum CONTROL_MODE{PID_OFF = 0, PID_ON = 1}; +int control_mode = PID_ON; const float Ts = 0.04; // 25Hz Sample Freq (40ms Sample Time) const float Ts_PID_CALLBACK = Ts/2.0; // Update Motors and sensers twice as // fast as PID sample rate, ensures // PID feedback is upto date every // time PID calculations run -// Vars to store gains for Speed and Position when switching Control modes -const float kp_init = 2.5; // Good Kp for Position Control -const float ki_init = 5.0; // Good Ki for Position Control -const float kd_init = 0.25; // Good Kd for Position Control +const float kp_init = 0.01; // Good Kp for Speed Control; Start with this +const float ki_init= 0.015; // Good Ki for Speed Control; Start with this +const float kd_init = 0.0001; // Good Kd for Speed Control; Start with this -PID pid(&setpoint, &feedback, &output, +PID pidL(&setpoint, &feedbackL, &outputL, output_lower_limit, output_upper_limit, - kp_init, ki_init, kd_init, Ts); // Init for position control -QEI encoder(p15, p16); -PwmOut mtr_pwm(p25); -DigitalOut mtr_dir(p24); + kp_init, ki_init, kd_init, Ts); +PID pidR(&setpoint, &feedbackR, &outputR, + output_lower_limit, output_upper_limit, + kp_init, ki_init, kd_init, Ts); +MotorDriver mtrR(p20, p19, p25, 10000.0, true); // in1, in2, pwm, pwmFreq, isBrakeable +MotorDriver mtrL(p18, p17, p24, 10000.0, true); // in1, in2, pwm, pwmFreq, isBrakeable +HALLFX_ENCODER encR(p21); +HALLFX_ENCODER encL(p22); void pid_callback(); // Updates encoder feedback and motor output -Ticker motor; +Ticker motor; // Interrupt for feedback and motor updates /*****************************************************************************/ /*****************************************************************************/ @@ -90,8 +103,8 @@ void init(char* buffer, int size); void getreply(int timeout_ms, char* buffer, int size, int numBytes); void startserver(char* buffer, int size); -void update_webpage(char* webpage, float setpoint, float kp, float ki, float kd); -void parse_input(char* webpage_user_data, bool* control_mode, float* setpoint, float* kp, float* ki, float* kd); +void update_webpage(char* webpage, float working_setpoint, float kp, float ki, float kd); +void parse_input(char* webpage_user_data, int* control_mode, float* working_setpoint, float* kp, float* ki, float* kd); int port =80; // set server port int serverTimeout_secs =5; // set server timeout in seconds in case // link breaks. @@ -108,7 +121,7 @@ /****************** Load Webpage from SD Card***************************************/ /***********************************************************************************/ - char file[] = "/sd/pid_dual.html"; + char file[] = "/sd/pid_bot.html"; // Get file size so we can dynamically allocate buffer size int num_chars = 0; @@ -141,7 +154,7 @@ } fclose(fp); printf("Webpage Buffer Size: %d bytes\r\n", sizeof(webpage)); - update_webpage(webpage, setpoint, kp_init, ki_init, kd_init); // Update Webpage for + update_webpage(webpage, working_setpoint, kp_init, ki_init, kd_init); // Update Webpage for // Position Mode /***********************************************************************************/ /***********************************************************************************/ @@ -162,13 +175,11 @@ /************Initialize the PID*****************************************************/ /***********************************************************************************/ - setpoint = 0.0; - encoder.reset(); - feedback = encoder.read(); - - // Init the motor - mtr_dir = 0; // Can be 0 or 1, sets the direction - mtr_pwm = 0.0; + working_setpoint = 0.0; + encL.reset(); + encR.reset(); + feedbackL = encL.read(); + feedbackR = encR.read(); // Update sensors and feedback twice as fast as PID sample time // this makes pid react in real-time avoiding errors due to @@ -176,7 +187,8 @@ motor.attach(&pid_callback, Ts_PID_CALLBACK); // Start PID sampling - pid.start(); + pidL.start(); + pidR.start(); /***********************************************************************************/ /***********************************************************************************/ @@ -216,34 +228,34 @@ printf("\r\n*************USER INPUT**********************************\r\n"); #endif - parse_input(buff, &control_mode, &setpoint, &kp, &ki, &kd); - setpoint = clip(setpoint, -999.99, 999.99); - kp = clip(kp, 0.00, 999.99); - ki = clip(ki, 0.00, 999.99); - kd = clip(kd, 0.00, 999.99); + parse_input(buff, &control_mode, &working_setpoint, &kp, &ki, &kd); + working_setpoint = clip(working_setpoint, -90.0, 90.0); // Redbot motors are ~90 RPM; max html field size for setpoint is 7 (to accomodate for a "-" sign + kp = clip(kp, 0.00, 999.99); // 999.99 is max size that can be updated to webpage, i.e. field is 6 digits (see html) + ki = clip(ki, 0.00, 999.99); // 999.99 is max size that can be updated to webpage, i.e. field is 6 digits (see html) + kd = clip(kd, 0.00, 999.99); // 999.99 is max size that can be updated to webpage, i.e. field is 6 digits (see html) #ifdef DEBUG printf("User Entered: \ncontrol_mode: %i\nSetpoint: %7.4f\nKp: %6.4f\nKi: %6.4f\nKd: %6.4f\n", - control_mode, setpoint, kp, ki, kd); + control_mode, working_setpoint, kp, ki, kd); #endif - pid.set_parameters(kp, ki, kd, Ts); // Updata PID params + pidL.set_parameters(kp, ki, kd, Ts); // Updata PID params + pidR.set_parameters(kp, ki, kd, Ts); // Updata PID params #ifdef DEBUG printf("Updated to Kp: %1.4f Ki: %1.4f Kd: %1.4f Ts: %1.4f\r\n", - pid.getKp(), pid.getKi(), pid.getKd(), pid.getTs()); - printf("Setpoint: %1.4f\r\n", setpoint); - printf("Output: %1.4f\r\n", output); + pidL.getKp(), pidL.getKi(), pidL.getKd(), pidL.getTs()); + printf("Setpoint: %1.4f\r\n", working_setpoint); printf("\r\n*************END USER INPUT******************************\r\n"); #endif // Update Webpage to reflect new values POSTED by client static bool isFirstRequest = true; - if(!isFirstRequest) update_webpage(webpage, setpoint, kp, ki, kd); + if(!isFirstRequest) update_webpage(webpage, working_setpoint, kp, ki, kd); else isFirstRequest = false; // First Request just send page with initial values #ifdef DEBUG - printf(webpage); // DEBUGGING ONLY!!! REMOVE FOR RELEASE!!! + printf(webpage); #endif // Command TCP/IP Data Tx @@ -292,9 +304,7 @@ } } /*********************************************************************/ - /*********************************************************************/ - - + /*********************************************************************/ } } @@ -375,7 +385,7 @@ IMPLEMENTATION OF THE parse_intput() function. MAKE SURE THESE TWO FUNCTIONS INTEGRATE PROPERLY!!! */ -void update_webpage(char* webpage, float setpoint, float kp, float ki, float kd){ +void update_webpage(char* webpage, float working_setpoint, float kp, float ki, float kd){ // Change output value to reflect new control mode, setpoint, kp, ki, kd values char* begin; char temp[8]; @@ -386,7 +396,7 @@ idx = 0; begin = strstr(webpage, "name=\"control_mode\" value=\"") + sizeof("name=\"control_mode\" value=\"0"); // Update Control Mode Position Radio Button - if(control_mode == POSITION) sprintf(temp, "%s", "checked");// If Position active "check" it + if(control_mode == PID_OFF) sprintf(temp, "%s", "checked");// If PID OFF active "check" it else sprintf(temp, "%s", " "); // else "clear" it while(temp[idx] != '\0'){ // Write "checked"/" " to field begin[idx] = temp[idx]; @@ -398,7 +408,7 @@ sizeof("name=\"control_mode\" value=\"0\""); // Nav to first Control Mode Radio Button (Position) begin = strstr(begin, "name=\"control_mode\" value=\"") + sizeof("name=\"control_mode\" value=\"1"); // Nav to second Control Mode Radio Button (Speed) - if(control_mode == SPEED) sprintf(temp, "%s", "checked"); // If Speed active "check" it + if(control_mode == PID_ON) sprintf(temp, "%s", "checked"); // If PID ON active "check" it else sprintf(temp, "%s", " "); // else "clear" it while(temp[idx] != '\0'){ // Write "checked"/" " to field begin[idx] = temp[idx]; @@ -460,15 +470,15 @@ sizeof("name=\"setpoint_input\" value="); // Points to start of kp_output field // Determine precision of float such temp string has no empty spaces; // i.e. each space must have a value or a decimal point, other wise webbrowser may not recognize value - if(setpoint >= 0.00){ - if(setpoint >= 100) sprintf(temp, "%6.3f", setpoint); // xxx.000 - else if(10 <= setpoint && setpoint < 100) sprintf(temp, "%7.4f", setpoint); // xx.0000 - else sprintf(temp, "%6.5f", setpoint); // x.00000 + if(working_setpoint >= 0.00){ + if(working_setpoint >= 100) sprintf(temp, "%6.3f", working_setpoint); // xxx.000 + else if(10 <= working_setpoint && working_setpoint < 100) sprintf(temp, "%7.4f", working_setpoint); // xx.0000 + else sprintf(temp, "%6.5f", working_setpoint); // x.00000 } else{ - if(setpoint <= -100) sprintf(temp, "%6.2f", setpoint); // -xxx.00 - else if(-100 < setpoint && setpoint <= -10) sprintf(temp, "%6.3f", setpoint); // -xx.000 - else sprintf(temp, "%6.4f", setpoint); // -x.0000 + if(working_setpoint <= -100) sprintf(temp, "%6.2f", working_setpoint); // -xxx.00 + else if(-100 < working_setpoint && working_setpoint <= -10) sprintf(temp, "%6.3f", working_setpoint); // -xx.000 + else sprintf(temp, "%6.4f", working_setpoint); // -x.0000 } while(temp[idx] != '\0'){ // Overwrite old digits with new digits begin[idx] = temp[idx]; @@ -490,7 +500,7 @@ THE update_webpage() function. MAKE SURE THESE TWO FUNCTIONS INTEGRATE PROPERLY!!! */ -void parse_input(char* webpage_user_data, bool* control_mode, float *setpoint, float* kp, float* ki, float* kd){ +void parse_input(char* webpage_user_data, int* control_mode, float *working_setpoint, float* kp, float* ki, float* kd){ char keys[] = {'&', '\0'}; // Parse out user input values @@ -516,7 +526,7 @@ for(long i = 0; i < end - begin; i++){ // Parse out the value one char at a time input_buff[i] = begin[i]; } - *setpoint = atof(input_buff); + *working_setpoint = atof(input_buff); // Parse and Update Kp Value memset(input_buff, '\0', sizeof(input_buff)); // Null out input buff @@ -547,51 +557,79 @@ input_buff[i] = begin[i]; } *kd = atof(input_buff); + + // Parse for stop button press; we only have to see if "STOP" exists in the input buffer + // because it is just a button and will not be included unless it is pressed!... Makes + // our job easy!... if stop was pressed then set setpoint to zero! + if(strstr(webpage_user_data, "STOP") != NULL) *working_setpoint = 0; } void pid_callback(){ - // If control_mode is POSITION run position pid - if(control_mode == POSITION){ - // Update motor - if(output >= 0.0) mtr_dir = 1; // Set direction to sign of output - else mtr_dir = 0; - mtr_pwm = abs(output); // Apply motor output - - // Update feedback - feedback = encoder.read()*FEEDBACK_SCALE;// Scale feedback to num wheel revs + static int lastMode = control_mode; + + // If control_mode is PID_OFF turn off PID controllers and run motors in open loop + if(control_mode == PID_OFF){ + // Mode changed; turn off PID controllers + if(lastMode != control_mode){ + pidL.stop(); pidR.stop(); + lastMode = PID_OFF; + } + // Set motor speed in open loop mode + // Motor direction based on working setpoint var + int dirL, dirR; + if(working_setpoint < 0.0){ + dirL = -1; dirR = -1; + } + else{ + dirL = 1; dirR = 1; + } + float speed = abs(working_setpoint) / 90.0; // Normalize based on 90 RPM + mtrL.forceSetSpeed(speed * dirL); + mtrR.forceSetSpeed(speed * dirR); } - // else control_mode must be SPEED, run speed pid + // else control_mode is PID_ON, turn on PID controllers and run motors in closed loop else{ - if(setpoint >= 0.0) mtr_dir = 1; // Set motor direction based on setpoint - else mtr_dir = 0; - if(-0.001 < setpoint && setpoint < 0.001){ - /* Setpoint = 0 is a special case, we allow output to control speed AND - direction to fight intertia and/or downhill roll. */ - if(output >= 0.0) mtr_dir = 1; - else mtr_dir = 0; - mtr_pwm = abs(output); + // Mode changed; turn on PID controllers + if(lastMode != control_mode){ + pidL.start(); pidR.start(); + lastMode = PID_ON; } - else{ - if(mtr_dir == 1){ // If CW then apply positive outputs - if(output >= 0.0) mtr_pwm = output; - else mtr_pwm = 0.0; - } - else{ // If CCW then apply negative outputs - if(output <= 0.0) mtr_pwm = abs(output); - else mtr_pwm = 0.0; - } - } - float k = Ts/2.0; // Discrete time, (Ts/2 because this callback is called - // at interval of Ts/2... or twice as fast as pid controller) + // Deal with feedback and update motors + // Motor direction based on working setpoint var + int dirL, dirR; + if(working_setpoint < 0.0){ + dirL = -1; dirR = -1; + } + else{ + dirL = 1; dirR = 1; + } - /* TODO: Implement a "rolling"/"moving" average */ - static int last_count = 0; - int count = encoder.read(); - float raw_speed = ((count - last_count)*FEEDBACK_SCALE) / k; - float rpm_speed = raw_speed * 60.0; // Convert speed to RPM + // Setpoint vars used by PID objects are concerned with + // only SPEED not direction. + setpoint = abs(working_setpoint); + + float k = Ts_PID_CALLBACK; // Discrete time, (Ts/2 because this callback is called + // at interval of Ts/2... or twice as fast as pid controller) + static int last_count_L = 0; + static int last_count_R = 0; + int countL = encL.read(); + int countR = encR.read(); - last_count = count; // Save last count - feedback = rpm_speed; + // Because encoders are not quadrature we must handle the sign outselves, + // i.e. explicitly make calcs based on the direction we have set the motor + float raw_speed_L = ((countL - last_count_L)*FEEDBACK_SCALE) / k; + float rpm_speed_L = raw_speed_L * 60.0; // Convert speed to RPM + + float raw_speed_R = ((countR - last_count_R)*FEEDBACK_SCALE) / k; + float rpm_speed_R = raw_speed_R * 60.0; // Convert speed to RPM + + last_count_L = countL; // Save last count + last_count_R = countR; + feedbackL = rpm_speed_L; + feedbackR = rpm_speed_R; + + mtrL.forceSetSpeed(outputL * dirL); + mtrR.forceSetSpeed(outputR * dirR); } } @@ -611,44 +649,35 @@ Copy and past text below into a html file and save as the given file name to your SD card. -file name: pid_dual.html +file name: pid_bot.html html text: <!DOCTYPE html> <html> <head> -<title>PID Motor Control</title> +<title>PID RedBot Control</title> </head> <body> <h1>PID Motor Control</h1> -<h2>Motor Status</h2> -<p> -<form title="Motor Status"> -<input type="text" value="Some user information" size="25" readonly /><br> -Current Setpoint: -<input type="number" name="current_setpoint" value="0000.00" readonly /><br> -Current Position: -<input type="number" name="current_position" value="0000.00" readonly /><br> -</form> -</p> <h2>PID Status</h2> <form title="User Input" method="post"> PID Controls: <br> -<input type="radio" name="control_mode" value="0"checked>Position(#Revolutions) +<input type="radio" name="control_mode" value="0"checked>PID OFF <br> -<input type="radio" name="control_mode" value="1" >Speed(RPM) +<input type="radio" name="control_mode" value="1" >PID ON <br> -Setpoint: +Setpoint:<br> <input type="number" name="setpoint_input" value="0000.00" step="0.0000001" size="6" /><br> -Proportional Gain: (Good Starting Values: Position = 2.50 Speed = 0.01)<br> -<input type="number" name="kp_input" value="002.50" step="0.0000001" size="6" /><br> -Integral Gain: (Good Starting Values: Position = 5.0 Speed = 0.015)<br> -<input type="number" name="ki_input" value="005.00" step="0.0000001" size="6" /><br> -Derivative Gain: (Good Starting Values: Position = 0.25 Speed = 0.0001)<br> -<input type="number" name="kd_input" value="000.25" step="0.0000001" size="6" /><br> +Proportional Gain: (Good Starting Value: 0.01)<br> +<input type="number" name="kp_input" value="000.01" step="0.0000001" size="6" /><br> +Integral Gain: (Good Starting Value: 0.015)<br> +<input type="number" name="ki_input" value="00.015" step="0.0000001" size="6" /><br> +Derivative Gain: (Good Starting Value: 0.0001)<br> +<input type="number" name="kd_input" value="0.0001" step="0.0000001" size="6" /><br> <br> <input type="submit" value="Update" /> +<input type="submit" name="STOP" value="STOP!" /> </form> </body> </html>