/***************************************************************************/ /* 22 Jan 17 Started afresh. Just getting the motor to turn in all ways possible.*/ /* Noticed that when I added the following lines in the setup I had a problem in that the motor would not work*/ /* //pinMode(TxPin, OUTPUT); // sets the digital pin as output /* //pinMode(RxPin, INPUT); // sets the digital pin as output*/ /* Removed those two lines */ #include "Wire.h" #include #include // for HC-05 #include // for DC Motor SoftwareSerial BTSerial(2, 3); // RXPin = 2, TXPin = 3 for HC-05 module int OB_GPS_Rx_length; int count=0; // Counter to keep track of how many times OBGPS data was not available static const uint32_t BTBaud = 38400; String BT_GPS_data; // a string to hold incoming data String OB_GPS_Rx; // String to hold OBGPS data received serially String latitudedeg,latitudemin,latitudesec1_2,latitudesec3_4,latitudesec,latitude; //Strings to hold LAT / LONG data String longitudedeg,longitudemin,longitudesec1_2,longitudesec3_4,longitudesec,longitude; String obgpslatitudedeg,obgpslatitudemin; //Strings to hold LAT / LONG data String obgpslongitudedeg,obgpslongitudemin; String obgpslatdeg,obgpslongdeg,obgpslatmin,obgpslongmin; // to convert received OBGPS data into float float btgpslatitude_DEC,btgpslongitude_DEC,obgps_lat_DEC,obgps_lon_DEC; float prev_btlat, prev_btlon, prev_oblat,prev_oblon,diff_lat, diff_lon; // variables for previous lat and long int xplus,xminus,yplus,yminus; // to decide x y directions String OBGPSdatastatus,Pres_Course,Dir2Go; String Ok2TurnRight, Ok2TurnLeft; float PC,DC,course_diff,LT,RT; // Present Course, Desired Course,Left Turn, Right Turn int PC_calc,DC_calc; int delay_count, delay_value; //long duration1, distanceRHS,duration2, distanceLHS; // for echo sounder // Defining LCD and Pins for interfacing. LiquidCrystal_SR lcd(6, 5, 9); // Pin 6 - Data Enable/ SER, Pin 5 - Clock/SCL, Pin 9 -SCK // Define trigger and echo pins for distance sensor //#define trigPin 4 // For use of distance sensor //#define echoPinLHS 10 // For distance sensor LHS - BLUE WIRE //#define echoPinRHS 8 // RHS - GREEN WIRE AF_DCMotor motorR(1, MOTOR12_64KHZ); // create motor #1, 64KHz pwm RIGHT MOTOR AF_DCMotor motorL(2, MOTOR12_64KHZ); // create motor #2, 64KHz pwm LEFT MOTOR int obgps_feed_avail = 0; // flag to check if obgps is available // Introduced for averaging BTGPS lat long 29 Jan 17 const int numReadings = 2; float avglatitude[numReadings]; // the readings from the analog input float avglongitude[numReadings]; int index = 0; // the index of the current reading float totallatitude = 0; // the running total float totallongitude =0; float averagelatitude = 0; // the average float averagelongitude = 0; /******************************************/ /**************SETUP **********************/ /*****************************************/ void setup() { Wire.begin(); Serial.begin(9600); // set up Serial library at 9600 bps lcd.begin(20,4); // originally 16 characters, 2 rows lcd.cursor(); motorR.setSpeed(250); motorL.setSpeed(250); BTSerial.begin(BTBaud); // This has to be 38400 for BT to work // initialize all the readings to 0: for (int thisReading = 0; thisReading < numReadings; thisReading++) { avglatitude[thisReading] = 0; totallatitude = 0; avglongitude[thisReading] = 0; totallongitude = 0; } delay_count=0; //Serial.println(F("This FMR board receives GPS location of the master and transmits it serially to the OBGPS board.")); //Serial.println(F("FMR Uses the TinyGPS++ library v. by Mikal Hart. Program adapted by Ramesh")); } /*************************************/ /******START OF LOOP SECTION**********/ /*************************************/ void loop() { if (obgps_feed_avail ==0) { //lcd.clear(); lcd.setCursor(0,1); lcd.print("PLEASE WAIT. ONBOARD"); lcd.setCursor(0,2); lcd.print("GPS FEED AWAITED..."); } /************************************************/ /******SERIAL RECEPTION FROM OBGPS BOARD ********/ /************************************************/ //Serial.println("FMR Rx"); while( Serial.available()>0) { char d = Serial.read(); //Conduct a serial read if (d =="#") { break; } //Exit the loop when the $ is detected after the word else { OB_GPS_Rx += d; //Shorthand for OB_GPS_Rx = OB_GPS_Rx + d } } delay(1500); //Serial.println(OB_GPS_Rx); // Display received OBGPS info OB_GPS_Rx_length =OB_GPS_Rx.length(); // to check if OBGPS data is received Serial.println(OB_GPS_Rx_length); if (OB_GPS_Rx_length > 35) { /********************************************************/ /***Introduce method to split the OBGPS data received **/ /*******************************************************/ /* Format of serial data received @OBGPS1# 9.962203$76.287849%227.24R1L0 */ int startobgpsLAT = OB_GPS_Rx.indexOf('#'); // OBGPS LAT int startobgpsLONG = OB_GPS_Rx.indexOf('$'); // OBGPS LONG int startOBGPSstat = OB_GPS_Rx.indexOf('@'); // OBGPS Status int startPC = OB_GPS_Rx.indexOf('%'); // OBGPS COURSE int startRightObst=OB_GPS_Rx.indexOf('R'); // OBGPS R DISTANCE int startLeftObst=OB_GPS_Rx.indexOf('L'); // OBGPS L DISTANCE obgpslatdeg=OB_GPS_Rx.substring(startobgpsLAT+1,startobgpsLAT+4); //Serial.print(obgpslatdeg); // For debugging obgpslatmin=OB_GPS_Rx.substring(startobgpsLAT+4,startobgpsLAT+10); //Serial.print(obgpslatmin); // For debugging //Serial.print(" "); obgpslongdeg=OB_GPS_Rx.substring(startobgpsLONG+1,startobgpsLONG+4); //Serial.print(obgpslongdeg); // For debugging obgpslongmin=OB_GPS_Rx.substring(startobgpsLONG+4,startobgpsLONG+10); //Serial.println(obgpslongmin); OBGPSdatastatus=OB_GPS_Rx.substring(startOBGPSstat+6,startOBGPSstat+7); //Serial.print(OBGPSdatastatus); Ok2TurnRight=OB_GPS_Rx.substring(startRightObst+1,startRightObst+2); RT=Ok2TurnRight.toFloat(); Ok2TurnLeft=OB_GPS_Rx.substring(startLeftObst+1,startLeftObst+2); LT=Ok2TurnLeft.toFloat(); Pres_Course=OB_GPS_Rx.substring(startPC+1,startPC+4); PC=Pres_Course.toFloat(); // Converting to numeric valu for calcs if(OBGPSdatastatus=="1") { count=0; // reset missed OBGPS data counter // Serial.println("Uncorrected OBGPS Data"); // Serial.print(obgps_lat_DEC,6); // For debugging // Serial.print(" "); // For debugging // Serial.println(obgps_lon_DEC,6); // For debugging /************************/ /***Update LCD display **/ /************************/ lcd.setCursor(0,0); lcd.print("OBGPS OK "); obgps_lat_DEC=(obgpslatdeg.toFloat()+(obgpslatmin.toFloat()/1000000)); obgps_lon_DEC=(obgpslongdeg.toFloat()+(obgpslongmin.toFloat()/1000000)); lcd.setCursor(0,1); lcd.print("GLO ");lcd.print(obgps_lon_DEC,4); lcd.setCursor(12,1); lcd.print("LT");lcd.print(obgps_lat_DEC,4); Serial.print(obgps_lat_DEC,4); // For debugging Serial.print(" "); // For debugging Serial.println(obgps_lon_DEC,4); // For debugging if (obgps_feed_avail==0) { obgps_feed_avail=1; lcd.clear(); lcd.setCursor(0,0); lcd.print("GPS FEED AVAILABLE"); delay(2500); // For persistence of vision lcd.clear(); } } else { lcd.setCursor(0,0); lcd.print("OBGPS X "); } } // end of if (OB_GPS_Rx_length > 35) /**********************************************************************/ /***Section to receive Master GPS (FMR) data through bluetooth ********/ /**********************************************************************/ /* Format of serial data received #0957719@7617266 */ BTSerial.listen(); // To ensure only BT port is being listened to while (BTSerial.available()>0) { char c = BTSerial.read(); //Conduct a serial read //Serial.print (c); if (c =="$") { break; } //Exit the loop when the $ is detected after the word else { BT_GPS_data += c; //Shorthand for BT_GPS_data = BT_GPS_data + c } } // End of Listening to BT Port /*************************************************************************/ /***Introduce method to split the received BT GPS data into LAT and LONG**/ /*************************************************************************/ /* $GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47 Where: GGA Global Positioning System Fix Data 123519 Fix taken at 12:35:19 UTC 4807.038,N Latitude 48 deg 07.038' N 01131.000,E Longitude 11 deg 31.000' E 1 Fix quality: 0 = invalid 1 = GPS fix (SPS) 2 = DGPS fix 3 = PPS fix 4 = Real Time Kinematic 5 = Float RTK 6 = estimated (dead reckoning) (2.3 feature) 7 = Manual input mode 8 = Simulation mode 08 Number of satellites being tracked 0.9 Horizontal dilution of position 545.4,M Altitude, Meters, above mean sea level 46.9,M Height of geoid (mean sea level) above WGS84 ellipsoid (empty field) time in seconds since last DGPS update (empty field) DGPS station ID number *47 the checksum data, always begins with * /* /*************************************************************/ //Serial.println(BT_GPS_data); int startLAT = BT_GPS_data.indexOf('P'); // Marks the position where latitude info starts int startLONG = BT_GPS_data.indexOf('N'); // Marks the position where logitude info starts latitudedeg= BT_GPS_data.substring(startLAT+12,startLAT+14); //Serial.println(latitudedeg); latitudemin= BT_GPS_data.substring(startLAT+14,startLAT+16); // First two decimal places - minutes latitudesec=BT_GPS_data.substring(startLAT+17,startLAT+22); //Serial.println(latitudesec); latitudesec1_2= BT_GPS_data.substring(startLAT+17,startLAT+19); // First two places - seconds latitudesec3_4= BT_GPS_data.substring(startLAT+19,startLAT+22); latitude= latitudedeg+"."+latitudemin+latitudesec1_2; // For LCD Display use only btgpslatitude_DEC=(((latitudedeg.toFloat())+(latitudemin.toFloat()/60)+(latitudesec.toFloat()/360000000))); //Serial.println(btgpslatitude_DEC,6); longitudedeg = BT_GPS_data.substring (startLONG+3,startLONG+5); //Serial.println(longitudedeg); longitudemin = BT_GPS_data.substring (startLONG+5,startLONG+7); // First two places corresponding to minutes longitudesec=BT_GPS_data.substring(startLONG+8,startLONG+13); //Serial.println(longitudesec); longitudesec1_2 = BT_GPS_data.substring (startLONG+8,startLONG+10); // First two places - seconds longitudesec3_4 = BT_GPS_data.substring (startLONG+10,startLONG+12); //Serial.println(longitudemin); longitude= longitudedeg+"."+longitudemin+longitudesec1_2; // For LCD Display use only btgpslongitude_DEC=(((longitudedeg.toFloat())+(longitudemin.toFloat()/60)+(longitudesec.toFloat()/360000000))); //Serial.println(btgpslongitude_DEC,6); // Introduced on 29 Jan 17 to average BT GPS data totallatitude = totallatitude - avglatitude[index]; avglatitude[index] = btgpslatitude_DEC; totallatitude = totallatitude + avglatitude[index]; totallongitude = totallongitude - avglongitude[index]; avglongitude[index] = btgpslongitude_DEC; totallongitude = totallongitude + avglongitude[index]; //Serial.println(totallongitude,6); index = index + 1; // if we're at the end of the array... if (index >= numReadings) // ...wrap around to the beginning: index=0; // calculate the average: averagelatitude = totallatitude / numReadings; averagelongitude = totallongitude / numReadings; //Serial.println(BT_GPS_data); // Display received GPS info /***********************************************/ /* CALCULATE COURSE AND STORE PREVIOUS VALUES **/ /***********************************************/ if (delay_count>15) { delay_count=0; //PC_calc=CalcBearing(prev_btlat, prev_btlon,btgpslatitude_DEC, btgpslongitude_DEC); // To be used for debugging when phone GPS is placed on buggy //Serial.print("Pres Course "); Serial.println(PC_calc); PC_calc=CalcBearing(prev_oblat, prev_oblon,obgps_lat_DEC, obgps_lon_DEC); Serial.print("PC Cal "); Serial.println(PC_calc); DC_calc=CalcBearing(obgps_lat_DEC, obgps_lon_DEC,btgpslatitude_DEC, btgpslongitude_DEC); Serial.print("DC Cal "); Serial.println(DC_calc); if (obgps_feed_avail==1) { lcd.setCursor(0,2); lcd.print("MLO ");lcd.print( averagelongitude,4); lcd.setCursor(12,2); lcd.print("LT");lcd.print(averagelatitude,4); lcd.setCursor(0,3);lcd.print("GC "); lcd.setCursor(3,3);lcd.print(PC,0); lcd.setCursor(7,3);lcd.print("CC "); lcd.setCursor(10,3);lcd.print(PC_calc); lcd.setCursor(14,3);lcd.print("DC "); lcd.setCursor(17,3);lcd.print(DC_calc); gomove(); } prev_btlat=btgpslatitude_DEC; prev_btlon=btgpslongitude_DEC; prev_oblat=obgps_lat_DEC; prev_oblon=obgps_lon_DEC; } delay_count=delay_count+1; /********************************************/ /******** DEBUGGING SECTION ***************/ /*******************************************/ // Serial.print(latitude); // Serial.print(" "); // For diagnostics on the serial monitor // Serial.println (longitude); // Serial.println("BT GPS DATA"); // Serial.print(averagelongitude,6); // Serial.print(" "); // Serial.println(averagelatitude,6); // // Serial.println("Corrected OBGPS Data"); // Serial.println("OBGPS DATA"); // Serial.print(obgps_lon_DEC,6); // For debugging // Serial.print(" "); // Serial.println(obgps_lat_DEC,6); // For debugging // // Serial.println("DIFFERENCE"); // Serial.print(diff_lon,6); // To calculate errors between two GPS // Serial.print(" "); // Serial.println(diff_lat,6); // // Serial.print (xplus); Serial.print(" ");Serial.println (xminus); // Serial.print (yplus); Serial.print(" ");Serial.println (yminus); /***************************************************/ /************** CLEAN UP OF BT LAT LONG ************/ /***************************************************/ latitudedeg=""; latitudemin=""; latitudesec1_2=""; latitudesec3_4=""; latitude=""; longitudedeg=""; longitudemin=""; longitudesec1_2=""; longitudesec3_4=""; longitude=""; obgpslatdeg=""; obgpslongdeg=""; obgpslatmin=""; obgpslongmin=""; obgpslatitudedeg=""; obgpslatitudemin=""; obgpslongitudedeg=""; obgpslongitudemin=""; BT_GPS_data=""; //Reset the variable after initiating OB_GPS_Rx=""; } /*************************************/ /******END OF LOOP SECTION **********/ /*************************************/ /*************************************/ /********* GO MOVE FUNCTION **********/ /*************************************/ void gomove() { course_diff= DC_calc-PC_calc; if ((course_diff >5) && (course_diff<=180)) { lcd.setCursor(17,0); lcd.print("->"); goright(); } if ((course_diff <-5) && (course_diff > -180) || ((course_diff>180) &&(course_diff<355))) { lcd.setCursor(17,0); lcd.print("<-"); goleft(); } else { lcd.setCursor(17,0); lcd.print("FW"); motorR.run(FORWARD); // Drive Forward motorL.run(FORWARD); // } } /*************************************/ /********* GO RIGHT FUNCTION *********/ /*************************************/ void goright() { if (RT>0) { lcd.setCursor(17,0); lcd.print("RT"); motorL.run(FORWARD); // the other way motorR.run(BACKWARD); // the other way delay(4000); } } /************************************/ /********* GO LEFT FUNCTION *********/ /************************************/ void goleft() { if (LT>0) { lcd.setCursor(17,0); lcd.print("LT"); motorR.run(FORWARD); // Drive Left motorL.run(BACKWARD); // delay(4000); } } /************************************/ /********* GO BACK FUNCTION *********/ /************************************/ void goback() { lcd.setCursor(17,0); lcd.print("BK"); motorR.run(BACKWARD); // Drive Backwards motorL.run(BACKWARD); // delay(5000); } /*************************************/ /******CalcBearing Function **********/ /*************************************/ //convert degrees to radians double dtor(double fdegrees) { return(fdegrees * PI / 180); } //Convert radians to degrees double rtod(double fradians) { return(fradians * 180.0 / PI); } //Calculate bearing from lat1/lon1 to lat2/lon2 //Note lat1/lon1/lat2/lon2 must be in radians //Returns bearing in degrees int CalcBearing(double lat1, double lon1, double lat2, double lon2) { Serial.print("lat1 [degrees]");Serial.println(lat1,6); Serial.print("lon1 [degrees]");Serial.println(lon1,6); Serial.print("lat2 [degrees]");Serial.println(lat2,6); Serial.print("lon2 [degrees]");Serial.println(lon2,6); lat1 = dtor(lat1); Serial.print("lat1 [radians]");Serial.println(lat1,6); lon1 = dtor(lon1); Serial.print("lon1 [radians]");Serial.println(lon1,6); lat2 = dtor(lat2);Serial.print("lat2 ");Serial.println(lat2,6); lon2 = dtor(lon2);Serial.print("lon2 ");Serial.println(lon2,6); //determine angle double bearing = atan2(sin(lon2-lon1)*cos(lat2), (cos(lat1)*sin(lat2))-(sin(lat1)*cos(lat2)*cos(lon2-lon1))); //Serial.print //convert to degrees bearing = rtod(bearing); //use mod to turn -90 = 270 bearing = fmod((bearing + 360.0), 360); return (int) bearing + 0.5; }