Hi all! Im using the following code for servo position according to compass module north position.
How can i set the servo to current position any time i push a button? For example, if the compass is to 90 deg. east, then i push a button and the servo set as default position the 90 deg.
Could you help me in code changes?
Thank you!
#include <Wire.h>
#include <Servo.h>
// Pins
#define SDA 4 // analog
#define SCL 5 // analog
#define servoPointerPin 10
// Controllers
Servo servoPointer;
void setup()
{
Serial.begin( 4800 );
delay( 500 );
setupCompass();
setupServo();
Serial.println( "Setup Complete" );
}
void setupCompass()
{
Wire.begin();
}
void setupServo()
{
servoPointer.attach( servoPointerPin );
positionServo( &servoPointer, 50 );
}
void positionServo( Servo* pServo, int position )
{
int currentPosition = pServo->read();
if( currentPosition != position )
pServo->write( position );
}
int lastHeading = -1;
bool getCurrentHeading( int& reading )
{
int compassAddress = 0x42 >> 1;
Wire.beginTransmission( compassAddress );
Wire.send( 'A' );
Wire.endTransmission();
delay(10);
Wire.requestFrom( compassAddress, 2 );
while( Wire.available() < 2 )
delay(10);
int newReading = Wire.receive();
newReading = newReading << 8;
newReading += Wire.receive();
newReading /= 10;
bool result = ( lastHeading == -1 ) ? true : ( abs( newReading - lastHeading ) > 2 );
reading = newReading;
if( result )
lastHeading = reading;
return result;
}
void loop()
{
int heading = 0;
if( getCurrentHeading(heading) )
PositionNeedle( heading );
}
void PositionNeedle( const int currentHeading )
{
int bearing = -currentHeading;
if( bearing < 0 )
bearing += 360;
Serial.print( "Bearing=" );
Serial.print( bearing, DEC );
float minSweep = 80.0f;
float sweepRange = 145.0f - minSweep;
int servoPosition = (int)(sweepRange - (bearing / 360.0f * sweepRange) + minSweep);
Serial.print( " Servo=" );
Serial.println( servoPosition, DEC );
positionServo( &servoPointer, servoPosition );
}