File content as of revision 3:bf6e71964ceb:

#include "mbed.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

Serial blue(p28,p27);
BusOut myled(LED1,LED2,LED3,LED4);
Serial pc(USBTX, USBRX);

// -------- PID Initialization -----------
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/384.0;         // Scale feedback to 1rev/3000cnts
                                                // this is encoder specific.
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

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 pidL(&setpoint, &feedbackL, &outputL,
        output_lower_limit, output_upper_limit,
        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;                   // Interrupt for feedback and motor updates
//global variables for main and interrupt routine

char state_num = 0, bnum =0 ;
int dirL = 0, dirR = 0;

//Interrupt routine to parse message with one new character per serial RX interrupt

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()
{
    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();
    
    ESPconfig();        //******************  include Config to set the ESP8266 configuration  ***********************

    imu.begin();
    if (!imu.begin()) {
        pc.printf("Failed to communicate with LSM9DS1.\n");
    }
    imu.calibrate(1);
    imu.calibrateMag(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 
    // missing counts etc. 
    
    motor.attach(&pid_callback, Ts_PID_CALLBACK);

    // Start PID sampling
    pidL.start();
    pidR.start();
    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);
                    }
                }
            }
        } 
        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;
        }
        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++;
        }
    }
}

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);
    
    strcpy(snd, "conn:send(\"<html>\")\r\n");
    SendCMD();
    wait(1);
    
    strcpy(snd, "conn:send(\"<h1>WIFI Monitoring Program</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");

}

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();
        }
    }
}
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;
    
    #ifdef DEBUG
    pc.printf("Magnetic Heading: %f degress\n\r",heading);
    #endif
}

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);
    
    // 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
    
    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);
}

/*
    Clips value to lower/ uppper
    @param value    The value to clip
    @param lower    The mininum allowable value
    @param upper    The maximum allowable value
    @return         The resulting clipped value
*/
float clip(float value, float lower, float upper){
    return std::max(lower, std::min(value, upper));
}