File content as of revision 0:81cd42bce3cb:

#include "mbed.h"
#include <math.h>
#include "LidarLitev2.h"
#include "Servo.h"
Servo myservo(D3);
PwmOut ENApwm(D3);
float level;
int period = 3000;
float Neutral_level=1500;
float clklevel = 0.9;
float ant_clklevel = 0.1;
DigitalOut DirPin(D4);
DigitalOut StepPin(D5);
int cnt = 0, Pulse;

Serial esp(PTC17, PTC16); // tx, rx
DigitalOut reset(PTE24);
Timer t;

int  count,ended,timeout;
char buf[1024];
char snd[1024];

char ssid[32] = "AbhiMotoXPlay"; //"BHNTG1682G25E3"."AbhiMotoXPlay"    // enter WiFi router ssid inside the quotes
char pwd [32] = "c904anbh"; // "03333295"."c904anbh" //enter WiFi router password inside the quotes

//void SendCMD(),getreply(),ESPconfig(),ESPsetbaudrate();
void SendCMD(),getreply(),ESPconfig(),ESPsetbaudrate(), TCP_IPSend();
//void send(void);


void Servo_Forward_degree(float level);
void Servo_Backward_degree(float level);

AnalogIn Port_X(PTB2);// X read of ADXL335 A0
AnalogIn Port_Y(PTB3);// Y read of ADXL335 A1
AnalogIn Port_Z(PTB10);//Z read of ASXL335 A2
float Scale(float inVal, float inMin, float inMax, float outMin, float outMax);
int StepperMove();
int Ldist;
float Port_Xval,Port_Yval,Port_Zval,X_sqr,Y_sqr,Z_sqr,Psi,Theta,Phi,Xacc,Yacc,Zacc,Val,Zsq,Ysq,Xsq,PsiAng,ThetaAng,PhiAng;
LidarLitev2 Lidar(PTC11, PTC10);
Serial pc(USBTX,USBRX);
//float inMin = 0.40,inMax =0.60 ,outMin = -1000,outMax = 1000;


int main(){
    Lidar.configure();
    ENApwm.period_us(period);
    while(1){
         Ldist = Lidar.distance(); 
         Port_Xval = Port_X.read();
         Xacc = Scale(Port_Xval,0.40,0.60,-1000,1000);
         //X_sqr = Port_Xval*Port_Xval;
         Port_Yval = Port_Y.read();
         Yacc = Scale(Port_Yval,0.40,0.60,-1000,1000);
         //Y_sqr = Port_Yval * Port_Yval;
         Port_Zval = Port_Z.read();
         Zacc = Scale(Port_Zval,0.40,0.60,-1000,1000);
         Xsq = Xacc*Xacc;
         Ysq = Yacc*Yacc;
         Zsq = Zacc*Zacc;
         //Z_sqr = Port_Zval*Port_Zval;
         Theta = atan2(Yacc,(sqrt(Xsq+Zsq))) ;
         ThetaAng = (Theta*180.0*7.0)/22.0;
         Psi = atan2(Xacc,(sqrt(Ysq+Zsq)));
         PsiAng = (Psi*180.0*7.0)/22.0;
         Phi = atan2((sqrt(Xsq+Ysq)),Zacc);
         Phi = Scale(Phi,0.0,3.1416,1.57,-1.57);
         PhiAng = (Phi*180.0*7.0)/22.0;
         //PhiAng = Scale(PhiAng,0.0,180.0,-90.0,90.0);
         //printf(" %d %f %f %f\n\r", Ldist,Port_Xval,Port_Yval,Port_Zval);
         //printf(" %f %f %f\n\r", Port_Xval,Port_Yval,Port_Zval);
         //printf(" %f %f %f\n\r", Xacc,Yacc,Zacc);
         pc.printf(" %d %f %f %f %f %f %f\n\r",Ldist,Psi,Theta,Phi,PsiAng,ThetaAng,PhiAng);
         //dt.reset();
         //Servo_Forward_degree(clklevel);
         if ((PhiAng>=-1.0)&&(PhiAng<=1.0)){
             pc.printf("Calibration done"); 
             break;            
         }
         else{
             Servo_Forward_degree(clklevel);
             pc.printf("Calibrating");
             wait(0.5);
             continue;
         }
    }
    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();

    esp.baud(115200); 
    cnt=0;
    ESPconfig();
    pc.printf("Start Stepper Motor\r\n");
    wait(6);
    
    while (1){
         Ldist = Lidar.distance(); 
         Port_Xval = Port_X.read();
         Xacc = Scale(Port_Xval,0.40,0.60,-1000,1000);
         //X_sqr = Port_Xval*Port_Xval;
         Port_Yval = Port_Y.read();
         Yacc = Scale(Port_Yval,0.40,0.60,-1000,1000);
         //Y_sqr = Port_Yval * Port_Yval;
         Port_Zval = Port_Z.read();
         Zacc = Scale(Port_Zval,0.40,0.60,-1000,1000);
         Xsq = Xacc*Xacc;
         Ysq = Yacc*Yacc;
         Zsq = Zacc*Zacc;
         //Z_sqr = Port_Zval*Port_Zval;
         Theta = atan2(Yacc,(sqrt(Xsq+Zsq))) ;
         ThetaAng = (Theta*180.0*7.0)/22.0;
         Psi = atan2(Xacc,(sqrt(Ysq+Zsq)));
         PsiAng = (Psi*180.0*7.0)/22.0;
         Phi = atan2((sqrt(Xsq+Ysq)),Zacc);
         Phi = Scale(Phi,0.0,3.1416,1.57,-1.57);
         PhiAng = (Phi*180.0*7.0)/22.0;
         //PhiAng = Scale(PhiAng,0.0,180.0,-90.0,90.0);
         //printf(" %d %f %f %f\n\r", Ldist,Port_Xval,Port_Yval,Port_Zval);
         //printf(" %f %f %f\n\r", Port_Xval,Port_Yval,Port_Zval);
         //printf(" %f %f %f\n\r", Xacc,Yacc,Zacc);
         pc.printf(" LidarVal= %d Psi= %f Theta= %f Phi= %f Psi(Deg)= %f Theta(Deg)= %f Phi(Deg)= %f\n\r",Ldist,Psi,Theta,Phi,PsiAng,ThetaAng,PhiAng);
         cnt = cnt+1;
         //printf("%d\r\n"cnt);
         if (cnt%3 == 0){             
             Servo_Forward_degree(clklevel);
             pc.printf("Servo Moved");
             }
         else if (cnt%10 == 0){
             TCP_IPSend();
             }
             
         else{
             continue;
             }
        
         
    }
    
    
    
    
    
    

}


float Scale(float inVal, float inMin, float inMax, float outMin, float outMax)
{
   float ScaleValue = ((inVal - inMin)*(outMax - outMin)/(inMax - inMin) + outMin ); 
   return ScaleValue; 
}

void Servo_Forward_degree(float level){
      while(1){
          ENApwm.write(level);
          wait(0.001);
          pc.printf("%f  \r",level);         
          if ((level >= 1.0)||(level<=0.0))
          {
            //ENApwm.write(1.0);
            break;
            
          }
          level = level+0.1;
       }
     
     
     
}

void Servo_Backward_degree(float level){
     while(1){
          /*ENApwm.write(level);
          wait(0.001);          
          pc.printf("%f   \r\n",level);         
          if ((level >= 1.0)||(level<=0.0))
          {
            //ENApwm.write(1.0);
            break;
            
          }
          level = level-0.05;*/
          ENApwm.pulsewidth_us(90);
          wait(0.001);
          ENApwm.pulsewidth_us(0);
          break;
          
         
       }
     
}

int StepperMove(){
    for(int x = 0; x < 50; x++) {
        StepPin=0; 
        wait(0.05); 
        StepPin=1; 
        wait(0.05);
        cnt = cnt+1;
        printf("%d\r\n",cnt);
        return cnt;
        
        }
}
   
void ESPsetbaudrate()
{
    strcpy(snd, "AT+CIOBAUD=115200\r\n");   // change the numeric value to the required baudrate
    SendCMD();
}

//  +++++++++++++++++++++++++++++++++ This is for ESP8266 config only, run this once to set up the ESP8266 +++++++++++++++
void ESPconfig()
{
    wait(5);
    strcpy(snd,"AT\r\n");
    SendCMD();
    wait(1);
    strcpy(snd,"AT\r\n");
    SendCMD();
    wait(1);
    strcpy(snd,"AT\r\n");
    SendCMD();
    timeout=1;
    getreply();
    wait(1);
    pc.printf("\f---------- Starting ESP Config ----------\r\n\n");

    pc.printf("---------- Reset & get Firmware ----------\r\n");
    strcpy(snd,"AT+RST\r\n");
    SendCMD();
    timeout=5;
    getreply();
    pc.printf(buf);

    wait(2);

    pc.printf("\n---------- Get Version ----------\r\n");
    strcpy(snd,"AT+GMR\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, "AT+CWMODE=1\r\n");
    SendCMD();
    timeout=4;
    getreply();
    pc.printf(buf);

    wait(2);

    // set CIPMUX to 0=Single,1=Multi
    pc.printf("\n---------- Setting Connection Mode ----------\r\n");
    strcpy(snd, "AT+CIPMUX=1\r\n");
    SendCMD();
    timeout=4;
    getreply();
    pc.printf(buf);

    wait(2);

    /*pc.printf("\n---------- Listing Access Points ----------\r\n");
    strcpy(snd, "AT+CWLAP\r\n");
    SendCMD();
    timeout=15;
    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, "AT+CWJAP=\"");
    strcat(snd, ssid);
    strcat(snd, "\",\"");/* Here (\") denotes inverted commas in strings */
    strcat(snd, pwd);
    strcat(snd, "\"\r\n");
    pc.printf(snd);
    SendCMD();
    timeout=10;
    getreply();
    pc.printf(buf);

    wait(5);

    pc.printf("\n---------- Get IP's ----------\r\n");
    strcpy(snd, "AT+CIFSR\r\n");
    SendCMD();
    timeout=3;
    getreply();
    pc.printf(buf);
    //const char* ipData = string(buf);
    //const char* msg="Hello World!"; 
    //pc.printf(ipData);

    wait(1);

    pc.printf("\n---------- Get Connection Status ----------\r\n");
    strcpy(snd, "AT+CIPSTATUS\r\n");
    SendCMD();
    timeout=5;
    getreply();
    pc.printf(buf);

    pc.printf("\n\n\n  If you get a valid (non zero) IP, ESP8266 has been set up.\r\n");
    pc.printf("  Run this if you want to reconfig the ESP8266 at any time.\r\n");
    pc.printf("  It saves the SSID and password settings internally\r\n");
    wait(10);
    
    /*pc.printf("\n---------- Start TCP_IP Connection with WAMP ----------\r\n");
    //strcpy(snd, "AT+CIFSR\r\n");
    strcpy(snd, "AT+CIPSTART=4,\"TCP\",\"149.165.231.70\",8660\r\n");
    //strcpy(snd, "AT+CIPSTART=4,\"TCP\",\"192.168.43.237\",8383\r\n");     
    SendCMD(); 
    timeout=5;
    getreply();
    pc.printf(buf);*/

}

void TCP_IPSend(){
    
    pc.printf("\n---------- Start TCP_IP Connection with WAMP ----------\r\n");
    //strcpy(snd, "AT+CIFSR\r\n");
    strcpy(snd, "AT+CIPSTART=4,\"TCP\",\"149.165.231.70\",8660\r\n");
    //strcpy(snd, "AT+CIPSTART=4,\"TCP\",\"192.168.43.237\",8383\r\n");     
    SendCMD(); 
    timeout=5;
    getreply();
    pc.printf(buf);

    //wait();
    
    
    
    pc.printf("\n---------- Set TCP Data frame ----------\r\n");
    strcpy(snd, "AT+CIPSEND=4,90\r\n");
    //strcpy(snd, "AT+CIPSTART=4,\"TCP\",\"184.106.153.149\",80\r\n") 
    SendCMD(); 
    timeout=1;
    getreply();
    pc.printf(buf);

    //wait(0.5);
    
    pc.printf("\n---------- Send Data frame ----------\r\n");
    //strcpy(snd, "//AT+CIPSEND=4,44\r\n"); 0.000000 39.026474 9.756618 36.102848
    // "GET /adxldatastore.php?Xvalue=234&Yvalue=237&Zvalue=359 HTTP/1.1Host: 127.0.0.1Connection: close\r\n"
    pc.printf("GET /MEuser/LidarDataStore.php?Xvalue=237&Yvalue=437&Zvalue=759&LidarValue=986\r\n");
    sprintf(snd, "GET /MEuser/LidarDataStore.php?Xvalue=%0.2f&Yvalue=%0.2f&Zvalue=%0.2f&LidarValue=%d\r\n",PsiAng,ThetaAng,PhiAng,Ldist);
    //sprintf(snd, "GET /update?key=6NNODQ1FN0J81A05&field1=%0.2f&field2=%0.2f&field3=%0.2f&field4=%0.2f\r\n", AvgCurrentVal,AvgPressureVal,AvgTempVal,AvgCO2Val);
    /*strcpy(snd, "GET /update?key=6NNODQ1FN0J81A05&field1="); 
    Strng1=sprintf (num, "%0.1f", AvgCurrentVal);
    strcat(snd, Strng);
    strcat(snd, "&field2=");
    Strng2=sprintf (num, "%0.1f", AvgPressureVal);
    strcat(snd, Strng2);
    strcat(snd, "&field3=");
    Strng3=sprintf (num, "%0.1f", AvgTempVal);
    strcat(snd, Strng3);
    strcat(snd, "&field4=");
    Strng4=sprintf (num, "%0.1f", AvgCO2Val);
    strcat(snd, Strng4);
    strcat(snd, "\r\n");*/
    
    SendCMD(); 
    timeout=1;
    getreply();
    pc.printf(buf);

    //wait(1);
    
    pc.printf("\n---------- Close TCP/IP Connection ----------\r\n");
    strcpy(snd, "AT+CIPCLOSE\r\n");
    //strcpy(snd, "GET /update?key=2IKJ99A6BPSCORQC&field1=70\r\n") 
    SendCMD(); 
    timeout=1;
    getreply();
    pc.printf(buf);

    //wait(2);
    
    
    
    
}

void SendCMD()
{
    esp.printf("%s", snd);
}

void getreply()
{
    memset(buf, '\0', sizeof(buf));
    t.start();
    ended=0;
    count=0;
    while(!ended) {
        if(esp.readable()) {
            buf[count] = esp.getc();
            count++;
        }
        if(t.read() > timeout) {
            ended = 1;
            t.stop();
            t.reset();
        }
    }
}