//*******2.2kOhm********1kOhm
//DPH ---/\/\/\-- [D+]--/\/\/\-- DPL

//*******2.2kOhm********1kOhm
//DMH ---/\/\/\-- [D-]--/\/\/\-- DML



const int DPH = 2;
const int DPL = 3;
const int DMH = 4;
const int DML = 5;

bool handshakeDone = false;
void handshake()
{
  pinMode(DPH, OUTPUT);
  pinMode(DPL, OUTPUT);
  digitalWrite(DPH, HIGH);
  digitalWrite(DPL, LOW);

  delay(1500);

  pinMode(DMH, OUTPUT);
  pinMode(DML, OUTPUT);
  digitalWrite(DMH, LOW);
  digitalWrite(DML, LOW);

  handshakeDone = true;
}

/**
 * Increases the current continuous mode voltage by 0.2v.
 */
void increaseVoltage()
{
  digitalWrite(DPH, LOW);
  digitalWrite(DPL, HIGH);

  delay(1);

  setVoltage(69); // return back to continuous mode
}

/**
 * Decreases the current continuous mode voltage by 0.2v.
 */
void decreaseVoltage()
{
  digitalWrite(DMH, HIGH);
  digitalWrite(DML, LOW);

  delay(1);

  setVoltage(69); // return back to continuous mode
}

void setVoltage(unsigned int voltage)
{
  if (! handshakeDone)
    handshake();

  switch (voltage)
  {
    /*case 20: // NOTE supported only on class B
      digitalWrite(DPH, LOW);
      digitalWrite(DPL, HIGH);
      digitalWrite(DMH, LOW);
      digitalWrite(DML, HIGH);
    break;*/
    case 69: // NOTE used to change to continuous mode
      digitalWrite(DPH, HIGH);
      digitalWrite(DPL, LOW);
      digitalWrite(DMH, LOW);
      digitalWrite(DML, HIGH);
    break;
    case 12:
      digitalWrite(DPH, HIGH);
      digitalWrite(DPL, LOW);
      digitalWrite(DMH, HIGH);
      digitalWrite(DML, LOW);
    break;
    case 9:
      digitalWrite(DPH, LOW);
      digitalWrite(DPL, HIGH);
      digitalWrite(DMH, HIGH);
      digitalWrite(DML, LOW);
    break;
    case 5:
    default:
      digitalWrite(DPH, HIGH);
      digitalWrite(DPL, LOW);
      digitalWrite(DMH, LOW);
      digitalWrite(DML, LOW);
  }
}

void setup()
{
  // do nothing
}

void loop()
{
  /*setVoltage(20);
  delay(5000);*/
  setVoltage(12);
  delay(5000);
  setVoltage(9);
  delay(5000);
  setVoltage(5);
  delay(5000);
  setVoltage(69); // change to continuous mode
  for (int i = 0; i < 25; i++)
  {
    delay(1000);
    increaseVoltage();
  }
  for (int i = 0; i < 50; i++)
  {
    delay(1000);
    decreaseVoltage();
  }
  delay(5000);
}