Isaac Lee
Bluetooth app controlled robot
Dependencies: ESP8266 HALLFX_ENCODER LSM9DS1_Library_cal MotorDriver PID mbed
Fork of
ESP8266_pid_redbot_webserver
by 
Bryce Williams
Diff: main.cpp
- Revision:
- 1:d4a95e3a8aeb
- Parent:
- 0:11bc7a815367
- Child:
- 2:3466e9e16c99
--- 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>