Kawai GFP-3 Teensy - Test code

/**
 * Based on code from CyberGene's DIY Hybrid Piano Controller
 * For Teensy 3.6 or 4.1
 * Author: Evgeni Kumanov (CyberGene)
 * 2019, 2020, 2022
 */

#define LEFT_PEDAL A1   // Green wire
#define MIDDLE_PEDAL A0 // Red wire
#define RIGHT_PEDAL A2   // Blue wire

#define LED 13

// where it starts generating values on pedal down
#define HIGH_PEDAL_LIMIT 192

// where it reaches value 127
#define LOW_PEDAL_LIMIT 85

#define LEFT_PEDAL_MIDI_CC 67
#define MIDDLE_PEDAL_MIDI_CC 66
#define RIGHT_PEDAL_MIDI_CC 64

byte lastPedalValue[3];
byte lastPedalVoltage[3];
byte ccValue[256]; // voltage is read as an 8-bit value, map it to a MIDI value
byte currentPedalVoltage[3];
byte currentPedalValue[3];
byte voltage;
uint8_t pedalInputs[3];
byte pedal_midi_cc[3];

int mockPedalVoltage = 0;
int currentPedal = 0;

void setup() {
  double pedalSegment = 127.0 / (HIGH_PEDAL_LIMIT - LOW_PEDAL_LIMIT);

  // create the mapping between voltage and MIDI
  for (int i = 0; i < 256; i++) {
    if (i < LOW_PEDAL_LIMIT) {
      ccValue[i] = 127;
    } else if (i > HIGH_PEDAL_LIMIT) {
      ccValue[i] = 0;
    } else {
      byte value = (byte) (127 - (pedalSegment * (i - LOW_PEDAL_LIMIT)) + 0.5);
      ccValue[i] = value < 0 ? 0 : value;
    }
  }

  for (int i=0; i < 3; i++) {
    lastPedalVoltage[i] = 0;
    lastPedalValue[i] = 0;
    currentPedalVoltage[i] = 0;
    currentPedalValue[i] = 0;
  }
  pedalInputs[0] = LEFT_PEDAL;
  pedalInputs[1] = MIDDLE_PEDAL;
  pedalInputs[2] = RIGHT_PEDAL;

  pedal_midi_cc[0] = LEFT_PEDAL_MIDI_CC;
  pedal_midi_cc[1] = MIDDLE_PEDAL_MIDI_CC;
  pedal_midi_cc[2] = RIGHT_PEDAL_MIDI_CC;

  digitalWrite(LED, HIGH);
  delay(100);
  digitalWrite(LED, LOW);
  delay(100);
  digitalWrite(LED, HIGH);
  delay(100);
  digitalWrite(LED, LOW);
  delay(100);
  digitalWrite(LED, HIGH);
  delay(100);
  digitalWrite(LED, LOW);
}

void loop() {
  for (int i=0; i < 512; i++) {
    if (i <= 255) {
      mockPedalVoltage = 255 - (i % 256);
    } else {
      mockPedalVoltage = i;
    }
    checkPedals_mock(mockPedalVoltage, currentPedal);
    usbMIDI.send_now();
    delayMicroseconds(3000);
  }
  currentPedal = (currentPedal + 1) % 3;
}

void checkPedals_mock(int mockValue, int currentPedal) {
  voltage = mockValue;
  if (voltage != lastPedalVoltage[currentPedal]) {
    lastPedalVoltage[currentPedal] = voltage;
    currentPedalValue[currentPedal] = ccValue[voltage];
    if (currentPedalValue[currentPedal] != lastPedalValue[currentPedal]) {
      lastPedalValue[currentPedal] = currentPedalValue[currentPedal];
      usbMIDI.sendControlChange(pedal_midi_cc[currentPedal], currentPedalValue[currentPedal], 1);
      Serial.println(pedal_midi_cc[currentPedal]);
      Serial.println(currentPedalValue[currentPedal]);
    }
  }
}