Diff: main.cpp

Revision:

0:81cd42bce3cb

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Dec 14 17:46:30 2017 +0000
@@ -0,0 +1,424 @@
+#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();
+        }
+    }
+}
+