Kawai GFP-3 Teensy

/**
 * 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];

void setup() {
  // Set analog read resolution to 8 bits, down from the default of 10 bits for most boards.
  analogReadResolution(8);

  pinMode(LEFT_PEDAL, INPUT);
  pinMode(MIDDLE_PEDAL, INPUT);
  pinMode(RIGHT_PEDAL, INPUT);
  pinMode(LED, OUTPUT);

  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() {
  checkPedals();
  usbMIDI.send_now();
}

void checkPedals() {
  for (int i=0; i < 3; i++) {
    voltage = analogRead(pedalInputs[i]);
    if (voltage != lastPedalVoltage[i]) {
      lastPedalVoltage[i] = voltage;
      currentPedalValue[i] = ccValue[voltage];
      if (currentPedalValue[i] != lastPedalValue[i]) {
        lastPedalValue[i] = currentPedalValue[i];
        usbMIDI.sendControlChange(pedal_midi_cc[i], currentPedalValue[i], 1);
        Serial.println(pedal_midi_cc[i]);
        Serial.println(currentPedalValue[i]);
      }
    }
  }
}