Code: void ShaftTankDrive() { // This flips the motors around so they go in the right direction bMotorFlippedMode[port2] = 0; bMotorFlippedMode[port3] = 1; bMotorFlippedMode[port4] = 0; bMotorFlippedMode[port5] = 1;
// Now lets define the temporary variables int LeftSide; int RightSide;
int ChangeVar; //int SensorAverage;
// Amount to speed up/slow down by ChangeVar = (Sensors.in1 - Sensors.in2);
// Are we there yet? >:( //SensorAverage = (Sensors.in1 + Sensors.in2) / 2;
LeftSide = Controller.Ch3 - ChangeVar; LeftSide = Math_Clamp(LeftSide,-127,127);
RightSide = Controller.Ch2 + ChangeVar; RightSide = Math_Clamp(RightSide,-127,127);
// This sets motor ports 1 and 2 to Controller.Ch2 value, 3 and 4 to Controller.Ch3 value. Motors.mtr2 = Math_Clamp(RightSide, -127, 127); Motors.mtr3 = Math_Clamp(RightSide, -127, 127); Motors.mtr4 = Math_Clamp(LeftSide, -127, 127); Motors.mtr5 = Math_Clamp(LeftSide, -127, 127); }
|