Diff: main.cpp

Revision:

1:21ee654912d1

Parent:

0:845bbc545b56

Child:

2:65cffee0c0f7

--- a/main.cpp	Tue Mar 07 18:35:22 2017 +0000
+++ b/main.cpp	Tue Mar 21 21:16:57 2017 +0000
@@ -1,13 +1,142 @@
 #include "mbed.h"
 #include "DHT/DHT.h"
+#include "gps_stg_venus.h"
 #include <string>
 
 using namespace std;
 
-DHT sensor(A2,DHT22); 
+DHT sensor(PC_0,DHT22); 
 Serial pc(SERIAL_TX,SERIAL_RX);
-Serial xbee(A0,A1);
+Serial gps(PA_0,PA_1);
+Serial xbee(PC_10,PC_11);
+Serial motor(PA_0,PA_1);
+AnalogIn gassensor(PC_1);
+char data_gps[256],ns,ew,sampah[256];
+int g=0, cek_gps,lock,satelit;
+float waktu,latx,longx, laty, longy,dilution, altitude;
+double lat_target,long_target;
+double degrees, minutes, seconds;
+int lattitude[3], longitude[3];
+float speed;
+void initialize_gps()
+{
+    venus.setBaud_9600();
+    wait(0.5); 
+    gps.baud(9600);
+    venus.setUpdateRate(10);
+    wait(0.5);
+    venus.setNmeaMessages(true, false, false, false, false, true);
+}
+
+void gps_interrupt()
+{
+//     char temp = gps.getc();
+//     if(g==0)
+//        {   led1=0;
+//            data_gps[g]=temp;
+//            if(data_gps[g]=='$')
+//            {g=1;data_gps[0]='$';}
+//        }
+//    else if (g==42){g=0;}
+//    else
+//        {
+//            data_gps[g]=temp;g++;led1=1;
+//            if(data_gps[g-1]=='\r')g=0;
+//        }
+    char temp=gps.getc();
+    if (temp=='$') {g=0;}
+    data_gps[g]=temp;
+    g++;
+    
+    
+}
+
+float trunc(float v) // pembulatan nilai
+{
+    if(v < 0.0) {
+        v*= -1.0;
+        v = floor(v);
+        v*=-1.0;
+    } else {
+        v = floor(v);
+    }
+    return v;
+}
 
+void get_gps()
+{       
+        //if(data_gps[0]=='G'&&data_gps[1]=='P'&&data_gps[2]=='G'&&data_gps[3]=='G'&&data_gps[4]=='A'&&data_gps[5]==','){
+        //$GPGGA,050749.299,0745.9647,S,11022.3071,E,0,00,,189.5,M,4.3,M,,0000*5E
+        sscanf(data_gps,"$GPGGA,%f,%f,%c,%f,%c,%d,%2d,%f,%f",&waktu, &latx,&ns, &longx, &ew, &lock, &satelit, &dilution, &altitude);
+           
+                degrees = (latx / 100.0f);
+                lattitude[0]=degrees;
+                degrees= degrees-lattitude[0];
+                minutes=(degrees*1000);
+                lattitude[1]=minutes;
+                minutes=minutes-lattitude[1];
+                seconds =(minutes*1000);
+                lattitude[2]= seconds;
+                
+                degrees = (longx / 100.0f);
+                longitude[0]=degrees;
+                degrees= degrees-longitude[0];
+                minutes=(degrees*1000);
+                longitude[1]=minutes;
+                minutes=minutes-longitude[1];
+                seconds =(minutes*1000);
+                longitude[2]= seconds;
+        sscanf(data_gps,"$GPVTG,%*f,%*c,%*f,%*c,%*f,%*c,%f",&speed);
+                       
+}
+
+void gps_ats()
+{
+    //fl waktu;
+    if(cek_gps==1){
+                sscanf(data_gps, "$GPGGA,%f,%f,%c,%f,%c",&waktu, &laty, &ns, &longy, &ew) ;
+           
+                if(ns == 'S') {    laty  *= -1.0; }
+                if(ew == 'W') {    longy *= -1.0; }
+                degrees = trunc(laty / 100.0f);
+                minutes = laty - (degrees * 100.0f);
+                lat_target = degrees + minutes / 60.0f;    
+                degrees = trunc(longy / 100.0f);
+                minutes = longy- (degrees * 100.0f);
+                long_target = degrees + minutes / 60.0f;  
+            }
+}
+
+void bin_dec_conv(unsigned int data)// anything to binary
+{
+    pc.printf("%d%d%d",(data/100),(data%100/10),(data%10));
+}
+
+void telemetry_gps(unsigned int data_1_x,unsigned int data_2_x, unsigned int data_2_y,unsigned int data_3_x, unsigned int data_3_y, unsigned int data_3_z)
+{
+
+        pc.putc(0x0D);
+        bin_dec_conv(106);
+        pc.putc(0x20);
+        
+        bin_dec_conv(data_1_x);
+        pc.putc(0x20);
+        
+        bin_dec_conv(data_2_x);
+        pc.putc(0x20);
+        
+        bin_dec_conv(data_2_y);
+        pc.putc(0x20);
+        
+        bin_dec_conv(data_3_x);
+        pc.putc(0x20);
+        
+        bin_dec_conv(data_3_y);
+        pc.putc(0x20);
+        
+        bin_dec_conv(data_3_z);
+
+}
 void getTemperatueHumidity(){
     int err;
     pc.printf("\r\nStarting to gather temperature and humidity......\n");
@@ -16,7 +145,7 @@
     while (1) {
  
         err = sensor.readData();
-        if (err == 0) {
+        if (err == 0||err==6) {
             pc.printf("Temperature is %4.2f C \r\n",sensor.ReadTemperature(CELCIUS));
             pc.printf("Temperature is %4.2f F \r\n",sensor.ReadTemperature(FARENHEIT));
             pc.printf("Temperature is %4.2f K \r\n",sensor.ReadTemperature(KELVIN));
@@ -26,24 +155,101 @@
             xbee.printf("Temperature is %4.2f F \r\n",sensor.ReadTemperature(FARENHEIT));
             xbee.printf("Temperature is %4.2f K \r\n",sensor.ReadTemperature(KELVIN));
             xbee.printf("Humidity is %4.2f \r\n",sensor.ReadHumidity());
+            float gasvalue=gassensor.read();
+             wait(1);
+             xbee.printf("gas sensor reading is %f\n", gasvalue);
      
             return;
         } else {
             pc.printf("\r\nErr %i \n",err);
             xbee.printf("\r\nErr %i \n",err);
+           
         }
         wait(5);
     }
 }
 
 void printInit(){
-     xbee.printf("Awaiting User Command....\n");
+     xbee.printf("\nAwaiting User Command....\n");
+     pc.printf("\nAwaiting User Command....\n");
 }
+void rovercommand(){
+        char input;
+        input= xbee.getc();
+        motor.baud(9600);
+        xbee.printf("command is %c\n",input);
+        if ( input == 'F'){
+            motor.putc(64);
+            wait(1);
+            motor.putc(127);
+            input=NULL;
+            return;
+            }
+        if ( input == 'B'){
+            motor.putc(64);
+            wait(1);
+            motor.putc(1);
+            input=NULL;
+            return;
+            }
+        if ( input == 'S'){
+            motor.putc(64);
+            wait(1);
+            input=NULL;
+            return;
+            }
+        if ( input == 'X'){
+            motor.putc(64);
+            input=NULL;
+            return;
+            }
+        
+    }
+
+void rovercontrol(){
+    wait(1);
+    
+    while(true){
+       char input;
+        input= xbee.getc();
+        motor.baud(9600);
+
+        if ( input == 'F'){
+            motor.putc(64);
+            wait(1);
+            motor.putc(127);
+            input=NULL;
+           
+            }
+        if ( input == 'B'){
+            motor.putc(64);
+            wait(1);
+            motor.putc(1);
+            input=NULL;
+            
+            }
+        if ( input == 'S'){
+            motor.putc(64);
+            wait(1);
+            input=NULL;
+            
+            }
+        if ( input == 'X'){
+            motor.putc(64);
+            input=NULL;
+            return;
+            }
+        
+        }
+    }
 
 int main() {
    
     char a;
     printInit();
+    gps.baud(9600);
+    gps.attach(&gps_interrupt);
+    initialize_gps();
     while(1){
         a = xbee.getc();        
         if(a == 'T'){
@@ -54,9 +260,19 @@
         
         if( a == 'G'){
             //GPS function
+            get_gps();
+            xbee.printf("Your Current location is %f %c,%f %c."latx,ns, longx,ew);
             a = NULL;
             printInit();    
         }      
+        if( a == 'R'){
+            xbee.printf("\nEntering Rover Controlling mode...");
+            xbee.printf("\nWork in progress, exiting :)\n");
+            rovercontrol();
+            
+            a = NULL;
+            printInit();
+            }
+        
     }
 }
-