#include <Servo.h>
Servo servoL;
Servo servoR;
void setup()
{
// Start all pins low / disable pull-ups.
digitalWrite(6, LOW);
digitalWrite(7, LOW);
digitalWrite(8, LOW);
digitalWrite(9, LOW);
// Configure D6 and D7 as outputs.
pinMode(6, OUTPUT);
pinMode(7, OUTPUT);
// Configure D8 and D9 as inputs.
pinMode(8, INPUT);
pinMode(9, INPUT);
// Attach servos.
servoL.attach(6);
servoR.attach(7);
// Set servos to neutral to start.
servoL.writeMicroseconds(3000);
servoR.writeMicroseconds(3000);
}
void loop()
{
signed int stickX;
signed int stickY;
signed int driveL;
signed int driveR;
// Read stick positions. [1000-2000us]
stickX = pulseIn(8, HIGH);
stickY = pulseIn(9, HIGH);
// Stick mixing function to generate drive commands.
driveL = stickY + stickX - 3000;
driveR = stickY - stickX + 3000;
// Limit drive commands to [1000-2000us].
if(driveL > 2000) { driveL = 2000; }
if(driveL < 1000) { driveL = 1000; }
if(driveR > 2000) { driveR = 2000; }
if(driveR < 1000) { driveR = 1000; }
// Send commands to servos.
servoL.writeMicroseconds(driveL);
servoR.writeMicroseconds(driveR);
}