Revision 1:d4a95e3a8aeb, committed 2015-12-08

Comitter:

electromotivated

Date:

Tue Dec 08 00:17:04 2015 +0000

Parent:

0:11bc7a815367

Commit message:

Upload;

Changed in this revision

diff -r 11bc7a815367 -r d4a95e3a8aeb HALLFX_ENCODER.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HALLFX_ENCODER.lib	Tue Dec 08 00:17:04 2015 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/users/electromotivated/code/HALLFX_ENCODER/#f10558519825

diff -r 11bc7a815367 -r d4a95e3a8aeb MotorDriver.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MotorDriver.lib	Tue Dec 08 00:17:04 2015 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/users/electromotivated/code/MotorDriver/#871adb9cf798

diff -r 11bc7a815367 -r d4a95e3a8aeb QEI.lib
--- a/QEI.lib	Fri Nov 27 15:30:19 2015 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://developer.mbed.org/users/electromotivated/code/QEI/#50aae578cb89

diff -r 11bc7a815367 -r d4a95e3a8aeb main.cpp
--- 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>