  */
  void forward(){
   dataStruct.tar = 20;
-  dataStruct.val = 25;
+  dataStruct.val = mSpeed / 2;
   etData.sendData();
   dataStruct.tar = 21;
-  dataStruct.val = -25;
+  dataStruct.val = -mSpeed / 2;
   etData.sendData();
   Serial.print("forward ");
  }

#include <Adafruit_BNO055.h>
#include <EasyTransfer.h>

//Minimumn distance in meters to waypoint
#define MIN_DISTANCE 2

//Turn directions
#define TURN_LEFT 1

   } else {
     //Navigate to the way point
     //Adjust max amount to be off course
     int maxTurnRange = MAX_TURN_RANGE;
     if(turning){
       maxTurnRange = TURNING_TURN_RANGE;
     } else {
       maxTurnRange = MAX_TURN_RANGE;
     }
     
     //Base turning range off of distance
     int turnRange = constrain(map(distance, CONSTRAIN_MIN, CONSTRAIN_MAX, MIN_TURN_RANGE, maxTurnRange), MIN_TURN_RANGE, maxTurnRange);     
     int turndirection = getdirection(currentHeading, destheading);

     if(abs(currentHeading - destheading) < turnRange){
