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:
- 2:3466e9e16c99
- Parent:
- 1:d4a95e3a8aeb
- Child:
- 3:bf6e71964ceb
--- a/main.cpp Tue Dec 08 00:17:04 2015 +0000
+++ b/main.cpp Wed Mar 16 16:03:44 2016 +0000
@@ -1,51 +1,17 @@
-/*
- Uses the ESP8266 WiFi Chip to set up a WiFi Webserver used to control
- the speed of a Sparkfun Redbot Rotbot. USE FIREFOX
- Web Browser
-
- NOTES:
- 1. Webpage Handling in this program is specific to a CUSTOM
- WEBPAGE. Program must be modified to handle specfically a new
- webpage. A copy of the webpage for this program can be found at
- the end of this program page. Simply copy and past text into a
- html file and save as the given name.
-
- 2. Developed and tested with FireFox 42.0 Web Browser. Does not seem to work
- well with Google Chrome or Internet Explorer for some reason... they seem
- to generate two post requests which messes with the user input values.
-
- 3. There are a bunch of printf statements in the code that can be
- uncommented for debugging in a serial terminal progrom.
-
-
- TODO: ESP8366 has a max packet send size. Make sure we implement
- a method to send webpages that exceed this value. The max size is
- listed in the official ESP8266 AT Commands Documentation, I think
- it is 2048 bytes/chars
-
- TODO: CREATE CONFIG FUNCTION TO SET SSID, PASSWORD, BAUDRATE ETC.
- Perhaps have a serial terminal method to take user input, and
- put the function call into a #ifdef WiFiConfig statement, so
- that the user can enable it to config Wifi module then turn
- it off once Wifi module is configed so that this program can
- run in a "stand alone" mode.
-
- 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 "HALLFX_ENCODER.h"
#include "MotorDriver.h"
+#include "LSM9DS1.h"
#include <algorithm>
+#define PI 3.14159
+#define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.
+#define REFRESH_TIME_MS 1
+#define DEBUG
-//#define DEBUG // Uncomment for serial terminal print debugging
- // Comment out to turn prints off for normal op
-
-/*********PID CONTROLLER SPECIFIC DECLARATIONS********************************/
-/*****************************************************************************/
+Serial blue(p28,p27);
+BusOut myled(LED1,LED2,LED3,LED4);
+Serial pc(USBTX, USBRX);
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
@@ -62,8 +28,6 @@
const float output_upper_limit = 1.0;
const float FEEDBACK_SCALE = 1.0/384.0; // Scale feedback to 1rev/3000cnts
// this is encoder specific.
-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
@@ -85,97 +49,80 @@
HALLFX_ENCODER encR(p21);
HALLFX_ENCODER encL(p22);
void pid_callback(); // Updates encoder feedback and motor output
+
Ticker motor; // Interrupt for feedback and motor updates
-/*****************************************************************************/
-/*****************************************************************************/
-
-/**********WEB SERVER SPECIFIC DECLARTATIONS**********************************/
-/*****************************************************************************/
-SDFileSystem sd(p5,p6,p7,p8,"sd"); // MOSI, MISO, SCLK, CS,
- // Virtual File System Name
-Serial esp(p13, p14); // tx, rx
-DigitalOut espRstPin(p26); // ESP Reset
-DigitalOut led(LED4);
+//global variables for main and interrupt routine
-Timer t1;
-Timer t2;
+char state_num = 0, bnum =0 ;
+int dirL = 0, dirR = 0;
-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 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.
-/*****************************************************************************/
-/*****************************************************************************/
+//Interrupt routine to parse message with one new character per serial RX interrupt
-// Common Application Declarations
-Serial pc(USBTX, USBRX);
float clip(float value, float lower, float upper);
+LSM9DS1 imu(p9, p10, 0xD6, 0x3C);
+void calculate_heading(float mx, float my, float mz);
+float oldX = 0;
+float oldY = 0;
+float oldZ = 0;
+float x= 0;
+float y = 0;
+float z = 0;
+
+float posx = 0;
+float velx = 0;
+float oldPosx = 0;
+float oldVelx = 0;
+
+float posy = 0;
+float vely = 0;
+float oldPosy = 0;
+float oldVely = 0;
+
+float posz = 0;
+float velz = 0;
+float oldPosz = 0;
+float oldVelz = 0;
+int sample_time = 0;
+
+float accelx = 0, accely = 0, accelz = 0;
+Serial esp(p13, p14); // tx, rx
+DigitalOut reset(p26);
+
+Timer t;
+void send_data(float *accelx, float *accely, float *accelz, float *velx, float *vely, float *velz);
+int count,ended,timeout;
+float heading = 0.0;
+char buf[1024];
+char snd[1024];
+char str_buf[80];
+
+char ssid[32] = "4180_test"; // enter WiFi router ssid inside the quotes
+char pwd [32] = "4180isawesome"; // enter WiFi router password inside the quotes
+
+void SendCMD(),getreply(),ESPconfig(),ESPsetbaudrate();
int main()
{
- printf("Starting\n");
-
- /****************** Load Webpage from SD Card***************************************/
- /***********************************************************************************/
- char file[] = "/sd/pid_bot.html";
+ pc.printf("\f---------- Program Start ----------\r\n\n");
+ reset=0; //hardware reset for 8266
+ pc.baud(9600); // set what you want here depending on your terminal program speed
+ pc.printf("\f\n\r-------------ESP8266 Hardware Reset-------------\n\r");
+ wait(0.5);
+ reset=1;
+ timeout=2;
+ getreply();
- // Get file size so we can dynamically allocate buffer size
- int num_chars = 0;
- FILE *fp = fopen(file, "r");
- while(!feof(fp)){
- fgetc(fp);
- num_chars++;
- }
- rewind(fp); // Go to beginning of file
+ ESPconfig(); //****************** include Config to set the ESP8266 configuration ***********************
- #ifdef DEBUG
- printf("Webpage Data Size: %d byte\r\n", num_chars);
- #endif
-
- const int WEBPAGE_SIZE = num_chars;
- char webpage[WEBPAGE_SIZE];
- webpage[0] = NULL; // Init our array so that element zero contains a null
- // This is important, ensures strings are placed into
- // buffer starting at element 0... not some random
- // elment
- // Read in and buffer file to memory
- if(fp == NULL){
- printf("Error: No Such File or something :(");
- return 1;
+ imu.begin();
+ if (!imu.begin()) {
+ pc.printf("Failed to communicate with LSM9DS1.\n");
}
- else{
- while(!feof(fp)){
- fgets(webpage + strlen(webpage), WEBPAGE_SIZE, fp); // Get a string from stream, add to buffer
- }
- }
- fclose(fp);
- printf("Webpage Buffer Size: %d bytes\r\n", sizeof(webpage));
- update_webpage(webpage, working_setpoint, kp_init, ki_init, kd_init); // Update Webpage for
- // Position Mode
- /***********************************************************************************/
- /***********************************************************************************/
+ imu.calibrate(1);
+ imu.calibrateMag(0);
+
+ working_setpoint = 0.0;
- /***************BRING UP SERVER*****************************************************/
- /***********************************************************************************/
- char buff[5000]; // Working buffer
- init(buff, sizeof(buff)); // Init ESP8266
-
- esp.baud(115200); // ESP8266 baudrate. Maximum on KLxx' is 115200, 230400 works on K20 and K22F
-
- startserver(buff, sizeof(buff)); // Configure the ESP8266 and Setup as Server
-
- printf(buff); // If start successful buff contains IP address...
- // if not if contains an error.
- /***********************************************************************************/
- /***********************************************************************************/
-
- /************Initialize the PID*****************************************************/
- /***********************************************************************************/
- working_setpoint = 0.0;
encL.reset();
encR.reset();
feedbackL = encL.read();
@@ -184,453 +131,327 @@
// Update sensors and feedback twice as fast as PID sample time
// this makes pid react in real-time avoiding errors due to
// missing counts etc.
+
motor.attach(&pid_callback, Ts_PID_CALLBACK);
-
+
// Start PID sampling
pidL.start();
pidR.start();
-
- /***********************************************************************************/
- /***********************************************************************************/
-
- while(1){
- /**************SERVICE WEBPAGE******************************************************/
- /***********************************************************************************/
- if(esp.readable()){
- getreply(500, buff, sizeof(buff), sizeof(buff) -1); // Get full buff, leave last element for null char
- #ifdef DEBUG
- printf("\r\n*************WORKING BUFFER******************************\r\n");
- printf(buff); printf("\n");
- printf("\r\n**************END WORKING BUFFER**************************\r\n");
- #endif
- // If Recieved Data get ID, Length, and Data
- char* rqstPnt = strstr(buff, "+IPD");
- if(rqstPnt != NULL){
- int id, len;
- char type[10]; memset(type, '\0', sizeof(type)); // Create and null out data buff
- sscanf(rqstPnt, "+IPD,%d,%d:%s ", &id, &len, type);
- #ifdef DEBUG
- printf("ID: %i\nLen: %i\nType: %s\n", id, len, type);
- #endif
- // If GET or POST request "type" parse and update user input then send webpage
- if(strstr(type, "GET") != NULL || strstr(type, "POST") != NULL){
- #ifdef DEBUG
- printf("I got web request\n");
- #endif
- /* Read Webpage <Form> data sent using "method=POST"...
- Note: Input elements in the <Form> need a set name attribute to
- appear in the returned HTML body. Thus to "POST" data ensure:
- <Form method="POST"> <input type="xxx" name="xxx" value="xxx">
- <input type="xxx" value="xxx"> </Form>
- Only the input with name="xxx" will appear in body of HTML
- */
- #ifdef DEBUG
- printf("\r\n*************USER INPUT**********************************\r\n");
- #endif
-
- 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, working_setpoint, kp, ki, kd);
- #endif
-
- 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",
- 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, working_setpoint, kp, ki, kd);
- else isFirstRequest = false; // First Request just send page with initial values
-
- #ifdef DEBUG
- printf(webpage);
- #endif
-
- // Command TCP/IP Data Tx
- esp.printf("AT+CIPSEND=%d,%d\r\n", id, strlen(webpage));
- getreply(200, buff, sizeof(buff), 15); /*TODO: Wait for "OK\r\n>"*/
-
- #ifdef DEBUG
- printf(buff); printf("\n");
- #endif
-
- // Send webpage
-// while(!esp.writeable()); // Wait until esp ready to send data
- int idx = 0;
- while(webpage[idx] != '\0'){
- esp.putc(webpage[idx]);
- idx++;
+ working_setpoint = 0;
+ dirL = 1;
+ dirR = 1;
+ while(1)
+ {
+ if(blue.readable())
+ {
+ if (blue.getc()=='!')
+ {
+ if (blue.getc()=='B') //button data
+ {
+ bnum = blue.getc(); //button number
+ if(bnum=='1') {
+ working_setpoint = 50;
+ sample_time = 1;
+ pc.printf("setpoint: %2.0f\r\n", working_setpoint);
}
-
- // Check status - Success: close channel and update PID controller, Error: reconnect
- bool weberror = true;
- t2.reset(); t2.start();
- while(weberror ==1 && t2.read_ms() < 5000){
- getreply(500, buff, sizeof(buff), 24);
- if(strstr(buff, "SEND OK") != NULL) weberror = false;
- }
- if(weberror){
- esp.printf("AT+CIPMUX=1\r\n");
- getreply(500, buff, sizeof(buff), 10);
- #ifdef DEBUG
- printf(buff); printf("\n");
- #endif
- esp.printf("AT+CIPSERVER=1,%d\r\n", port);
- getreply(500, buff, sizeof(buff), 10);
- #ifdef DEBUG
- printf(buff); printf("\n");
- #endif
- }
- else{
- esp.printf("AT+CIPCLOSE=%d\r\n", id); // Notice id is an int formatted to string
- getreply(500, buff, sizeof(buff), 24);
- #ifdef DEBUG
- printf(buff); printf("\n");
- #endif
- }
- }
+ }
}
+ }
+ while(!imu.accelAvailable());
+ imu.readAccel();
+ while(!imu.magAvailable());
+ imu.readMag();
+
+ if( sample_time % 800 == 0)
+ {
+ velx = oldVelx + REFRESH_TIME_MS * oldX/1000;
+ posx = oldPosx + REFRESH_TIME_MS * oldVelx/1000;
+
+ vely = oldVely + REFRESH_TIME_MS * oldY/1000;
+ posy = oldPosy + REFRESH_TIME_MS * oldVely/1000;
+
+ accelx = imu.calcAccel(imu.ax);
+ accely = imu.calcAccel(imu.ay);
+ accelz = imu.calcAccel(imu.az);
+ calculate_heading(imu.calcMag(imu.mx), imu.calcMag(imu.my), imu.calcMag(imu.mz));
+
+ #ifdef DEBUG
+ pc.printf(" X axis Y axis Z axis\n\r");
+ pc.printf("accel: %2.2f %2.2f %2.2f in Gs\r\n", imu.calcAccel(imu.ax), imu.calcAccel(imu.ay), imu.calcAccel(imu.az));
+ pc.printf("Veloc: %2.2f %2.2f %2.2f in G*s\r\n", velx, vely, velz);
+ pc.printf("Magnetic heading: %2.2f", heading);
+ #endif
+
+ oldVelx = velx;
+ oldPosx = posx;
+ oldVely = vely;
+ oldPosy = posy;
+ oldX = imu.ax;
+ oldY = imu.ay;
}
- /*********************************************************************/
- /*********************************************************************/
- }
-}
-
-// Initialize ESP8266
-void init(char* buffer, int size){
- // Hardware Reset ESP
- espRstPin=0;
- wait(0.5);
- espRstPin=1;
- // Get start up junk from ESP8266
- getreply(6000, buffer, size, 500);
-}
-
-// Get Command and ESP status replies
-void getreply(int timeout_ms, char* buffer, int size, int numBytes)
-{
- memset(buffer, '\0', size); // Null out buffer
- t1.reset();
- t1.start();
- int idx = 0;
- while(t1.read_ms()< timeout_ms && idx < numBytes) {
- if(esp.readable()) {
- buffer[idx] = esp.getc();
- idx++;
+ if(working_setpoint > 0)
+ {
+ if(sample_time % 1500 == 0)
+ {
+ #ifdef DEBUG
+ pc.printf("Pushing data to server\r\n");
+ #endif
+
+ myled[0] = !myled[0];
+ working_setpoint = 0;
+
+ strcpy(snd, "srv:listen(80,function(conn)\r\n");
+ SendCMD();
+ wait(1);
+ strcpy(snd, "conn:on(\"receive\",function(conn,payload)\r\n");
+ SendCMD();
+ wait(1);
+ strcpy(snd, "print(payload)\r\n");
+ SendCMD();
+ wait(1);
+ strcpy(snd, "conn:send(\"<!DOCTYPE html>\")\r\n");
+ SendCMD();
+ wait(1);
+
+ strcpy(snd, "conn:send(\"<html>\")\r\n");
+ SendCMD();
+ wait(1);
+ sprintf(str_buf,"%2.2f",velx);
+ strcpy(snd, "conn:send(\"<h1>Velocity x: ");
+ strcat(snd, str_buf);
+ strcat(snd, "</h1>\")\r\n");
+ SendCMD();
+ wait(1);
+
+ sprintf(str_buf,"%2.2f",vely);
+ strcpy(snd, "conn:send(\"<h1>Velocity y: ");
+ strcat(snd, str_buf);
+ strcat(snd, "</h1>\")\r\n");
+ SendCMD();
+ wait(1);
+
+ sprintf(str_buf,"%2.2f",heading);
+ strcpy(snd, "conn:send(\"<h1>Magnetic Heading: ");
+ strcat(snd, str_buf);
+ strcat(snd, "</h1>\")\r\n");
+ SendCMD();
+ wait(1);
+
+ strcpy(snd, "conn:send(\"</html>\")\r\n");
+ SendCMD();
+ wait(1);
+
+ strcpy(snd, "end)\r\n");
+ SendCMD();
+ wait(1);
+
+ strcpy(snd, "conn:on(\"sent\",function(conn) conn:close() end)\r\n");
+ SendCMD();
+ wait(1);
+ strcpy(snd, "end)\r\n");
+ SendCMD();
+ wait(1);
+ timeout=17;
+ getreply();
+ pc.printf(buf);
+ pc.printf("\r\nDONE");
+ }
+ if(sample_time > 100000)
+ {
+ sample_time = 1;
+ }
+ sample_time++;
}
}
- t1.stop();
-}
-
-// Starts and restarts webserver if errors detected.
-void startserver(char* buffer, int size)
-{
- esp.printf("AT+RST\r\n"); // BWW: Reset the ESP8266
- getreply(8000, buffer, size, 1000);
-
- if (strstr(buffer, "OK") != NULL) {
- // BWW: Set ESP8266 for multiple connections
- esp.printf("AT+CIPMUX=1\r\n");
- getreply(500, buffer, size, 20);
-
- // BWW: Set ESP8266 as Server on given port
- esp.printf("AT+CIPSERVER=1,%d\r\n", port);
- getreply(500, buffer, size, 20); // BWW: Wait for reply
-
- // BWW: Set ESP8266 Server Timeout
- esp.printf("AT+CIPSTO=%d\r\n", serverTimeout_secs);
- getreply(500, buffer, size, 50); // BWW: Wait for reply
-
- // BWW: Request IP Address from router for ESP8266
- int weberror = 0;
- while(weberror==0) {
- esp.printf("AT+CIFSR\r\n");
- getreply(2500, buffer, size, 200);
- if(strstr(buffer, "0.0.0.0") == NULL) {
- weberror=1; // wait for valid IP
- }
- }
- }
- // else ESP8266 did not reply "OK" something is messed up
- else {
- strcpy(buffer, "ESP8266 Error\n");
- }
}
-/*
- update_webpage() updates output fields based on webpage user inputs "POSTED"
- Preconditions: webpage[] must have the following elements
- "kp_output" value="xxx.xx"
- "ki_output" value="xxx.xx"
- "kp_output" value="xxx.xx"
- @param webpage Pointer to webpage char[]
- @param kp New kp value posted by user
- @param ki New ki value posted by user
- @param kd New kd value posted by user
-
- NOTE: THIS IS WEBPAGE SPECIFIC!!!! CHANGE THE CODE IN HERE TO SUITE THE
- SPECIFIC APPLICATION WEBPAGE!!! ALSO USED TO REFLECT THE CUSTOM
- IMPLEMENTATION OF THE parse_intput() function. MAKE SURE THESE TWO FUNCTIONS
- INTEGRATE PROPERLY!!!
-*/
-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];
- int idx;
-
- // Update Control Mode Radio Buttons
- memset(temp, '\0', sizeof(temp));
- idx = 0;
- begin = strstr(webpage, "name=\"control_mode\" value=\"") +
- sizeof("name=\"control_mode\" value=\"0"); // Update Control Mode Position Radio Button
- 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];
- idx++;
- }
- memset(temp, '\0', sizeof(temp));
- idx = 0;
- begin = strstr(webpage, "name=\"control_mode\" value=\"") +
- 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 == 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];
- idx++;
- }
+void ESPconfig()
+{
+ wait(5);
+ pc.printf("\f---------- Starting ESP Config ----------\r\n\n");
+ strcpy(snd,".\r\n.\r\n");
+ SendCMD();
+ wait(1);
+ pc.printf("---------- Reset & get Firmware ----------\r\n");
+ strcpy(snd,"node.restart()\r\n");
+ SendCMD();
+ timeout=5;
+ getreply();
+ pc.printf(buf);
+
+ wait(2);
+
+ pc.printf("\n---------- Get Version ----------\r\n");
+ strcpy(snd,"print(node.info())\r\n");
+ SendCMD();
+ timeout=4;
+ getreply();
+ pc.printf(buf);
+ wait(3);
+
+ // set CWMODE to 1=Station,2=AP,3=BOTH, default mode 1 (Station)
+ pc.printf("\n---------- Setting Mode ----------\r\n");
+ strcpy(snd, "wifi.setmode(wifi.STATION)\r\n");
+ SendCMD();
+ timeout=4;
+ getreply();
+ pc.printf(buf);
+
+ wait(2);
+
+ pc.printf("\n---------- Connecting to AP ----------\r\n");
+ pc.printf("ssid = %s pwd = %s\r\n",ssid,pwd);
+ strcpy(snd, "wifi.sta.config(\"");
+ strcat(snd, ssid);
+ strcat(snd, "\",\"");
+ strcat(snd, pwd);
+ strcat(snd, "\")\r\n");
+ SendCMD();
+ timeout=10;
+ getreply();
+ pc.printf(buf);
+
+ wait(5);
+
+ pc.printf("\n---------- Get IP's ----------\r\n");
+ strcpy(snd, "print(wifi.sta.getip())\r\n");
+ SendCMD();
+ timeout=3;
+ getreply();
+ pc.printf(buf);
+
+ wait(1);
+
+ pc.printf("\n---------- Get Connection Status ----------\r\n");
+ strcpy(snd, "print(wifi.sta.status())\r\n");
+ SendCMD();
+ timeout=5;
+ getreply();
+ pc.printf(buf);
+
+ pc.printf("\n---------- Setting up http server ----------\r\n");
+ strcpy(snd, "srv=net.createServer(net.TCP)\r\n");
+ SendCMD();
+ wait(1);
+ strcpy(snd, "srv:listen(80,function(conn)\r\n");
+ SendCMD();
+ wait(1);
+ strcpy(snd, "conn:on(\"receive\",function(conn,payload)\r\n");
+ SendCMD();
+ wait(1);
+ strcpy(snd, "print(payload)\r\n");
+ SendCMD();
+ wait(1);
+ strcpy(snd, "conn:send(\"<!DOCTYPE html>\")\r\n");
+ SendCMD();
+ wait(1);
- // Update Kp Paramater Field
- memset(temp, '\0', sizeof(temp));
- idx = 0;
- begin = strstr(webpage, "name=\"kp_input\" value=\"") +
- sizeof("name=\"kp_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(kp >= 100) sprintf(temp, "%6.2f", kp); // xxx.00
- else if(10 <= kp && kp < 100) sprintf(temp, "%6.3f", kp); // xx.000
- else sprintf(temp, "%6.4f", kp); // x.0000
- while(temp[idx] != '\0'){ // Overwrite old digits with new digits
- begin[idx] = temp[idx];
- idx++;
- }
+ strcpy(snd, "conn:send(\"<html>\")\r\n");
+ SendCMD();
+ wait(1);
- // Update Ki Parameter Field
- memset(temp, '\0', sizeof(temp));
- idx = 0;
- begin = strstr(webpage, "name=\"ki_input\" value=\"") +
- sizeof("name=\"ki_input\" value="); // Points to start of ki_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(ki >= 100) sprintf(temp, "%6.2f", ki); // xxx.00
- else if(10 <= ki && ki < 100) sprintf(temp, "%6.3f", ki); // xx.000
- else sprintf(temp, "%6.4f", ki); // x.0000
- while(temp[idx] != '\0'){ // Overwrite old digits with new digits
- begin[idx] = temp[idx];
- idx++;
- }
+ strcpy(snd, "conn:send(\"<h1>WIFI Monitoring Program</h1>\")\r\n");
+ SendCMD();
+ wait(1);
+
+ strcpy(snd, "conn:send(\"</html>\")\r\n");
+ SendCMD();
+ wait(1);
- // Update Kd Parameter Field
- memset(temp, '\0', sizeof(temp));
- idx = 0;
- begin = strstr(webpage, "name=\"kd_input\" value=\"")+
- sizeof("name=\"kd_input\" value="); // Points to start of kd_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(kd >= 100) sprintf(temp, "%6.2f", kd); // xxx.00
- else if(10 <= kd && kd < 100) sprintf(temp, "%6.3f", kd); // xx.000
- else sprintf(temp, "%6.4f", kd); // x.0000
- while(temp[idx] != '\0'){ // Overwrite old digits with new digits
- begin[idx] = temp[idx];
- idx++;
- }
+ strcpy(snd, "end)\r\n");
+ SendCMD();
+ wait(1);
- // Update Setpoint Parameter Field
- // Determine precision of float such temp string has no empty spaces;
- // i.e. each space must have a value or a decimal point or neg sign,
- // other wise webbrowser may not recognize value
- memset(temp, '\0', sizeof(temp));
- idx = 0;
- begin = strstr(webpage, "name=\"setpoint_input\" value=\"")+
- 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(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(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];
- idx++;
- }
+ strcpy(snd, "conn:on(\"sent\",function(conn) conn:close() end)\r\n");
+ SendCMD();
+ wait(1);
+ strcpy(snd, "end)\r\n");
+ SendCMD();
+ wait(1);
+ timeout=17;
+ getreply();
+ pc.printf(buf);
+ pc.printf("\r\nDONE");
+
}
-/*
- parse_input() take a char*, in particular a pointer to Webpage User
- Input Data, for example:
- char str[] = "+IPD,0,44:kp_input=0.12&ki_input=14.25&kd_input=125.42";
-
- and parses out the Setpoint Kp, Ki, Kd values that the user entered
- and posted in the webpage. Values are converted to floats and
- assigned to the given argurments.
-
- NOTE: THIS IS WEBPAGE SPECIFIC!!!! CHANGE THE CODE IN HERE TO SUITE THE
- SPECIFIC APPLICATION WEBPAGE!!! THESE EXTRACTED VALUES WILL BE USED IN
- THE update_webpage() function. MAKE SURE THESE TWO FUNCTIONS INTEGRATE
- PROPERLY!!!
-*/
-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
- char input_buff[50];
- char* begin;
- char* end;
-
- // Parse and Update Control Mode Value
- memset(input_buff, '\0', sizeof(input_buff)); // Null out input buff
- begin = strstr(webpage_user_data, "control_mode=") +
- sizeof("control_mode"); // Points to start of setpoint_input value
- end = begin + strcspn(begin, keys); // Points to end of setpoint_input value
- for(long i = 0; i < end - begin; i++){ // Parse out the value one char at a time
- input_buff[i] = begin[i];
+void SendCMD()
+{
+ esp.printf("%s", snd);
+}
+
+void getreply()
+{
+ memset(buf, '\0', sizeof(buf));
+ t.start();
+ ended=0;
+ int count=0;
+ while(!ended) {
+ if(esp.readable()) {
+ buf[count] = esp.getc();
+ count++;
+ }
+ if(t.read() > timeout) {
+ ended = 1;
+ t.stop();
+ t.reset();
+ }
}
- *control_mode = atoi(input_buff);
+}
+void calculate_heading(float mx, float my, float mz)
+{
+
+// touchy trig stuff to use arctan to get compass heading (scale is 0..360)
+ mx = -mx;
+ if (my == 0.0)
+ heading = (mx < 0.0) ? 180.0 : 0.0;
+ else
+ heading = atan2(mx, my)*360.0/(2.0*PI);
+ heading -= DECLINATION; //correct for geo location
+ if(heading>180.0) heading = heading - 360.0;
+ else if(heading<-180.0) heading = 360.0 + heading;
+ else if(heading<0.0) heading = 360.0 + heading;
- // Parse and Update Setpoint Value
- memset(input_buff, '\0', sizeof(input_buff)); // Null out input buff
- begin = strstr(webpage_user_data, "setpoint_input=") +
- sizeof("setpoint_input"); // Points to start of setpoint_input value
- end = begin + strcspn(begin, keys); // Points to end of setpoint_input value
- for(long i = 0; i < end - begin; i++){ // Parse out the value one char at a time
- input_buff[i] = begin[i];
- }
- *working_setpoint = atof(input_buff);
-
- // Parse and Update Kp Value
- memset(input_buff, '\0', sizeof(input_buff)); // Null out input buff
- begin = strstr(webpage_user_data, "kp_input=") +
- sizeof("kp_input"); // Points to start of kp_input value
- end = begin + strcspn(begin, keys); // Points to end of kp_input value
- for(long i = 0; i < end - begin; i++){ // Parse out the value one char at a time
- input_buff[i] = begin[i];
- }
- *kp = atof(input_buff);
-
- // Parse and Update Ki Value
- memset(input_buff, '\0', sizeof(input_buff)); // Null out input buff
- begin = strstr(webpage_user_data, "ki_input=") +
- sizeof("ki_input"); // Points to start of ki_input value
- end = begin + strcspn(begin, keys); // Points to end of ki_input value
- for(long i = 0; i < end - begin; i++){ // Parse out the value one char at a time
- input_buff[i] = begin[i];
- }
- *ki = atof(input_buff);
-
- // Parse and Update Kd Value
- memset(input_buff, '\0', sizeof(input_buff)); // Null out input buff
- begin = strstr(webpage_user_data, "kd_input=") +
- sizeof("kd_input"); // Points to start of kd_input value
- end = begin + strcspn(begin, keys); // Points to end of kd_input value
- for(long i = 0; i < end - begin; i++){ // Parse out the value one char at a time
- 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;
+ #ifdef DEBUG
+ pc.printf("Magnetic Heading: %f degress\n\r",heading);
+ #endif
}
-void pid_callback(){
- static int lastMode = control_mode;
+void pid_callback()
+{
+
+ // Deal with feedback and update motors
+ // Motor direction based on working setpoint var
+
+ //myled[0] = dirL;
+ //myled[1] = dirR;
+ // 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();
+ //pc.printf("%d\r\n", countL);
- // 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 is PID_ON, turn on PID controllers and run motors in closed loop
- else{
- // Mode changed; turn on PID controllers
- if(lastMode != control_mode){
- pidL.start(); pidR.start();
- lastMode = PID_ON;
- }
- // 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;
- }
-
- // Setpoint vars used by PID objects are concerned with
- // only SPEED not direction.
- setpoint = abs(working_setpoint);
+ // Because encoders are not quadrature we must handle the sign ourselves,
+ // 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
- 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();
-
- // 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);
- }
+ 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);
}
/*
@@ -642,45 +463,4 @@
*/
float clip(float value, float lower, float upper){
return std::max(lower, std::min(value, upper));
-}
-
-/**************************WEB PAGE TEXT**************************************/
-/*****************************************************************************
-Copy and past text below into a html file and save as the given file name to
-your SD card.
-
-file name: pid_bot.html
-
-html text:
-
-<!DOCTYPE html>
-<html>
-<head>
-<title>PID RedBot Control</title>
-</head>
-<body>
-<h1>PID Motor Control</h1>
-<h2>PID Status</h2>
-<form title="User Input" method="post">
-PID Controls: <br>
-<input type="radio" name="control_mode" value="0"checked>PID OFF
-<br>
-<input type="radio" name="control_mode" value="1" >PID ON
-<br>
-Setpoint:<br>
-<input type="number" name="setpoint_input" value="0000.00" 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>
-
-*****************************************************************************/
-/*****************************************************************************/
\ No newline at end of file
+}
\ No newline at end of file