File content as of revision 0:72c099aedf8e:

/* Robot 2
Project 3: Autonomous Multi-Robot Security Alert System
Notes:
1. Code checks entry id from the PostToSever results
2. Sends two xvalue and yvalue to server
3. Wall-following routine
*/

#include "mbed.h"
#include "ESP8266.h"
#define IP "184.106.153.149"       // IP Address of "api.thingspeak.com\"
#include <iostream>   // std::cout
#include <string>     // std::string, std::stoi
#include <sstream>
#include "HCSR04.h"  //Ultrasonic distance sensor lib
#include <math.h> 

//Subfunctions
    void EncoderCount(void);
    void Forward(void);
    void Stop(void);
    void Right90(void);
    void Left90(void);
    int GetFrontDistance(void);
    int GetRightDistance(void);
    void ConnectToServer(void);
    void GetCurrentEntryID(void);
    void CloseConnection(void);  
    unsigned int Wifi(void);

//Pin Setup
    DigitalOut right_cont1(A2), right_cont2(A3), left_cont1(A0), left_cont2(A1);
    DigitalOut red(LED_RED), blue(LED_BLUE), green(LED_GREEN); 
    DigitalIn sw2(SW2);
//Interrupt Setup
    InterruptIn EncoderIn(D7); //External Interrupt 
    
//PWM Setup 
    PwmOut right_pwm(A5);
    PwmOut left_pwm(A4);
    
Serial pc(USBTX,USBRX); //Uart connection for TeraTerm
ESP8266 wifi(PTC17, PTC16, 115200); // tx, rx, baud rate for wifi

//Public variables
    int frontDistance = 0, rightDistance = 0, setDistance = 0, lastRightDistance = 0; 
    int xvalue = 0, yvalue = 0; 
    unsigned int entryID = 0; 
    float leftSpeed = 0, rightSpeed = 0;
    float ileftSpeed = 0, irightSpeed = 0;
    float bleftSpeed = 0, brightSpeed = 0; 
    float inc = 0; 
    
int main() 
{
    red = 1; //off
    blue = 1; //off
    green = 1; //off
    while(sw2); 
    unsigned int go = 0; 
    pc.baud(9600);  
    pc.printf("starting\r\n");
    //go = 1; 
    while(go != 1)
    {
        blue = 0;
        wait(1); //wait 1 sec
        blue = 1; 
        go = Wifi();
        pc.printf("go = %d\r\n", go);
    }
    red = 0; //on
    pc.printf("\r\n\r\nRecieved Threat Alert\r\n Proceeding to Coordinate: %d", xvalue);
    wait(10); 
//set motor period
  right_pwm.period(0.007f); 
  left_pwm.period(0.007f); 
  
//motor speed
    leftSpeed = 0.50f;
    rightSpeed = 0.70f; 
    ileftSpeed = 0.50f;
    irightSpeed = 0.70f;    
    left_pwm.write(ileftSpeed);  
    right_pwm.write(irightSpeed);
    Forward();   
    setDistance = GetRightDistance(); //check right distance distance
    lastRightDistance = setDistance; 
    pc.printf("setDistance:%d\r\n", setDistance);
    while(1) 
    { 
        EncoderIn.rise(NULL);  //disable EncoderIn inturrpt
         
        rightDistance = GetRightDistance(); //check distance
        if( rightDistance > (lastRightDistance + 20) || rightDistance < (lastRightDistance - 20))
        {
            pc.printf("Old righDistance:%d, lastRightDistance: %d\r\n", rightDistance, lastRightDistance);
            rightDistance = lastRightDistance -10; 
            pc.printf("New righDistance:%d\r\n", rightDistance);
        }
        else
            lastRightDistance = rightDistance; 
            
        frontDistance = GetFrontDistance(); //check distance 
        //pc.printf("Right:%d , Front: %d\r\n", rightDistance, frontDistance);
        
        if(rightDistance >=1000) 
        {
            rightDistance = 1;
        }
        else
            rightDistance = rightDistance; 
        //***turn right
        if( rightDistance > setDistance + 1 ) 
        {
            inc = 0.2f; 
            leftSpeed = leftSpeed + inc;
            if(leftSpeed > .55f)
                leftSpeed = .55f;
            left_pwm.write(leftSpeed);
            
            rightSpeed = rightSpeed - inc;
            if(rightSpeed < 0.30f)
                rightSpeed = 0.30f; 
            right_pwm.write(rightSpeed);
            pc.printf("Turn Right,right distance:%d, %d\r\n", rightDistance, setDistance);
            pc.printf("leftSpeed:%f, rightSpeed:%f\r\n\r\n", leftSpeed, rightSpeed);
            //Stop();
            //while(sw2);
            //wait(1); 
            //Forward();
        }
        //****turn left
        //else ( rightDistance < (setDistance - 1)) 
        else
        {
            inc = 0.2f;
            leftSpeed = leftSpeed - inc;
            if(leftSpeed < 0.25f)
                leftSpeed = 0.25f;           
            left_pwm.write(leftSpeed);
            
            rightSpeed = rightSpeed + inc;
            if(rightSpeed > 0.65f)
                rightSpeed = 0.65f;
            right_pwm.write(rightSpeed);
            pc.printf("Turn Left,right distance:%d, %d\r\n", rightDistance, setDistance);
            pc.printf("leftSpeed:%f, rightSpeed:%f\r\n\r\n", leftSpeed, rightSpeed);
            //Stop();
            //while(sw2);
            //wait(1); 
            //Forward();
        }
        /*else
        {
            left_pwm.write(ileftSpeed);
            right_pwm.write(irightSpeed);
            pc.printf("Go Straight,right distance:%d, %d\r\n", rightDistance, setDistance);
            pc.printf("leftSpeed:%f, rightSpeed:%f\r\n\r\n", ileftSpeed, irightSpeed);
            //Stop();
            //while(sw2);
            //wait(1); 
            //Forward();
        }   */  
    EncoderIn.rise(&EncoderCount);  /*Interrupt: On rising edge of encoder tick*/ 
    wait(.05);
    }//end of while(1)
}//end of main()

        
unsigned int Wifi(void) 
{  
    unsigned int go = 0;
    pc.printf("\r\n******************Wifi Begin*********************************\r\n");    
    entryID = 0;   
    while(entryID == 0)
    {
        ConnectToServer();
        GetCurrentEntryID();
        CloseConnection();
    }
    if(xvalue != 0 || yvalue != 0)
        go = 1;
    else 
        go = 0;  
    pc.printf("\r\n******************Wifi Completed*********************************\r\n");    
    return go;
}//end of Wifi()



void GetCurrentEntryID(void)
{
    unsigned int i = 0, len = 0, lastEntryID = 0,diff = 0, xvalue1 = 0, yvalue1 = 0, dash = 0, end =0, k = 0; 
    char snd[255],rcv1[1000];
    const int size = sizeof(rcv1);
//Get values from ThinkSpeak
    pc.printf("\r\nGet Values from ThingSpeak\r\n");
    strcpy(snd,"GET http://api.thingspeak.com/channels/104257/fields/1.json?key=1YAN0O31ADCV9MEP HTTP/1.0\r\n\r\n");//97 
    //strcpy(snd,"GET http://api.thingspeak.com/channels/104257/status.json?key=1YAN0O31ADCV9MEP HTTP/1.0\r\n\r\n");//97 
    //send
    //pc.printf("\r\nSending: %s",snd);
    wifi.SendCMD(snd);
    wifi.RcvReply(rcv1, 750);
    pc.printf("\r\nSent Status: %s", rcv1);
    //send again
    //pc.printf("\r\nSending: %s",snd);
    wifi.SendCMD(snd);
    wifi.RcvReply(rcv1, 750);
    pc.printf("\r\nSent Status: %s", rcv1);  
    for(i = 0; i <= size - 1; i++)
    {  
        if(rcv1[i-1]==121 && rcv1[i]==95 && rcv1[i+1]==105 && rcv1[i+2]==100 && rcv1[i+6]<=57 && rcv1[i+6]>=44) //check for "_id"  entry_id 
        { 
            if(rcv1[i+6] ==44)
            {
                len = 0;
                entryID = rcv1[i+5]-48;
            }
            else
            {
                len = 1;
                entryID = (rcv1[i+5]-48)*10+(rcv1[i+6]-48); 
            }
            dash = 0; 
            end = 0; 
            for(k = 1;k<=20;k++)
            {
                if(rcv1[i+17+len+k] == 45) //look for "-"
                {
                    dash = k;
                    k = 21;  
                }
            }
            for(k = 1;k<=20;k++)
            {
                if(rcv1[i+17+len+k] == 34)
                {
                    end = k - 1;
                    diff = end - dash;
                    k = 21; 
                }
            }
            xvalue1 = 0; 
            yvalue1 = 0;
            
            for(k=1;k<=dash;k++)
            { 
                xvalue1 = xvalue1 + (rcv1[i+16+len+k]-48)*pow(10,float(dash - k));
            }
            for(k=1;k<=diff;k++)
            {
                yvalue1 = yvalue1 + (rcv1[i+17+len+dash+k]-48)*pow(10,float(diff-k));
            }
        }//if(entry_id)
        else
            i=i;

        if(entryID <=99 && entryID > lastEntryID)
        {
            lastEntryID = entryID; 
            xvalue = xvalue1;
            yvalue = yvalue1;
        }
        else
        {
            lastEntryID = lastEntryID; 
            xvalue = xvalue;
            yvalue = yvalue;   
        }
    } //end of For loop
    pc.printf("\r\nfinal:: lastEntry_id= %d, xvalue = %d, yvalue = %d",lastEntryID, xvalue, yvalue);
} //end of GetCurrentEntryID





















void ConnectToServer(void)
{   
    char snd[255],rcv[500];
    pc.printf("\r\n\r\n*************Sending WiFi information*************\r\n");
    
//Set Wifi to Single Channel Mode    
    pc.printf("\r\nSet WiFi into Single Channel mode\r\n");
    strcpy(snd,"AT+CIPMUX=0");//Setting WiFi into single Channel mode
    wifi.SendCMD(snd);
    //pc.printf(snd);
    wifi.RcvReply(rcv, 750);
    pc.printf("\r\nSingle Channel Mode Status: %s", rcv);
    //wait(1);

//Initiate connection with THINGSPEAK server 
    pc.printf("\r\nInitiating connection with ThinkSpeak sever\r\n");
    strcpy(snd,"AT+CIPSTART=\"TCP\",\"api.thingspeak.com\",80");    
    //pc.printf("\r\nsending: %s", snd);
    wifi.SendCMD(snd);
    wifi.RcvReply(rcv, 750);
    pc.printf("\r\nConnection Status: %s", rcv);
    
//Send Number of Characters to be sent
    pc.printf("\r\nSending Number of Characters to be sent");
    strcpy(snd,"AT+CIPSEND=110");    //Send Number of open connections,Characters to send 
    wifi.SendCMD(snd);
    //pc.printf(snd);
    wifi.RcvReply(rcv, 750);
    pc.printf("\r\nSent Status: %s", rcv);    
} //end of ConnectToServer()



void CloseConnection(void)
{
    char snd[255],rcv[500];
    //close connection   
    pc.printf("\r\nClose the Connection\r\n");                                   
    strcpy(snd,"AT+CIPCLOSE");        //closing the connection with thingspeak server
    wifi.SendCMD(snd);   
    //pc.printf("\r\nSending: %s",snd);  
    wifi.RcvReply(rcv, 750);
    pc.printf("\r\nClose Connection Status: %s", rcv);
         
    pc.printf("\r\nEnd of Closed Connection Response\r\n\r\n");
}//void connection

void EncoderCount()
{ 
    static int xval = 0, yval = 0;
    xval++;
    //yval++;
    pc.printf("\r\nX: %d, Y: %d\r\n\r\n", xval, yval);
    if(xval >= xvalue)
    {
        EncoderIn.rise(NULL);
        Stop();
        green = 0; //on
        while(1); 
    }
}

int GetFrontDistance(void)
{
    int distance = 0; 
    HCSR04 sensor(D4, D5);
    distance = sensor.distance();
    return distance;
}

int GetRightDistance(void)
{
    int distance = 0; 
    HCSR04 sensor(D2, D3);
    distance = sensor.distance();
    return distance; 
}

void Forward(void)
{
      right_cont1=0;
      right_cont2=1;
      left_cont1=0;
      left_cont2=1;
}

void Stop(void)
{
    right_cont1=0;
    right_cont2=0;
    left_cont1=0;
    left_cont2=0;
}

void Right90(void)
{
      right_cont1=0;
      right_cont2=0;
      left_cont1=0;
      left_cont2=1;
}

void Left90(void)
{
      right_cont1=0;
      right_cont2=1;
      left_cont1=0;
      left_cont2=0;
}