HI guys, first of all thanks to this group as its helped massively in getting my project to where it is now.
BACKGROUND - I'm in the process for building an autonomous boat which travels to a set of predefined coordinates. My program is just about finished and working well. It comprises of 1 UNO which does the bulk of the work by using GPS and a compass to calculate a heading error. The PID loop then takes care of feeding a servo and angle.
However, I've realised that the the PID is outputting in the opposite direction to that it should. eg. when the rudder needs to go left its going right. The degree of rudder and everything else are perfect so I'm happy with the numbers but there is an obvious error I cant get my head around.
#include <TinyGPSPlus.h>
#include <SoftwareSerial.h>
#include <Wire.h>
#include <LSM303.h>
#include <PWMServo.h>
#include <PID_v1.h>
// SERVO PID CONTROL
double Setpoint, Input, Output;
double Kp = 1.2, Ki = 0.0, Kd = 0.0;
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, REVERSE);
#define PIN_OUTPUT 10
// VOLTAGE
#define ANALOG_IN_PIN A0
// Floats for ADC voltage & Input voltage
float adc_voltage = 0.0;
float in_voltage = 0.0;
// Floats for resistor values in divider (in ohms)
float R1 = 30000.0;
float R2 = 7500.0;
// Float for Reference Voltage
float ref_voltage = 4.20;
// Integer for ADC value
int adc_value = 0;
///AMPS
int analogPin = A1; // Current sensor output
const int averageValue = 500;
long int sensorValue = 0; // variable to store the sensor value read
float voltage = 0;
float current = 0;
// GPS
static const int RXPin = 6, TXPin = 7; //GPS TX & RX
SoftwareSerial ss(RXPin, TXPin); //GPS TX & RX
// Create objects
TinyGPSPlus gps;
LSM303 compass;
PWMServo myservo;
PWMServo ESC;
// GPS buadrate
static const uint32_t GPSBaud = 9600;
// ESC inputs - 180 = full power etc
int full_power = 101;
void setup()
{
Serial.begin(115200);
ss.begin(GPSBaud);
Wire.begin();
compass.init();
compass.enableDefault();
myservo.attach(SERVO_PIN_B); // myservo.attach(SERVO_PIN_B, 1000, 2000);
ESC.attach(SERVO_PIN_A); //myservo.attach(SERVO_PIN_A, 1000, 2000);
Setpoint = 0; // Desired bearing is 0° off bearing to base
myPID.SetMode(AUTOMATIC);
compass.m_min = (LSM303::vector<int16_t>){ -480, -465, -509 };
compass.m_max = (LSM303::vector<int16_t>){ +659, +703, +429 };
Serial.println(F("Time , Current location , Destination , Distance to desination , Course to destination , Heading , Heading error , Speed(kph) , GPS aquired course , Rudder input , System amps , System voltage "));
}
void loop() {
while (ss.available() > 0) {
gps.encode(ss.read());
compass.read();
}
if (gps.location.isUpdated()) {
// NAVIGATION FOMULAS
static const double Destination_lat = 55.294285, Destination_lng = -1.580930; /// 55.294285458482264, -1.5809302852304756
double distanceToDestination = TinyGPSPlus::distanceBetween(gps.location.lat(), gps.location.lng(), Destination_lat, Destination_lng);
double courseToDestination = TinyGPSPlus::courseTo(gps.location.lat(), gps.location.lng(), Destination_lat, Destination_lng);
double heading = compass.heading();
int heading_error_INT = round(heading - courseToDestination);
int heading_error = (heading_error_INT + 540) % 360 - 180; //int heading_error = (bearing-heading+540)%360 -180;
// Serial prints
Serial.print(gps.time.hour());
Serial.print(F(":"));
Serial.print(gps.time.minute());
Serial.print(F(":"));
Serial.print(gps.time.second());
Serial.print(" , ");
Serial.print(gps.location.lat(), 6);
Serial.print(",");
Serial.print(gps.location.lng(), 6);
Serial.print(",");
Serial.print(Destination_lat, 6);
Serial.print(",");
Serial.print(Destination_lng, 6);
Serial.print(",");
Serial.print(distanceToDestination / 1000);
Serial.print(",");
Serial.print(courseToDestination);
Serial.print(",");
Serial.print(heading);
Serial.print(",");
Serial.print(heading_error);
Serial.print(",");
Serial.print(gps.speed.kmph());
Serial.print(",");
Serial.print(gps.course.deg());
for (int i = 0; i < averageValue; i++) {
sensorValue += analogRead(analogPin);
}
sensorValue = sensorValue / averageValue;
voltage = sensorValue * 4.20 / 1024.0;
current = (voltage - 2.5) / 0.185;
adc_value = analogRead(ANALOG_IN_PIN);
adc_voltage = (adc_value * ref_voltage) / 1024.0;
in_voltage = adc_voltage / (R2 / (R1 + R2));
Serial.print(current);
Serial.print(",");
Serial.print(in_voltage, 2);
// PID RUDDER LOOP
if (heading_error >= -180 && heading_error <= 0) {
Input = heading_error;
myPID.SetOutputLimits(40, 90);
Serial.print("LEFT");
}
if (heading_error >= 0 && heading_error <= 180) {
Input = heading_error;
myPID.SetOutputLimits(90, 140);
Serial.print("RIGHT");
}
myPID.Compute();
myservo.write(Output);
Serial.print(",");
Serial.println(Output);
}
}
