main.cpp

00001 #include "mbed.h"
00002 #include <math.h>
00003 #include "LidarLitev2.h"
00004 #include "Servo.h"
00005 Servo myservo(D3);
00006 PwmOut ENApwm(D3);
00007 float level;
00008 int period = 3000;
00009 float Neutral_level=1500;
00010 float clklevel = 0.9;
00011 float ant_clklevel = 0.1;
00012 DigitalOut DirPin(D4);
00013 DigitalOut StepPin(D5);
00014 int cnt = 0, Pulse;
00015 
00016 Serial esp(PTC17, PTC16); // tx, rx
00017 DigitalOut reset(PTE24);
00018 Timer t;
00019 
00020 int  count,ended,timeout;
00021 char buf[1024];
00022 char snd[1024];
00023 
00024 char ssid[32] = "AbhiMotoXPlay"; //"BHNTG1682G25E3"."AbhiMotoXPlay"    // enter WiFi router ssid inside the quotes
00025 char pwd [32] = "c904anbh"; // "03333295"."c904anbh" //enter WiFi router password inside the quotes
00026 
00027 //void SendCMD(),getreply(),ESPconfig(),ESPsetbaudrate();
00028 void SendCMD(),getreply(),ESPconfig(),ESPsetbaudrate(), TCP_IPSend();
00029 //void send(void);
00030 
00031 
00032 void Servo_Forward_degree(float level);
00033 void Servo_Backward_degree(float level);
00034 
00035 AnalogIn Port_X(PTB2);// X read of ADXL335 A0
00036 AnalogIn Port_Y(PTB3);// Y read of ADXL335 A1
00037 AnalogIn Port_Z(PTB10);//Z read of ASXL335 A2
00038 float Scale(float inVal, float inMin, float inMax, float outMin, float outMax);
00039 int StepperMove();
00040 int Ldist;
00041 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;
00042 LidarLitev2 Lidar(PTC11, PTC10);
00043 Serial pc(USBTX,USBRX);
00044 //float inMin = 0.40,inMax =0.60 ,outMin = -1000,outMax = 1000;
00045 
00046 
00047 int main(){
00048     Lidar.configure();
00049     ENApwm.period_us(period);
00050     while(1){
00051          Ldist = Lidar.distance(); 
00052          Port_Xval = Port_X.read();
00053          Xacc = Scale(Port_Xval,0.40,0.60,-1000,1000);
00054          //X_sqr = Port_Xval*Port_Xval;
00055          Port_Yval = Port_Y.read();
00056          Yacc = Scale(Port_Yval,0.40,0.60,-1000,1000);
00057          //Y_sqr = Port_Yval * Port_Yval;
00058          Port_Zval = Port_Z.read();
00059          Zacc = Scale(Port_Zval,0.40,0.60,-1000,1000);
00060          Xsq = Xacc*Xacc;
00061          Ysq = Yacc*Yacc;
00062          Zsq = Zacc*Zacc;
00063          //Z_sqr = Port_Zval*Port_Zval;
00064          Theta = atan2(Yacc,(sqrt(Xsq+Zsq))) ;
00065          ThetaAng = (Theta*180.0*7.0)/22.0;
00066          Psi = atan2(Xacc,(sqrt(Ysq+Zsq)));
00067          PsiAng = (Psi*180.0*7.0)/22.0;
00068          Phi = atan2((sqrt(Xsq+Ysq)),Zacc);
00069          Phi = Scale(Phi,0.0,3.1416,1.57,-1.57);
00070          PhiAng = (Phi*180.0*7.0)/22.0;
00071          //PhiAng = Scale(PhiAng,0.0,180.0,-90.0,90.0);
00072          //printf(" %d %f %f %f\n\r", Ldist,Port_Xval,Port_Yval,Port_Zval);
00073          //printf(" %f %f %f\n\r", Port_Xval,Port_Yval,Port_Zval);
00074          //printf(" %f %f %f\n\r", Xacc,Yacc,Zacc);
00075          pc.printf(" %d %f %f %f %f %f %f\n\r",Ldist,Psi,Theta,Phi,PsiAng,ThetaAng,PhiAng);
00076          //dt.reset();
00077          //Servo_Forward_degree(clklevel);
00078          if ((PhiAng>=-1.0)&&(PhiAng<=1.0)){
00079              pc.printf("Calibration done"); 
00080              break;            
00081          }
00082          else{
00083              Servo_Forward_degree(clklevel);
00084              pc.printf("Calibrating");
00085              wait(0.5);
00086              continue;
00087          }
00088     }
00089     reset=0; //hardware reset for 8266
00090     pc.baud(9600);  // set what you want here depending on your terminal program speed
00091     pc.printf("\f\n\r-------------ESP8266 Hardware Reset-------------\n\r");
00092     wait(0.5);
00093     reset=1;
00094     timeout=2;
00095     getreply();
00096 
00097     esp.baud(115200); 
00098     cnt=0;
00099     ESPconfig();
00100     pc.printf("Start Stepper Motor\r\n");
00101     wait(6);
00102     
00103     while (1){
00104          Ldist = Lidar.distance(); 
00105          Port_Xval = Port_X.read();
00106          Xacc = Scale(Port_Xval,0.40,0.60,-1000,1000);
00107          //X_sqr = Port_Xval*Port_Xval;
00108          Port_Yval = Port_Y.read();
00109          Yacc = Scale(Port_Yval,0.40,0.60,-1000,1000);
00110          //Y_sqr = Port_Yval * Port_Yval;
00111          Port_Zval = Port_Z.read();
00112          Zacc = Scale(Port_Zval,0.40,0.60,-1000,1000);
00113          Xsq = Xacc*Xacc;
00114          Ysq = Yacc*Yacc;
00115          Zsq = Zacc*Zacc;
00116          //Z_sqr = Port_Zval*Port_Zval;
00117          Theta = atan2(Yacc,(sqrt(Xsq+Zsq))) ;
00118          ThetaAng = (Theta*180.0*7.0)/22.0;
00119          Psi = atan2(Xacc,(sqrt(Ysq+Zsq)));
00120          PsiAng = (Psi*180.0*7.0)/22.0;
00121          Phi = atan2((sqrt(Xsq+Ysq)),Zacc);
00122          Phi = Scale(Phi,0.0,3.1416,1.57,-1.57);
00123          PhiAng = (Phi*180.0*7.0)/22.0;
00124          //PhiAng = Scale(PhiAng,0.0,180.0,-90.0,90.0);
00125          //printf(" %d %f %f %f\n\r", Ldist,Port_Xval,Port_Yval,Port_Zval);
00126          //printf(" %f %f %f\n\r", Port_Xval,Port_Yval,Port_Zval);
00127          //printf(" %f %f %f\n\r", Xacc,Yacc,Zacc);
00128          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);
00129          cnt = cnt+1;
00130          //printf("%d\r\n"cnt);
00131          if (cnt%3 == 0){             
00132              Servo_Forward_degree(clklevel);
00133              pc.printf("Servo Moved");
00134              }
00135          else if (cnt%10 == 0){
00136              TCP_IPSend();
00137              }
00138              
00139          else{
00140              continue;
00141              }
00142         
00143          
00144     }
00145     
00146     
00147     
00148     
00149     
00150     
00151 
00152 }
00153 
00154 
00155 float Scale(float inVal, float inMin, float inMax, float outMin, float outMax)
00156 {
00157    float ScaleValue = ((inVal - inMin)*(outMax - outMin)/(inMax - inMin) + outMin ); 
00158    return ScaleValue; 
00159 }
00160 
00161 void Servo_Forward_degree(float level){
00162       while(1){
00163           ENApwm.write(level);
00164           wait(0.001);
00165           pc.printf("%f  \r",level);         
00166           if ((level >= 1.0)||(level<=0.0))
00167           {
00168             //ENApwm.write(1.0);
00169             break;
00170             
00171           }
00172           level = level+0.1;
00173        }
00174      
00175      
00176      
00177 }
00178 
00179 void Servo_Backward_degree(float level){
00180      while(1){
00181           /*ENApwm.write(level);
00182           wait(0.001);          
00183           pc.printf("%f   \r\n",level);         
00184           if ((level >= 1.0)||(level<=0.0))
00185           {
00186             //ENApwm.write(1.0);
00187             break;
00188             
00189           }
00190           level = level-0.05;*/
00191           ENApwm.pulsewidth_us(90);
00192           wait(0.001);
00193           ENApwm.pulsewidth_us(0);
00194           break;
00195           
00196          
00197        }
00198      
00199 }
00200 
00201 int StepperMove(){
00202     for(int x = 0; x < 50; x++) {
00203         StepPin=0; 
00204         wait(0.05); 
00205         StepPin=1; 
00206         wait(0.05);
00207         cnt = cnt+1;
00208         printf("%d\r\n",cnt);
00209         return cnt;
00210         
00211         }
00212 }
00213    
00214 void ESPsetbaudrate()
00215 {
00216     strcpy(snd, "AT+CIOBAUD=115200\r\n");   // change the numeric value to the required baudrate
00217     SendCMD();
00218 }
00219 
00220 //  +++++++++++++++++++++++++++++++++ This is for ESP8266 config only, run this once to set up the ESP8266 +++++++++++++++
00221 void ESPconfig()
00222 {
00223     wait(5);
00224     strcpy(snd,"AT\r\n");
00225     SendCMD();
00226     wait(1);
00227     strcpy(snd,"AT\r\n");
00228     SendCMD();
00229     wait(1);
00230     strcpy(snd,"AT\r\n");
00231     SendCMD();
00232     timeout=1;
00233     getreply();
00234     wait(1);
00235     pc.printf("\f---------- Starting ESP Config ----------\r\n\n");
00236 
00237     pc.printf("---------- Reset & get Firmware ----------\r\n");
00238     strcpy(snd,"AT+RST\r\n");
00239     SendCMD();
00240     timeout=5;
00241     getreply();
00242     pc.printf(buf);
00243 
00244     wait(2);
00245 
00246     pc.printf("\n---------- Get Version ----------\r\n");
00247     strcpy(snd,"AT+GMR\r\n");
00248     SendCMD();
00249     timeout=4;
00250     getreply();
00251     pc.printf(buf);
00252 
00253     wait(3);
00254 
00255     // set CWMODE to 1=Station,2=AP,3=BOTH, default mode 1 (Station)
00256     pc.printf("\n---------- Setting Mode ----------\r\n");
00257     strcpy(snd, "AT+CWMODE=1\r\n");
00258     SendCMD();
00259     timeout=4;
00260     getreply();
00261     pc.printf(buf);
00262 
00263     wait(2);
00264 
00265     // set CIPMUX to 0=Single,1=Multi
00266     pc.printf("\n---------- Setting Connection Mode ----------\r\n");
00267     strcpy(snd, "AT+CIPMUX=1\r\n");
00268     SendCMD();
00269     timeout=4;
00270     getreply();
00271     pc.printf(buf);
00272 
00273     wait(2);
00274 
00275     /*pc.printf("\n---------- Listing Access Points ----------\r\n");
00276     strcpy(snd, "AT+CWLAP\r\n");
00277     SendCMD();
00278     timeout=15;
00279     getreply();
00280     pc.printf(buf);
00281 
00282     wait(2);*/
00283 
00284     pc.printf("\n---------- Connecting to AP ----------\r\n");
00285     pc.printf("ssid = %s   pwd = %s\r\n",ssid,pwd);
00286     strcpy(snd, "AT+CWJAP=\"");
00287     strcat(snd, ssid);
00288     strcat(snd, "\",\"");/* Here (\") denotes inverted commas in strings */
00289     strcat(snd, pwd);
00290     strcat(snd, "\"\r\n");
00291     pc.printf(snd);
00292     SendCMD();
00293     timeout=10;
00294     getreply();
00295     pc.printf(buf);
00296 
00297     wait(5);
00298 
00299     pc.printf("\n---------- Get IP's ----------\r\n");
00300     strcpy(snd, "AT+CIFSR\r\n");
00301     SendCMD();
00302     timeout=3;
00303     getreply();
00304     pc.printf(buf);
00305     //const char* ipData = string(buf);
00306     //const char* msg="Hello World!"; 
00307     //pc.printf(ipData);
00308 
00309     wait(1);
00310 
00311     pc.printf("\n---------- Get Connection Status ----------\r\n");
00312     strcpy(snd, "AT+CIPSTATUS\r\n");
00313     SendCMD();
00314     timeout=5;
00315     getreply();
00316     pc.printf(buf);
00317 
00318     pc.printf("\n\n\n  If you get a valid (non zero) IP, ESP8266 has been set up.\r\n");
00319     pc.printf("  Run this if you want to reconfig the ESP8266 at any time.\r\n");
00320     pc.printf("  It saves the SSID and password settings internally\r\n");
00321     wait(10);
00322     
00323     /*pc.printf("\n---------- Start TCP_IP Connection with WAMP ----------\r\n");
00324     //strcpy(snd, "AT+CIFSR\r\n");
00325     strcpy(snd, "AT+CIPSTART=4,\"TCP\",\"149.165.231.70\",8660\r\n");
00326     //strcpy(snd, "AT+CIPSTART=4,\"TCP\",\"192.168.43.237\",8383\r\n");     
00327     SendCMD(); 
00328     timeout=5;
00329     getreply();
00330     pc.printf(buf);*/
00331 
00332 }
00333 
00334 void TCP_IPSend(){
00335     
00336     pc.printf("\n---------- Start TCP_IP Connection with WAMP ----------\r\n");
00337     //strcpy(snd, "AT+CIFSR\r\n");
00338     strcpy(snd, "AT+CIPSTART=4,\"TCP\",\"149.165.231.70\",8660\r\n");
00339     //strcpy(snd, "AT+CIPSTART=4,\"TCP\",\"192.168.43.237\",8383\r\n");     
00340     SendCMD(); 
00341     timeout=5;
00342     getreply();
00343     pc.printf(buf);
00344 
00345     //wait();
00346     
00347     
00348     
00349     pc.printf("\n---------- Set TCP Data frame ----------\r\n");
00350     strcpy(snd, "AT+CIPSEND=4,90\r\n");
00351     //strcpy(snd, "AT+CIPSTART=4,\"TCP\",\"184.106.153.149\",80\r\n") 
00352     SendCMD(); 
00353     timeout=1;
00354     getreply();
00355     pc.printf(buf);
00356 
00357     //wait(0.5);
00358     
00359     pc.printf("\n---------- Send Data frame ----------\r\n");
00360     //strcpy(snd, "//AT+CIPSEND=4,44\r\n"); 0.000000 39.026474 9.756618 36.102848
00361     // "GET /adxldatastore.php?Xvalue=234&Yvalue=237&Zvalue=359 HTTP/1.1Host: 127.0.0.1Connection: close\r\n"
00362     pc.printf("GET /MEuser/LidarDataStore.php?Xvalue=237&Yvalue=437&Zvalue=759&LidarValue=986\r\n");
00363     sprintf(snd, "GET /MEuser/LidarDataStore.php?Xvalue=%0.2f&Yvalue=%0.2f&Zvalue=%0.2f&LidarValue=%d\r\n",PsiAng,ThetaAng,PhiAng,Ldist);
00364     //sprintf(snd, "GET /update?key=6NNODQ1FN0J81A05&field1=%0.2f&field2=%0.2f&field3=%0.2f&field4=%0.2f\r\n", AvgCurrentVal,AvgPressureVal,AvgTempVal,AvgCO2Val);
00365     /*strcpy(snd, "GET /update?key=6NNODQ1FN0J81A05&field1="); 
00366     Strng1=sprintf (num, "%0.1f", AvgCurrentVal);
00367     strcat(snd, Strng);
00368     strcat(snd, "&field2=");
00369     Strng2=sprintf (num, "%0.1f", AvgPressureVal);
00370     strcat(snd, Strng2);
00371     strcat(snd, "&field3=");
00372     Strng3=sprintf (num, "%0.1f", AvgTempVal);
00373     strcat(snd, Strng3);
00374     strcat(snd, "&field4=");
00375     Strng4=sprintf (num, "%0.1f", AvgCO2Val);
00376     strcat(snd, Strng4);
00377     strcat(snd, "\r\n");*/
00378     
00379     SendCMD(); 
00380     timeout=1;
00381     getreply();
00382     pc.printf(buf);
00383 
00384     //wait(1);
00385     
00386     pc.printf("\n---------- Close TCP/IP Connection ----------\r\n");
00387     strcpy(snd, "AT+CIPCLOSE\r\n");
00388     //strcpy(snd, "GET /update?key=2IKJ99A6BPSCORQC&field1=70\r\n") 
00389     SendCMD(); 
00390     timeout=1;
00391     getreply();
00392     pc.printf(buf);
00393 
00394     //wait(2);
00395     
00396     
00397     
00398     
00399 }
00400 
00401 void SendCMD()
00402 {
00403     esp.printf("%s", snd);
00404 }
00405 
00406 void getreply()
00407 {
00408     memset(buf, '\0', sizeof(buf));
00409     t.start();
00410     ended=0;
00411     count=0;
00412     while(!ended) {
00413         if(esp.readable()) {
00414             buf[count] = esp.getc();
00415             count++;
00416         }
00417         if(t.read() > timeout) {
00418             ended = 1;
00419             t.stop();
00420             t.reset();
00421         }
00422     }
00423 }
00424