rc winch controller change 2 position switch signal to toggle with arduino

winch_controller.ino

#include <Servo.h>

Servo myservo;

int ch1;

const int AVERAGE_COUNT = 3;
const int INPUT_CHANNEL = 12;
const int OUTPUT_CHANNEL = 10;

bool reads[AVERAGE_COUNT];
int readIndex = 0;

bool debug = false;
bool previousState = false;
bool currentState = false;
bool initialized = false;
bool rest = true;
bool winch = false;
int trueState;
int falseState;

int pos = 0;
int i = 0;

void setup() {
  // put your setup code here, to run once:

  if (debug) {
    Serial.begin(9600);
  }

  // Set input pin
  pinMode(INPUT_CHANNEL, INPUT);

  //output servo pin
  myservo.attach(OUTPUT_CHANNEL);

  // default to doing nothing
  myservo.write(90);

  // wait 1 second before doing anything
  delay(1000);
}

void loop() {
  // put your main code here, to run repeatedly:

  // Read the pulse width
  ch1 = pulseIn(INPUT_CHANNEL, HIGH, 25000);

  if (debug) {
    Serial.println(ch1);
  }

  // channel value changes from ~1045 to ~1965 (for me)
  if (ch1 < 950 || ch1 > 2050 || (ch1 > 1150 && ch1 < 1850 )) {
    // assume this is error in reading signal state and do nothing
    currentState = previousState;
  } else if (ch1 > 1850) {
    currentState = true;
  } else {
    currentState = false;
  }

  // use AVERAGE_COUNT latest reads and use the state which has more occurences
  // makes a slight delay on changing state, but prevents single interference
  // from changing direction
  reads[(readIndex++)] = currentState;
  readIndex = readIndex % AVERAGE_COUNT;

  trueState = 0;
  falseState = 0;

  // initialize array with current value
  if (!initialized) {
    for (i = 0; i < AVERAGE_COUNT; i++) {
      reads[i] = currentState;
    }
  }

  for (i = 0; i < AVERAGE_COUNT; i++) {
    if (reads[i]) {
      trueState++;
    } else {
      falseState++;
    }
  }

  if (trueState > falseState) {
    currentState = true;
  } else {
    currentState = false;
  }

  // do the toggle if needed
  if (initialized && currentState != previousState) {
    if (rest == false) {
      rest = true;
    } else {
      rest = false;

      if (winch == true) {
        winch = false;
      } else {
        winch = true;
      }
    }
  }

  // change output or keep as same
  if (rest) {
    // output zero signal

    if (myservo.read() != 90) {
      myservo.write(90);
    }

    delay(100);
  } else if (winch) {
    // output +100% signal

    // soft start
    if (myservo.read() != 180) {
      for (pos = 90; pos <= 180; pos += 1) {
        myservo.write(pos);
        delay(5);
      }
    }
  } else {
    // output -100% signal
    // soft start
    if (myservo.read() != 0) {
      for (pos = 90; pos >= 0; pos -= 1) {
        myservo.write(pos);
        delay(5);
      }
    }
  }

  initialized = true;
  previousState = currentState;
}