Kontiki autopilot adding servo

Projects Programming
1

hi i am new to coding i have this code working i am trying to add a servo to take the Error with when the button is pressed it goes to 0 i would like to center the servo and have it move about 25 degrees each way from center depending on the error from the compass i have been trying but i just cant get servo to work
thanks

#include <LSM303.h>
#include <Wire.h>
#include <Servo.h> 
 
#define SERVO  9
#define CENTER 100
#define GOAL   0

//  int angle = getDegrees();
int Error;
const int buttonPin = 2;                                         // Used to start comparations
int storedHeading;                                               // Desired course
int deltaHeading;                                                // +/- Difference (Course +/- Desired Course)
boolean buttonState;                                             // Initiates comparations
LSM303 compass;                                                  // Output from LSM303
Servo    myservo;
void setup ()
{
  Serial.begin(9600);         // Initiates debuging serial COM
  myservo.attach(SERVO);
  Wire.begin();                                                  // Initiates I2C comunication
  compass.init();                                                // Initiates LSM303 
  compass.enableDefault();                                       // Enable dafault parameters on LSM303
  compass.m_min = (LSM303::vector<int16_t>){-361, -424, -268};   // Last parameter calibration readings made AUG-2014
  compass.m_max = (LSM303::vector<int16_t>){+371, +287, +378};   // Last parameter calibration readings made AUG-2014
}
void loop()
{ 
  compass.read();
  int heading = compass.heading();
  Serial.print("Heading: ");
  Serial.println(heading);
  int buttonState = digitalRead(buttonPin);
  if (buttonState == HIGH)
  {
    storedHeading = heading;
    Serial.print("Stored Heading: ");
    Serial.println(storedHeading);
  }
  int myHeading = storedHeading + 360;
  deltaHeading = heading - myHeading; 
  if (deltaHeading < 180) deltaHeading += 360;
  if (deltaHeading < 180) deltaHeading += 360;
  if (deltaHeading > 180) deltaHeading -= 360;
  Serial.print("Error: ");
  Serial.println(deltaHeading);
  delay (1000);
  }
2

Something like:
myservo.write (90 + constrain (deltaHeading, -20, +20)) ;
Where 90 is the typical midpoint for a servo, constrain limits the value to +/-20.
Perhaps deltaHeading needs to have its sign flipped depending on sign convention used.

You can streamline the angle delta stuff using modulo as
deltaHeading = (heading - storedHeading + 360 + 180) % 360 - 180 ;

3

thanks mark
that worked thanks
i was not sure were to put the other line of code that u streamline the angle delta stuff using modulo as
deltaHeading = (heading - storedHeading + 360 + 180) % 360 - 180 ;

#include <LSM303.h>
#include <Wire.h>
#include <Servo.h> 
 
#define SERVO  9
#define CENTER 100
#define GOAL   0

//  int angle = getDegrees();
int Error;
const int buttonPin = 2;                                         // Used to start comparations
int storedHeading;                                               // Desired course
int deltaHeading;                                                // +/- Difference (Course +/- Desired Course)
boolean buttonState;                                             // Initiates comparations
LSM303 compass;                                                  // Output from LSM303
Servo    myservo;
void setup ()
{
  Serial.begin(9600);         // Initiates debuging serial COM
  myservo.attach(SERVO);
  Wire.begin();                                                  // Initiates I2C comunication
  compass.init();                                                // Initiates LSM303 
  compass.enableDefault();                                       // Enable dafault parameters on LSM303
  compass.m_min = (LSM303::vector<int16_t>){-361, -424, -268};   // Last parameter calibration readings made AUG-2014
  compass.m_max = (LSM303::vector<int16_t>){+371, +287, +378};   // Last parameter calibration readings made AUG-2014
pinMode(3, INPUT_PULLUP);
}
void loop()
{ 
  compass.read();
  int heading = compass.heading();
  Serial.print("Heading: ");
  Serial.println(heading);
  int buttonState = digitalRead(buttonPin);
  if (buttonState == HIGH)
  {
    storedHeading = heading;
    Serial.print("Stored Heading: ");
    Serial.println(storedHeading);
  }
  int myHeading = storedHeading + 360;
  deltaHeading = heading - myHeading; 
  if (deltaHeading < 180) deltaHeading += 360;
  if (deltaHeading < 180) deltaHeading += 360;
  if (deltaHeading > 180) deltaHeading -= 360;
  Serial.print("Error: ");
  Serial.println(deltaHeading);
  
  myservo.write (90 + constrain
  (deltaHeading, -40,+40)) ;
   
(deltaHeading =(heading - storedHeading + 360 +180 %360 - 180)) ;
//delay (1000);
}
4

It replaces the lines calculating myHeading and deltaHeading

5

Thanks mark
All works perfect

6

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.