नीचे रसेल मैकमोहन उत्तर द्वारा वर्णित एल्गोरिथ्म कार्यान्वयन का उदाहरण दिया गया है:
http://www.youtube.com/watch?v=sGpgWDIVsoE
//Atmega328p based Arduino code (should work withouth modifications with Atmega168/88), tested on RBBB Arduino clone by Modern Device:
const byte joysticYA = A0; //Analog Jostick Y axis
const byte joysticXA = A1; //Analog Jostick X axis
const byte controllerFA = 10; //PWM FORWARD PIN for OSMC Controller A (left motor)
const byte controllerRA = 9; //PWM REVERSE PIN for OSMC Controller A (left motor)
const byte controllerFB = 6; //PWM FORWARD PIN for OSMC Controller B (right motor)
const byte controllerRB = 5; //PWM REVERSE PIN for OSMC Controller B (right motor)
const byte disablePin = 2; //OSMC disable, pull LOW to enable motor controller
int analogTmp = 0; //temporary variable to store
int throttle, direction = 0; //throttle (Y axis) and direction (X axis)
int leftMotor,leftMotorScaled = 0; //left Motor helper variables
float leftMotorScale = 0;
int rightMotor,rightMotorScaled = 0; //right Motor helper variables
float rightMotorScale = 0;
float maxMotorScale = 0; //holds the mixed output scaling factor
int deadZone = 10; //jostick dead zone
void setup() {
//initialization of pins
Serial.begin(19200);
pinMode(controllerFA, OUTPUT);
pinMode(controllerRA, OUTPUT);
pinMode(controllerFB, OUTPUT);
pinMode(controllerRB, OUTPUT);
pinMode(disablePin, OUTPUT);
digitalWrite(disablePin, LOW);
}
void loop() {
//aquire the analog input for Y and rescale the 0..1023 range to -255..255 range
analogTmp = analogRead(joysticYA);
throttle = (512-analogTmp)/2;
delayMicroseconds(100);
//...and the same for X axis
analogTmp = analogRead(joysticXA);
direction = -(512-analogTmp)/2;
//mix throttle and direction
leftMotor = throttle+direction;
rightMotor = throttle-direction;
//print the initial mix results
Serial.print("LIN:"); Serial.print( leftMotor, DEC);
Serial.print(", RIN:"); Serial.print( rightMotor, DEC);
//calculate the scale of the results in comparision base 8 bit PWM resolution
leftMotorScale = leftMotor/255.0;
leftMotorScale = abs(leftMotorScale);
rightMotorScale = rightMotor/255.0;
rightMotorScale = abs(rightMotorScale);
Serial.print("| LSCALE:"); Serial.print( leftMotorScale,2);
Serial.print(", RSCALE:"); Serial.print( rightMotorScale,2);
//choose the max scale value if it is above 1
maxMotorScale = max(leftMotorScale,rightMotorScale);
maxMotorScale = max(1,maxMotorScale);
//and apply it to the mixed values
leftMotorScaled = constrain(leftMotor/maxMotorScale,-255,255);
rightMotorScaled = constrain(rightMotor/maxMotorScale,-255,255);
Serial.print("| LOUT:"); Serial.print( leftMotorScaled);
Serial.print(", ROUT:"); Serial.print( rightMotorScaled);
Serial.print(" |");
//apply the results to appropriate uC PWM outputs for the LEFT motor:
if(abs(leftMotorScaled)>deadZone)
{
if (leftMotorScaled > 0)
{
Serial.print("F");
Serial.print(abs(leftMotorScaled),DEC);
analogWrite(controllerRA,0);
analogWrite(controllerFA,abs(leftMotorScaled));
}
else
{
Serial.print("R");
Serial.print(abs(leftMotorScaled),DEC);
analogWrite(controllerFA,0);
analogWrite(controllerRA,abs(leftMotorScaled));
}
}
else
{
Serial.print("IDLE");
analogWrite(controllerFA,0);
analogWrite(controllerRA,0);
}
//apply the results to appropriate uC PWM outputs for the RIGHT motor:
if(abs(rightMotorScaled)>deadZone)
{
if (rightMotorScaled > 0)
{
Serial.print("F");
Serial.print(abs(rightMotorScaled),DEC);
analogWrite(controllerRB,0);
analogWrite(controllerFB,abs(rightMotorScaled));
}
else
{
Serial.print("R");
Serial.print(abs(rightMotorScaled),DEC);
analogWrite(controllerFB,0);
analogWrite(controllerRB,abs(rightMotorScaled));
}
}
else
{
Serial.print("IDLE");
analogWrite(controllerFB,0);
analogWrite(controllerRB,0);
}
Serial.println("");
//To do: throttle change limiting, to avoid radical changes of direction for large DC motors
delay(10);
}
