void Ptz::writePayload(uint8_t *payload) {
int panDistance = translateDistance(payload[0]);
int tiltDistance = translateDistance(payload[1]);
writePair(panDistance, _leftPin, _rightPin);
writePair(tiltDistance, _upPin, _downPin);
}
void CenterPosition1() {
turnUltra(0, 90);
moveDistanceRamp(50, 12);
binaryTillSenseHeading(120, 270, 30, frontUS);
PlaySound(soundLowBuzzShort);
translateDistance(200, 270, 7);
writeDebugStream("DONE WITH FIRST");
turnUltra(0,0);
pause(0.2);
irTillSensePeak(30);
playSound(soundLowBuzz);
turnWithGyro(30,0);
}
void CenterToKickstand(int position) {
translateDistance(100,270,10);
pause(0.2);
moveDistance(-100, 30);
pause(0.2);
turnWithGyro(100, 90);
turnUltra(0,0);
pause(0.2);
repeatedTillSense(50, 0, false, 90, rearUS);
pause(0.2);
if(position == 2)
tillSense(200, 270, false, 28, rearUS);
else if(position == 1)
tillSense(200, 270, false, 27, rearUS);
else if(position == 3);
tillSense(200, 270, false, 25, rearUS);
pause(0.2);
deployKnocker();
moveDistance(30, 10);
moveDistance(100, 30);
}
