void DriveServosRC()
{
// ********************************************************************
// If the stick moves UP along Y...Ch2 1500-2000
//*********************************************************************
if (Ch2 > Ch2_Upper_Trigger) {
// Set the range of motion of the left wheel, Ch2_Max_Value is the maximum speed desired
// map(value, fromLow, fromHigh, toLow, toHigh)
Ch2 = map(Ch2, Ch2_Upper_Trigger, 1900, Ch2_Upper_Trigger, Ch2_Max_Value);
//************
//If the stick stays center on X
//************
if (Ch1 < Ch1_Upper_Trigger && Ch1 > Ch1_Lower_Trigger) {
L_DCMotor.writeMicroseconds(Ch2); // sets the servo position
R_DCMotor.writeMicroseconds(Ch2); // Set so that stearing would be horz stick
}
//************
//If the stick moves left on X
//************
if (Ch1 > Ch1_Upper_Trigger) {
L_DCMotor.writeMicroseconds(Ch2); // Set so that stearing would be horz stick
CalcHold = Ch1;
CalcHold = map(CalcHold, Ch1_Upper_Trigger, 1900, -50, -300);// map(value, fromLow, fromHigh, toLow, toHigh)
R_DCMotor.writeMicroseconds(Ch2 + CalcHold); // sets the servo position
}
//************
//If the stick moves Right on X
//************
if (Ch1 < Ch1_Lower_Trigger) {
L_DCMotor.writeMicroseconds(Ch2); // Set so that stearing would be horz stick
CalcHold = Ch1;
CalcHold = map(CalcHold, Ch1_Lower_Trigger, 1000, 50, 300);// map(value, fromLow, fromHigh, toLow, toHigh)
R_DCMotor.writeMicroseconds(Ch2 + CalcHold); // sets the servo position
}
}
// ********************************************************************
// If the stick moves DOWN along Y...Ch2 1500-1000
//*********************************************************************
else if (Ch2 < Ch2_Lower_Trigger) {
// Set the range of motion of the left wheel, Ch2_Min_Value is the minimum speed desired
// map(value, fromLow, fromHigh, toLow, toHigh)
Ch2 = map(Ch2, Ch2_Lower_Trigger, 1000, Ch2_Lower_Trigger, Ch2_Min_Value);
//************
//If the stick stays center on X
//************
if (Ch1 < Ch1_Upper_Trigger && Ch1 > Ch1_Lower_Trigger) {
L_DCMotor.writeMicroseconds(Ch2); // sets the servo position
R_DCMotor.writeMicroseconds(Ch2); // Set so that stearing would be horz stick
}
//************
//If the stick moves left on X
//************
if (Ch1 > Ch1_Upper_Trigger) {
L_DCMotor.writeMicroseconds(Ch2); // Set so that stearing would be horz stick
CalcHold = Ch1;
CalcHold = map(CalcHold, Ch1_Upper_Trigger, 1900, -50, -300);// map(value, fromLow, fromHigh, toLow, toHigh)
R_DCMotor.writeMicroseconds(Ch2 + CalcHold); // sets the servo position
}
//************
//If the stick moves Right on X
//************
if (Ch1 < Ch1_Lower_Trigger) {
L_DCMotor.writeMicroseconds(Ch2); // Set so that stearing would be horz stick
CalcHold = Ch1;
CalcHold = map(CalcHold, Ch1_Lower_Trigger, 1000, 50, 300);// map(value, fromLow, fromHigh, toLow, toHigh)
R_DCMotor.writeMicroseconds(Ch2 + CalcHold); // sets the servo position
}
}
else {
L_DCMotor.writeMicroseconds(1500); // sets the servo position to center to stop
R_DCMotor.writeMicroseconds(1500); // sets the servo position to center to stop
}
}