/* LMU uninitialised data */ StartOfUninitialised_LMURam_Variables /* Put your LMU RAM fast access variables that have no initial values here e.g. uint32 LMU_var; */ EndOfUninitialised_LMURam_Variables /* LMU uninitialised data */ StartOfInitialised_LMURam_Variables const int ledPin = 13; int ledStateOne = LOW; int ledStateTwo = LOW; int ledStateThree = LOW; int ledStateFour = LOW; unsigned long previousMicrosOne = 0; unsigned long previousMicrosTwo = 0; unsigned long previousMicrosThree = 0; unsigned long previousMicrosFour = 0; unsigned long intervalOne = 500; // Right hand wheel turn speed unsigned long intervalTwo = 500; // Left hand wheel turn speed. unsigned long intervalThree = 1000; // Right hand wheel drive speed. unsigned long intervalFour = 1000; // Left hand wheel drive speed. int difference=0; int previousFinalSteeringValue=15300; long rightWheel=0; long leftWheel=0; long wheelsPosition=0; long ferrets=0; int finalDriveValue=0; long finalSteeringValue =0; int forwards = LOW; int backwards =LOW; int rightHandLock =LOW; int leftHandLock=LOW; int clockW=LOW; int antiClockW=LOW; EndOfInitialised_LMURam_Variables /*** Core 0 ***/ void setup() { delay(5000); pinMode(ledPin, OUTPUT); pinMode(5,OUTPUT); //STEP Steer Motor pinMode(6,OUTPUT); //DIRECTION HIGH is clockwise pinMode(7,OUTPUT); //STEP Steer Motor pinMode(8,OUTPUT); //DIRECTION HIGH is clockwise pinMode(9,OUTPUT); //STEP Drive Motor pinMode(10,OUTPUT); //DIRECTION HIGH is clockwise pinMode(11,OUTPUT); //STEP Drive Motor pinMode(12,OUTPUT); //DIRECTION HIGH is clockwise } void loop() { unsigned long currentMicrosOne = micros(); unsigned long currentMicrosTwo = micros(); unsigned long currentMicrosThree = micros(); unsigned long currentMicrosFour = micros(); speedDifferential(); if (finalDriveValue>=600) //Backwards. { backwards = HIGH; forwards = LOW; intervalThree = (600000/finalDriveValue)-450; // 140 is max speed. intervalFour = (600000/finalDriveValue)-450; // 140 is max speed. speedDifferential(); digitalWrite(10,HIGH); digitalWrite(12,HIGH); if (currentMicrosThree - previousMicrosThree >= intervalThree) { changeStateThree(); digitalWrite(9,ledStateThree); // Drive motor step } if (currentMicrosFour - previousMicrosFour >= intervalFour) { changeStateFour(); digitalWrite(11,ledStateFour); // Drive motor step } } if (finalDriveValue<400)//Forwards. { backwards = LOW; forwards = HIGH; intervalThree = (finalDriveValue*2)+100; // 140 is max speed. Right hand wheel. intervalFour = (finalDriveValue*2)+100; // 140 is max speed. speedDifferential(); digitalWrite(10,LOW); digitalWrite(12,LOW); if (currentMicrosThree - previousMicrosThree >= intervalThree) { changeStateThree(); digitalWrite(9,ledStateThree); // Drive motor step } if (currentMicrosFour - previousMicrosFour >= intervalFour) { changeStateFour(); digitalWrite(11,ledStateFour); // Drive motor step } } ////////////////////////////////////////////////////////////////////////////////////////////////////// difference = finalSteeringValue - previousFinalSteeringValue; // This gives actual movement of the steering. ////////////////////////////////////////////////////////////////////////////////////////////////////// if((difference>500)&&(wheelsPosition>=0)) // Clockwise from the centre { clockWise(); if (currentMicrosTwo - previousMicrosTwo >= intervalTwo) // Right wheel turns(SLOW) { changeStateTwo(); digitalWrite(7,ledStateTwo); //STEP } if (currentMicrosOne - previousMicrosOne >= intervalOne) // Left wheel turns(FAST) { changeStateOne(); digitalWrite(5,ledStateOne); //STEP wheelsPosition++; previousFinalSteeringValue++; // This must be within backets containing 'changeStateOne'. } } ////////////////////////////////////////////////////////////////////////////////////////////////////// if((difference>500)&&(wheelsPosition<0)) // Clockwise from the full lock position { clockWise(); if (currentMicrosOne - previousMicrosOne >= intervalOne) // Right wheel turns (FAST) { changeStateOne(); digitalWrite(7,ledStateOne); //STEP wheelsPosition++; previousFinalSteeringValue++; // This must be within backets containing 'changeStateOne'. } if (currentMicrosTwo - previousMicrosTwo >= intervalTwo) // Left wheel turns(SLOW) { changeStateTwo(); digitalWrite(5,ledStateTwo); //STEP } } ////////////////////////////////////////////////////////////////////////////////////////////////////// if((difference<-500)&&(wheelsPosition>=0)) // Anti-clockwise from the full lock position { antiClockWise(); if (currentMicrosTwo - previousMicrosTwo >= intervalTwo) // Right wheel turns(SLOW) { changeStateTwo(); digitalWrite(7,ledStateTwo); //STEP } if (currentMicrosOne - previousMicrosOne >= intervalOne) // Left wheel turns(FAST) { changeStateOne(); digitalWrite(5,ledStateOne); //STEP wheelsPosition--; previousFinalSteeringValue--; // This must be within backets containing 'changeStateOne'. } } ////////////////////////////////////////////////////////////////////////////////////////////////////// if((difference<-500)&&(wheelsPosition<0)) // Anti-clockwise from the centre { antiClockWise(); if (currentMicrosOne - previousMicrosOne >= intervalOne) // Right wheel turns(FAST) { changeStateOne(); digitalWrite(7,ledStateOne); //STEP wheelsPosition--; previousFinalSteeringValue--; // This must be within backets containing 'changeStateOne'. } if (currentMicrosTwo - previousMicrosTwo >= intervalTwo) // Left wheel turns (SLOW) { changeStateTwo(); digitalWrite(5,ledStateTwo); //STEP } } if(wheelsPosition>0) { rightHandLock=HIGH; leftHandLock=LOW; } if(wheelsPosition<0) { rightHandLock=LOW; leftHandLock=HIGH; } ////////////////////////////////////////////////////////////////////////////////////////////////////// difference = finalSteeringValue - previousFinalSteeringValue; } ////////////////////////////////////////////////////////////////////////////////////////////////////// void speedDifferential() { ferrets = map(wheelsPosition,-1,-15000,1,15000); //Changes all negative values of wheelsPosition to positive. if(wheelsPosition>200) { intervalThree = intervalThree+(wheelsPosition*intervalThree)/20000; // Makes right hand inside wheel drive slower. } if(wheelsPosition<-200) { intervalFour = intervalFour+(ferrets*intervalFour)/20000; // Makes left hand inside wheel drive slower. } actualTurnSpeedDifferential(); } void actualTurnSpeedDifferential() // When the wheels are actually steering, inside wheel can grind badly as it turns against drive. { if((clockW==HIGH)&&(rightHandLock==HIGH)&&(forwards==HIGH)&&((difference<-500)||(difference>500))) { intervalThree = intervalThree+(wheelsPosition*intervalThree)/10000; // Makes right hand inside wheel drive much slower. } if((antiClockW==HIGH)&&(leftHandLock==HIGH)&&(forwards==HIGH)&&((difference<-500)||(difference>500))) { intervalFour = intervalFour+(ferrets*intervalFour)/10000; // Makes left hand inside wheel drive much slower. } //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// if((antiClockW==HIGH)&&(rightHandLock==HIGH)&&(backwards==HIGH)&&((difference<-500)||(difference>500))) { intervalThree = intervalThree+(wheelsPosition*intervalThree)/10000; // Makes right hand inside wheel drive much slower. } if((clockW==HIGH)&&(leftHandLock==HIGH)&&(backwards==HIGH)&&((difference<-500)||(difference>500))) { intervalFour = intervalFour+(ferrets*intervalFour)/10000; // Makes left hand inside wheel drive much slower. } } /*** Core 1 ***/ /* CPU1 Uninitialised Data */ StartOfUninitialised_CPU1_Variables /* Put your CPU1 fast access variables that have no initial values here e.g. uint32 CPU1_var; */ EndOfUninitialised_CPU1_Variables /* CPU1 Initialised Data */ StartOfInitialised_CPU1_Variables /* Put your CPU1 fast access variables that have an initial value here e.g. uint32 CPU1_var_init = 1; */ long driveValue=0; long steeringValue =0; int k=0; EndOfInitialised_CPU1_Variables void setup1() { } void loop1() { steeringValue=0; driveValue=0; k=0; while(k<200) { steeringValue = steeringValue + analogRead(A0); driveValue = driveValue + analogRead(A1); k++; } finalDriveValue = driveValue/k; finalSteeringValue = 30*steeringValue/k; delay(10); } void changeStateOne() { unsigned long currentMicrosOne = micros(); previousMicrosOne = currentMicrosOne; if (ledStateOne == LOW) { ledStateOne = HIGH; } else { ledStateOne = LOW; } digitalWrite(ledPin, ledStateOne); } void changeStateTwo() { unsigned long currentMicrosTwo = micros(); previousMicrosTwo = currentMicrosTwo; if (ledStateTwo == LOW) { ledStateTwo = HIGH; } else { ledStateTwo = LOW; } digitalWrite(ledPin, ledStateTwo); } void changeStateThree() { unsigned long currentMicrosThree = micros(); previousMicrosThree = currentMicrosThree; if (ledStateThree == LOW) { ledStateThree = HIGH; } else { ledStateThree = LOW; } digitalWrite(ledPin, ledStateThree); } void changeStateFour() { unsigned long currentMicrosFour = micros(); previousMicrosFour = currentMicrosFour; if (ledStateFour == LOW) { ledStateFour = HIGH; } else { ledStateFour = LOW; } digitalWrite(ledPin, ledStateFour); } void clockWise() { clockW=HIGH;antiClockW=LOW; intervalOne = 2000; // Speed of outer wheel turn (fast) higher the value, slower the speed. Min 1000. intervalTwo = 2500; // Speed of inner wheel turn (slow) higher the value, slower the speed. Min 1250. digitalWrite(6,HIGH); //DIRECTION HIGH is clockwise digitalWrite(8,HIGH); //DIRECTION HIGH is clockwise unsigned long currentMicrosOne = micros(); unsigned long currentMicrosTwo = micros(); } void antiClockWise() { clockW=LOW;antiClockW=HIGH; intervalOne = 2000; // Fast intervalTwo = 2500; // Slow digitalWrite(6,LOW); //DIRECTION HIGH is clockwise digitalWrite(8,LOW); //DIRECTION HIGH is clockwise unsigned long currentMicrosOne = micros(); unsigned long currentMicrosTwo = micros(); } /*** Core 2 ***/ /* CPU2 Uninitialised Data */ #include #include "tone.h" // TwoWire Wire; // Instantiate TwoWire class #include "SPI.h" #include "Adafruit_GFX.h" #include "Adafruit_ILI9341.h" #define TFT_CS 40 #define TFT_RST 24 // you can also connect this to the Arduino reset #define TFT_DC 22 Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC, TFT_RST); /* LMU uninitialised data */ StartOfUninitialised_LMURam_Variables int i; String initiator; String dataString; String nothing; char c; String y; String w; String a; String b; String d; long latFona; long lonFona; long latitude; long longitude; long latitudeUblox; long longitudeUblox; float bearingDegrees; String laatUbloxString; String longUbloxString; String latFonaString; String lonFonaString; String lat; String lon; String laatUblox; String longUblox; long prevDistMetres; long distanceMetres; String yawString; double yaw; float yawFloat; float heading; char q; char ww[200]; String text4; String text1; String text2; String text3; String textFonaData; String textDistanceData; String textBearingData; char fonaCharacter[100]; int makeTurnValue; EndOfUninitialised_LMURam_Variables /* LMU uninitialised data */ StartOfInitialised_LMURam_Variables String bearing = "100000"; String bearingString = "100000"; String distance = "100000"; String distanceString = "100000"; char url[120]; long x = 12345678; long result=0; String triggerWord = "Important"; int STOP = LOW; char fonaData[40]; int aa=0; char distanceCharacter[40]; String cmd = "S"; int emicBearing=0; char phpPage[2] = "1"; unsigned long previousMillis1Core3 = 0; unsigned long previousMillis2Core3 = 0; const long interval1Core3 = 10000; const long interval2Core3 = 30000; const long interval3Core3 = 20000; unsigned long millisCalc1 = 0; unsigned long millisCalc2 = 0; int phpPageInt = 0; int previousPhpPageInt = 0; String phpPageString ="0"; String inititiator = ""; String compassString = "1000"; String compass = "1000"; float compassFloat = 0; int makeTurn = LOW; String text5 = " .Make a clockwise turn. "; int ledBlueState = LOW; EndOfInitialised_LMURam_Variables void setup2() { Serial1.begin(9600); // Emic //emicIntro(); Wire.begin(); //Wire.begin(); // join i2c bus (address optional for master) SerialASC.begin(115200); //Serial1.begin(115200); SerialASC.println("Ready to test"); tft.begin(); tft.fillScreen(ILI9341_BLACK); tft.setRotation(0); rectangle2 (); tft.setTextColor(ILI9341_BLUE); tft.setTextSize(4); tft.setCursor(0, 20); tft.println("WEEDINATOR"); Wire.beginTransmission(43); // transmit to device #43 FONA Nano. Wire.write(phpPage); Wire.endTransmission(); // stop transmitting tone(2,1000,1000); // pin,pitch,duration delay(1000); noTone(2); pinMode(37, OUTPUT); // BLUE LED pinMode(39, OUTPUT); // ORANGE LED digitalWrite(37,HIGH); digitalWrite(39,HIGH); } void loop2() { if (ledBlueState == LOW) { ledBlueState = HIGH; } else { ledBlueState = LOW; } digitalWrite(37, ledBlueState); makeTurnValue = heading - compassFloat; if((makeTurnValue)>0) // Does not work for quadrants one and four. { makeTurn = HIGH; } else { makeTurn = LOW; } if((heading>=0)&&(heading<90)&&(compassFloat>=270)&&(compassFloat<360)) // Quadrants one and four makeTurn is reversed. { if((makeTurnValue)>0) { makeTurn = LOW; } else { makeTurn = HIGH; makeTurnValue = 360 + makeTurnValue; } } delay(200); unsigned long currentMillisCore3 = millis(); millisCalc1 = currentMillisCore3 - previousMillis1Core3; millisCalc2 = currentMillisCore3 - previousMillis2Core3; ///////////////////////////////////////////////////////////////////////////////////////////////////////// if (millisCalc2 >= 5000) // timer ..... 5,000 { printTftText(); // TFT screen cuases code to pause. previousMillis2Core3=currentMillisCore3; } ///////////////////////////////////////////////////////////////////////////////////////////////////////// if (millisCalc1 >= 30000) // timer .... 30,000 { //emicSpeech1(); previousMillis1Core3=currentMillisCore3; // this is the only previousMillis reset! } ///////////////////////////////////////////////////////////////////////////////////////////////////////// if ((millisCalc1 > 10000)&&(millisCalc1 < 11000)) // timer ..... 10,000 { Wire.beginTransmission(43); // transmit to device #43 FONA Nano. Wire.write(phpPage); Wire.endTransmission(); // stop transmitting } //////////////////////////////////////////////////////////////////////////////////////////////////////// if ((millisCalc1 > 20000)&&(millisCalc1 < 21000)) // timer ..... 20,000 { Wire.requestFrom(43, 27); // request 32 bytes from slave device #43 FONA Data aa=0; a=""; while (Wire.available()) { //SerialASC.println("Recieve data from Fona:"); //delay(1000); char c = Wire.read(); // receive a byte as character if (isalpha(c)) // analyse c for letters { a=a+c; // string = string + character //SerialASC.print("a: ");SerialASC.println(a); if (a=="LAT") { lon=""; lat=a; //SerialASC.print("Trigger word LAT detected!: ");SerialASC.println(lat); a=""; } if (a=="LONG") { lat=""; lon=a; //SerialASC.print("Trigger word LONG detected!: ");SerialASC.println(lon); a=""; } } if (lat=="LAT") { if (isdigit(c)) // analyse c for numerical digit { latFonaString=latFonaString+c; // string = string + character } } if (lon=="LONG") { if (isdigit(c)) // analyse c for numerical digit { lonFonaString=lonFonaString+c; // string = string + character } } if (lat=="LAT") { result=(latFonaString).toInt(); latFona = result; //SerialASC.println(""); //SerialASC.print("Integer latitude Fona: ");SerialASC.println(latFona); } if (lon=="LONG") { result=(lonFonaString).toInt(); lonFona = result; //SerialASC.println(""); //SerialASC.print("Integer longitude Fona: ");SerialASC.println(lonFona); } //SerialASC.print(c); // print the character fonaData[aa] =q; // set each character to "". // Builds the fonaData character: fonaData[aa] = fonaData[aa] + c; //fonaData character is then transmitted to Ublox for analysis. aa++; } } ///////////////////////////////////////////////////////////////////////////////////////////////////////// if((distanceMetres<500)&&(phpPageInt<11)&&(phpPageInt==previousPhpPageInt)) { tone(2,1000,1000); // pin,pitch,duration delay(1000); noTone(2); phpPageInt++; phpPageString = ""; phpPageString = inititiator + phpPageInt; // This is for Emic speech. phpPage[0] = phpPageString[0]; // Character phpPage, which gets transmitted to FONA. } if((distanceMetres>500)&&(phpPageInt<11)&&(phpPageInt>previousPhpPageInt)) { previousPhpPageInt = phpPageInt; } if(distanceMetres!=prevDistMetres) // Make a beep if GNSS data is updated. { prevDistMetres = distanceMetres; tone(2,2000,100); // pin,pitch,duration delay(100); noTone(2); } ///////////////////////////////////////////////////////////////////////////////////////////////////////// i=0; y=""; a=""; b=""; d=""; lat=""; lon=""; latFonaString=""; lonFonaString=""; yawString=""; SerialASC.print("Data captured from Fona: ");SerialASC.println(fonaData); SerialASC.print("Integer latitude Fona: ");SerialASC.println(latFona); SerialASC.print("Integer longitude Fona: ");SerialASC.println(lonFona); SerialASC.print("Integer latitude UBLOX: ");SerialASC.println(latitudeUblox); SerialASC.print("Integer longitude UBLOX: ");SerialASC.println(longitudeUblox); //SerialASC.print("Way point (phpPage)= ");SerialASC.println(phpPage); //SerialASC.print("distanceMetres= ");SerialASC.println(distanceMetres); SerialASC.print("bearingInt= ");SerialASC.println(bearingDegrees); SerialASC.print("Way point (phpPage)= ");SerialASC.println(phpPage); SerialASC.print("distanceMetres= ");SerialASC.println(distanceMetres); SerialASC.print("Compass integer: ");SerialASC.println(compassFloat); SerialASC.println(""); //SerialASC.print("phpPageInt= ");SerialASC.println(phpPageInt); //SerialASC.print("previousPhpPageInt= ");SerialASC.println(previousPhpPageInt); //SerialASC.print("phpPageString= ");SerialASC.println(phpPageString); //SerialASC.print("phpPageString[0]= ");SerialASC.println(phpPageString[0]); //SerialASC.print("phpPageString[1]= ");SerialASC.println(phpPageString[1]); //SerialASC.print("phpPageString[2]= ");SerialASC.println(phpPageString[2]); //SerialASC.print("Final Steering Value= ");SerialASC.println(finalSteeringValue); //SerialASC.print("Previous steering Value= ");SerialASC.println(previousFinalSteeringValue); //SerialASC.print("Difference= ");SerialASC.println(difference); //SerialASC.print("wheelsPosition= ");SerialASC.println(wheelsPosition); //SerialASC.print("Final Drive Value= ");SerialASC.println(finalDriveValue); //SerialASC.print("analogueRead A0= ");SerialASC.println(steeringValue); //SerialASC.print("analogueRead A1= ");SerialASC.println(driveValue); //SerialASC.print("IntervalThree= ");SerialASC.println(intervalThree); //SerialASC.print("IntervalFour= ");SerialASC.println(intervalFour); if(forwards==HIGH) { SerialASC.println("FORWARDS"); } if(backwards==HIGH) { SerialASC.println("BACKWARDS"); } if(rightHandLock==HIGH) { SerialASC.println("RIGHT HAND LOCK"); } if(leftHandLock==HIGH) { SerialASC.println("LEFT HAND LOCK"); } if(clockW==HIGH) { SerialASC.println("GOING CLOCKWISE"); } if(antiClockW==HIGH) { SerialASC.println("GOING ANTI-CLOCKWISE"); } SerialASC.println(""); Wire.beginTransmission(26); // transmit to device #26 Ublox controller Pro Micro. Wire.write(fonaData); Wire.endTransmission(); // stop transmitting ///////////////////////////////////////////////////////////////////////////////////////// Wire.requestFrom(26, 32); // request 32 bytes from slave device #26 UBLOX Data // Dont use 25. a=""; bearing=""; bearingString=""; distance=""; distanceString=""; longUblox=""; laatUblox=""; laatUbloxString=""; longUbloxString=""; compassString = ""; compass=""; //SerialASC.print("C: "); while (Wire.available()) { c = Wire.read(); // receive a byte as character //SerialASC.print(c); if (isalpha(c)) // analyse c for letters { a=a+c; // string = string + character if (a=="LAAT") { longUblox=""; bearing=""; // distance=""; lat=""; lon=""; laatUblox=a; //SerialASC.print("Trigger word LAAT detected!: ");SerialASC.println(laatUblox); a=""; } if (a=="LOON") { lat=""; lon=""; laatUblox=""; bearing=""; // distance=""; longUblox=a; //SerialASC.print("Trigger word LOON detected!: ");SerialASC.println(longUblox); a=""; } if (a=="BEAR") { lat=""; lon=""; longUblox=""; laatUblox=""; // distance=""; bearing=a; // String = string. //SerialASC.print("Trigger word BEAR detected!: ");SerialASC.println(bearingDegrees); a=""; } if (a=="DIST") { lat=""; lon=""; longUblox=""; laatUblox=""; bearing=""; distance=a; // String = string. //SerialASC.print("Trigger word DIST detected!: ");SerialASC.println(distanceString); a=""; } if (a=="COMP") { lat=""; lon=""; longUblox=""; laatUblox=""; bearing=""; distance=""; compass=a; // String = string. //SerialASC.print("Trigger word COMP detected!: ");SerialASC.println(compass); a=""; } } if (laatUblox=="LAAT") { if (isdigit(c)) // analyse c for numerical digit { laatUbloxString=laatUbloxString+c; // string = string + character } } if (longUblox=="LOON") { if (isdigit(c)) // analyse c for numerical digit { longUbloxString=longUbloxString+c; // string = string + character } } if (bearing=="BEAR") { if (isdigit(c)) // analyse c for numerical digit { bearingString=bearingString+c; // string = string + character } } if (distance=="DIST") { if (isdigit(c)) // analyse c for numerical digit { distanceString=distanceString+c; // string = string + character } } if (compass=="COMP") { if (isdigit(c)) // analyse c for numerical digit { compassString=compassString+c; // string = string + character } } if (laatUblox=="LAAT") { result=(laatUbloxString).toInt(); latitudeUblox = result; //SerialASC.println(""); //SerialASC.print("latitude Ublox: ");SerialASC.println(latitudeUblox); } if (longUblox=="LOON") { result=(longUbloxString).toInt(); longitudeUblox = result; //SerialASC.println(""); //SerialASC.print("Longitude Ublox: ");SerialASC.println(longitudeUblox); } if (bearing=="BEAR") { result=(bearingString).toInt(); bearingDegrees = result; heading = bearingDegrees/100; emicBearing=(bearingString).toInt(); //SerialASC.println(""); //SerialASC.print("Heading: ");SerialASC.println(heading,2); } if (distance=="DIST") { result=(distanceString).toInt(); distanceMetres = result; //SerialASC.println(""); //SerialASC.print("distanceMetres integer: ");SerialASC.println(distanceMetres); } if (compass=="COMP") { result=(compassString).toInt(); compassFloat = result/100.00; //SerialASC.println(""); //SerialASC.print("Compass integer: ");SerialASC.println(compassFloat); } } //////// End of I2C write SerialASC.println(""); // SerialASC.print("Latitude: ");SerialASC.println(latitude); // SerialASC.print("Longitude: ");SerialASC.println(longitude); /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // This is where the distance data is compiled into a character .... Do we actually need this any more? distanceString = initiator + distanceString ; int n = distanceString.length(); // SerialASC.print("Distance string in character compile: ");SerialASC.println(distanceString); // SerialASC.print("Size of string: ");SerialASC.println(n); // Builds the character: for (int aa=0;aa<=n;aa++) { distanceCharacter[aa] = distanceString[aa]; } // SerialASC.println(""); // SerialASC.print("Distance character in character compile: ");SerialASC.println(distanceCharacter); // SerialASC.println(""); /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // delay(100); } unsigned long printTftText() { tft.setRotation(0); rectangle2 (); tft.setTextColor(ILI9341_BLUE); tft.setTextSize(4); tft.setCursor(0, 20); tft.println("WEEDINATOR"); tft.setTextColor(ILI9341_GREEN); tft.setTextSize(2); tft.setCursor(0, 72); tft.println("Compass: "); tft.setCursor(110, 72); tft.println(compassFloat); tft.setCursor(185, 67); tft.println("o"); tft.setCursor(0, 102); tft.println("Heading:"); tft.setCursor(110, 102); tft.println(heading,2); tft.setCursor(185, 97); tft.println("o"); tft.setCursor(0, 132); tft.println("Distance: mm "); tft.setCursor(110, 132); tft.println(distanceMetres); tft.setCursor(0, 160); tft.println("Waypoint: "); tft.setCursor(110, 160); tft.println(phpPage); rectangle1 (); tft.setCursor(0, 190); tft.println("Steering Val: "); tft.setCursor(170, 190); tft.println(finalSteeringValue); tft.setCursor(0, 218); tft.println("Difference: "); tft.setCursor(170, 218); tft.println(difference); tft.setCursor(0, 246); tft.println("Wheels Pos: "); tft.setCursor(170, 246); tft.println(wheelsPosition); tft.setTextSize(1); tft.setCursor(0, 274); tft.println("FONA: LAT LON"); tft.setCursor(70, 274); tft.println(latFona); tft.setCursor(170, 274); tft.println(lonFona); tft.setCursor(0, 284); tft.println("UBLOX: LAT LON"); tft.setCursor(70, 284); tft.println(latitudeUblox); tft.setCursor(170, 284); tft.println(longitudeUblox); tft.setTextSize(2); tft.setCursor(0, 302); tft.println("Direction: "); tft.setCursor(170, 302); if(forwards==HIGH) { tft.println("FORWARDS"); } if(backwards==HIGH) { tft.println("BACKWARDS"); } tft.println(finalSteeringValue); } void rectangle1 () { tft.fillRect(0, 186, 250, 135, ILI9341_RED); // x,y,(from top left corner)w,h } void rectangle2 () { tft.fillRect(0, 65, 250, 118, ILI9341_RED); // x,y,(from top left corner)w,h } void flushSerial1() { while (Serial1.available()) { //Flush serial1 int inByte = Serial1.read(); //SerialASC.write(inByte); } } void emicIntro() { // Check for response from Emic 2 Serial1.print('\n'); // Emic 2 returns ':' if ready for command while (Serial1.read() != ':'); // Set volume to maximum Serial1.print('V18\n'); delay(10); Serial1.flush(); Serial1.print('N'); Serial1.print(6); Serial1.print('\n'); Serial1.print('\n'); while (Serial1.read() != ':'); // 'S' command = say cmd = "S"; text1 = "hello world. welcome to the weedinator. Lets go smash the fuck out of some weeds."; Serial1.print(cmd + text1 + "\n"); ; } void emicSpeech1() { int ss=30; for (int aa=0;aa<=ss;aa++) { fonaCharacter[aa] = fonaData[aa]; } // Check for response from Emic 2 Serial1.print('\n'); // Emic 2 returns ':' if ready for command while (Serial1.read() != ':'); delay(10); Serial1.flush(); Serial1.print('\n'); Serial1.print('\n'); while (Serial1.read() != ':'); // 'S' command = say cmd = "S"; text1 = " and this is the latitude data from the FONA module."; text2 = ".. and this is the distance for the weedinator to travel."; text3 = "and the bearing is."; textFonaData = fonaData; textDistanceData = distanceCharacter; //SerialASC.print("Distance to travel: ");SerialASC.println(distanceCharacter); //SerialASC.print("Distance to travel: ");SerialASC.println(distanceMetres); if(makeTurn == HIGH) { text5 = " Make a clock wise turn. of .."; } else { text5 = " Make an anti clock wise turn. of .."; } text4 = "We are now going to way point " + phpPageString + text2 + distanceMetres + " .. milli metres .. " + text3 + emicBearing/100 + " .. degrees .. " + text5 + makeTurnValue + " .. degrees .. End of message .. "; //SerialASC.println(text4); Serial1.print(cmd + text4 + "\n"); // "Here is some very interesting information too listen to .. " + ; }