Need someone to help me modify a code

I’ve been working on an autonomous boat lately to participate into the Microtransat race in 2017. So far so good, except for the code. I took it from the internet, and it works like a charm, except for the fact that it uses motors for steering. I need it to use servo-motors.

Since the TinyGPS++ library forbids the use of the default <Servo.h> library, <PWMServo.h> will be used instead.

Here’s the code (there are actually two codes, this one defines the steering):

#include <Wire.h>
#include "compass.h"
#include <SoftwareSerial.h>
#include <SoftEasyTransfer.h>
#include <Average.h>
#include "LowPower.h"
#include <avr/interrupt.h>
#include <avr/power.h>
#include <avr/sleep.h>
#include <avr/io.h>

//Motor Control
int maxspeed;
//maxspeed - motgain * 30 > 0
//MOTGAIN 4
int motgain = 4;
int anglerange = 30;
int rightpin = 10;
int leftpin = 9;
int led = 13;
int overavg = 10;
int steerarray[10];
int throttlearray[10];

//GPS
static int r = 6371;
const float Pi = 3.14159;
int accuracy = 5;
unsigned long lasttime = 0;

SoftEasyTransfer rx;

SoftwareSerial link(7, 8);

struct RECEIVE_DATA_STRUCTURE{
  float latitude;
  float longitude;
};

RECEIVE_DATA_STRUCTURE gpsin;

void setup() {
  Serial.begin(115200);
  Wire.begin();
  pinMode(rightpin, OUTPUT);
  pinMode(leftpin, OUTPUT);
  pinMode(led, OUTPUT);
  compass_init(2);
  compass_debug = 1;
  compass_offset_calibration(3);
  link.begin(38400);
  rx.begin(details(gpsin), &link);
  digitalWrite(led, HIGH);
}

void loop() {
  Navigate(49.231623, -122.695656, 80);
  Navigate(49.231600, -122.695745, 80);
  Navigate(49.231545, -122.695796, 80);
  Navigate(49.231474, -122.695756, 80);
  Navigate(49.231450, -122.695659, 80);
  Navigate(49.231472, -122.695568, 80);
  Navigate(49.231535, -122.695523, 80);
  Navigate(49.231600, -122.695572, 80);
  //Control();
}

void Control() {
  while (pulseIn(5, HIGH, 100000) > 0){
    int throttle = pulseIn(4, HIGH, 20000);
    int steer = pulseIn(5, HIGH, 20000);
    if (throttle > 1000 && steer > 1000) {
      Override(throttle, steer);
    }
    delay(50);
  }
}

void Navigate(float lat2, float long2, int velocity) {
  maxspeed = velocity;
  while (GetDistanceInM(gpsin.latitude, gpsin.longitude, lat2, long2) > accuracy) {
    if (rx.receiveData()){
      lasttime = millis();
    }
    while (pulseIn(5, HIGH, 100000) > 0){
      int throttle = pulseIn(4, HIGH, 20000);
      int steer = pulseIn(5, HIGH, 20000);
      if (throttle > 1000 && steer > 1000) {
        Override(throttle, steer);
      }
      delay(50);
    }
    if ((millis() - lasttime) < 2000) {
      int bearing = GetBearing();
      int heading = GetHeading(gpsin.latitude, gpsin.longitude, lat2, long2);
      int leftmot = MotorSpeed(bearing, heading, 1);
      int rightmot = MotorSpeed(bearing, heading, 0);
      if (leftmot < 0) {
        leftmot = 0;
      }
      if (rightmot < 0) {
        rightmot = 0;
      }
      RunMotor(leftmot, 1);
      RunMotor(rightmot, 0);
      Serial.print(bearing);
      Serial.print("  ");
      Serial.print(heading);
      Serial.print("  ");
      Serial.print(GetDistanceInM(gpsin.latitude, gpsin.longitude, lat2, long2));
      Serial.print("  ");
      Serial.print(leftmot);
      Serial.print("  ");
      Serial.println(rightmot);
    }
    else {
      RunMotor(0, 1);
      RunMotor(0, 0);
      Serial.println("No GPS Signal");
    }
  }
  Serial.println("NavLoop Return");
  RunMotor(0, 1);
  RunMotor(0, 0);
}

void Override(int throttle, int steering) {
  int left;
  int right;
  if (throttle > 1600)
  {
    throttle = (int)(0.51 * (throttle - 1500));
    if (steering > 1500) {
      right = (int)(throttle - 0.51* (steering - 1500));
      if (right < 0)
      {
        right = 0;
      }
      left = throttle;
    }
    else {
      left = (int)(throttle + 0.51 * (steering - 1500));
      if (left < 0)
      {
        left = 0;
      }
      right = throttle;
    }
  }
  else {
    left = 0;
    right = 0;
  }
  RunMotor(right, 0);
  RunMotor(left, 1);
  Serial.print(left);
  Serial.print("  ");
  Serial.println(right);
}

void RunMotor(int val, int option){
  if (option == 0) {
    //Right Motor
    analogWrite(rightpin, val);
  }
  else {
    //Left Motor
    analogWrite(leftpin, val);
  }
}

int GetBearing() {
  compass_heading();
  float headingradians = bearing / (180 / M_PI);
  float declinationAngle = 0.289724656;
  headingradians += declinationAngle;
  bearing = (int)((headingradians * 180 / M_PI)+270) % 360;
  return bearing;
}

int MotorSpeed(int bearing, int heading, int option) {
  heading = heading % 360;
  bearing = bearing % 360;
  if (heading == bearing)
  {
    return (maxspeed);
  }
  int o1 = (int)heading - (int)bearing;
  int o2 = (int)bearing - (int)heading;
  int direction = ((360 - ((int)heading - (int)bearing)) % 360);
  if (direction < 180) {
    if (direction > anglerange)
    {
      //Right Motor
      if (option == 0) {
        return maxspeed;
      }
      //Left Motor
      else {
        return 0;
      }
    }
    else
    {
      //Right Motor
      if (option == 0) {
        return maxspeed;
      }
      //Left Motor
      else {
        return maxspeed - (direction * motgain);
      }
    }
  }
  else if (direction >= 180)
  {
    if ((360 - direction) > anglerange)
    {
      //Right Motor
      if (option == 0) {
        return 0;
      }
      //Left Motor
      else {
        return maxspeed;
      }
    }
    else
    {
      //Right Motor
      if (option == 0) {
        return maxspeed - ((360 - direction) * motgain);
      }
      //Left Motor
      else {
        return maxspeed;
      }
    }
  }
  return 0;
}

static float GetHeading(float lat1, float long1, float lat2, float long2)
{
  //Current Latitude, Current Longitude
  //Projected Latitude, Projected Longitude
  //To get reverse heading, add 180
  lat1 = ConvertToRadians(lat1);
  lat2 = ConvertToRadians(lat2);
  long1 = ConvertToRadians(long1);
  long2 = ConvertToRadians(long2);
  float dLon = long2 - long1;
  float y = sin(dLon) * cos(lat2);
  float x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(dLon);
  return ConvertToStaticDegrees(atan2(y, x));
}
static float GetDistanceInM(float lat1, float long1, float lat2, float long2)
{
  //Current Latitude, Current Longitude
  //Projected Latitude, Projected Longitude
  lat1 = ConvertToRadians(lat1);
  lat2 = ConvertToRadians(lat2);
  long1 = ConvertToRadians(long1);
  long2 = ConvertToRadians(long2);
  float dLat = lat2 - lat1;
  float dLon = long2 - long1;
  float a = haversin(dLat) + cos(lat1) * cos(lat2) * haversin(dLon);
  float c = 2 * atan2(sqrt(a), sqrt(1 - a));
  return r * c * 1000; // Distance in M
}
static float ConvertToDegrees(float rad)
{
  float degrees = rad / (Pi / 180);
  if (degrees <= -180) {
    return degrees + 360;
  }
  else {
    return degrees;
  }
}
static float ConvertToStaticDegrees(float rad)
{
  float degrees = rad / (Pi / 180);
  if (degrees <= 0) {
    return degrees + 360;
  }
  else {
    return degrees;
  }
}
static float ConvertToRadians(float angle)
{
  return (Pi / 180) * angle;
}
static float haversin(float input)
{
  return (sin(input / 2)) * (sin(input / 2));
}

Thanks a lot!

What you need to know is that PWM duty cycles in the range 0 to 100% ( representing motor off to motor full on) translate to pulsewidths in the range 1000-2000 microseconds

u_secs = 1000 + duty_cycle * 10

wg0z:
What you need to know is that PWM duty cycles in the range 0 to 100% ( representing motor off to motor full on) translate to pulsewidths in the range 1000-2000 microseconds

u_secs = 1000 + duty_cycle * 10

I see.
That means that if I have a servo defined as “myservo” for instance, writing

pwm.setPWM(myservo, 0, 2000);

would have the same effects as

myservo.write(180); with the regular <Servo.h> library

…right? Please correct me if I’m wrong.