// PPM Encoder Analog Add-ons for Devo7e
// For use with Arduino Pro Mini 328P
// J. Diehl 3/09/2015
// Based on a sketch by Ian Johnston 29/04/2010
//  modified by Epyon and Cereal_Killer
// Version 2.5 
// Low cost way to add extra POTs, switches or mode switch to any radio that accepts PPM input
//  using an arduino pro mini
// Visit DeviationTX.com for info. 
// POT's on A1 and A2, optional 6-position rotary switch on A3, optional switches on D6-D9
//  Use On-Off-On switches on D6-D7, and D8-D9 for 3-way effect
// Optional 6-switch/6-LED Flight Mode matrix uses D2-D4, D10-D12
// PPM output on D13
//  PPM inout to tip of 3.5 mono jack, this is the black wire on your Devo DSC port
//
//  Sketch uses:
//  A1-3 Analog in for Pots 1, 2 and 3
//  D6-9 Digital in for up to 2x3-way
//  D5 Digital in for additional 2-way switch (optional)
//  D2-4 Digital out for 6-LED Flight Mode charlieplex
//  D10-12 Digital in for 6-switch Flight Mode charliplex
//  D13 - PPM Output with LED

int potPin_A1= A1;        //Declare potPin_A1 to be analog pin A1
int potPin_A2= A2;        //Declare potPin_A2 to be analog pin A2
int potPin_A3= A3;        //Declare potPin_A3 to be analog pin A3
int AI_Raw_A1;            // Analog In raw var - 0->1023
int AI_Raw_A2;            // Analog In raw var - 0->1023
int AI_Raw_A3;            // Analog In raw var - 0->1023


byte swPin_D1= 6;         //Declare swPin_D1 to be Digital pin 6
byte swPin_D2= 7;         //Declare swPin_D2 to be Digital pin 7
byte swPin_D3= 8;         //Declare swPin_D3 to be Digital pin 8
byte swPin_D4= 9;         //Declare swPin_D4 to be Digital pin 9
byte swPin_D5= 5;         //Declare swPin_D5 to be Digital pin 5

int outPinPPM = 13;       // Declare ppmPin to be arduino pin 13

int runspeed = 150;       // speed of Cylong effect, smaller = faster
int Fixed_uS = 300;       // PPM frame fixed LOW phase
int pulseMin = 750;       // pulse minimum width minus start in uS
int pulseMax = 1700;      // pulse maximum width in uS
int pulseRng = pulseMax - pulseMin;
int pulseMid = pulseMin + (pulseRng / 2);
float adjustRng = pulseRng / 1023.0; // adjust range from 0-1023 for pots

int A1_uS = pulseMin;     // Analog 1 uS var
int A2_uS = pulseMin;     // Analog 2 uS var
int A3_uS = pulseMin;     // Analog 3 uS var
int sw1_uS = pulseMin;    // Switch 1 combines result from swPin_D1 and D2
int sw2_uS = pulseMin;    // Switch 2 combines result from swPin_D3 and D4
int sw3_uS = pulseMin;    // Switch 3 combines result from swPin_D5 and D6
int sw4_uS = pulseMin;    // Switch 4
int M1_uS = pulseMin;     // Switch Matrix set to lowest
/*
// analog rotary switch input definitions
// only needed if sw3 is used for mode switching
int fm1 = 740;            // slightly below actual read from position 1
int fm2 = 1200;           // slightly below actual read from position 2
int fm3 = 1350;           // slightly below actual read from position 3
int fm4 = 1450;           // slightly below actual read from position 4
int fm5 = 1500;           // slightly below actual read from position 5
int fm6 = 1685;           // slightly below actual read from position 6
*/

// led charlieplex definitions
int lpins[3];

/*
 led1 = {0 ,1}; // led1 is indicated by the current flow from 3 to 4
 led2 = {1 ,2}; // led2 is indicated by the current flow from 2 to 3
 led3 = {1 ,0}; // led3 is indicated by the current flow from 2 to 4
 led4 = {2 ,1}; // led4 is indicated by the current flow from 4 to 3
 led5 = {0 ,2}; // led5 is indicated by the current flow from 3 to 2
 led6 = {2 ,0}; // led6 is indicated by the current flow from 4 to 2
*/
const int ledPins[6][2] ={ // This stores the led pins order 
  { 0 , 1 } ,
  { 1 , 2 } ,
  { 1 , 0 } ,
  { 2 , 1 } ,
  { 0 , 2 } ,
  { 2 , 0 } 
  };

//switch charliplex definitions

int sPins[3];
  
int swState;            // passes keypress value to LEDs
int lastswState;        // previous keypress value
int swMode;             // catches the keypress value
int sPinA;              // state of Pin A
int sPinB;              // state of Pin B
int sPinC;              // state of Pin C

/*
 * For LEDs, use D2 for A (Red), D3 for B (Yellow) and D3 for C (White)
 * For switches, use D10 for A (Red), D11 for B (Yellow) and D12 for C (White)
 */
 const int swPins[6][2]={ // This stores the switch pins order
  { 2 , 1 } ,
  { 1 , 0 } ,
  { 1 , 2 } ,
  { 0 , 1 } ,
  { 2 , 0 } ,
  { 0 , 2 }
  };


ISR(TIMER1_COMPA_vect) {
    ppmoutput(); // Jump to ppmoutput subroutine
}


void setup() {
  pinMode(potPin_A1, INPUT);  //set potPin_A1 to be an input
  pinMode(potPin_A2, INPUT);  //set potPin_A2 to be an input
  pinMode(potPin_A3, INPUT);  //set potPin_A3 to be an input
  pinMode(swPin_D1, INPUT_PULLUP);  //set swPin_D1 to be an input
  pinMode(swPin_D2, INPUT_PULLUP);  //set swPin_D2 to be an input
  pinMode(swPin_D3, INPUT_PULLUP);  //set swPin_D3 to be an input
  pinMode(swPin_D4, INPUT_PULLUP);  //set swPin_D4 to be an input
  pinMode(swPin_D5, INPUT_PULLUP);  //set swPin_D5 to be an input

  pinMode(outPinPPM, OUTPUT); //set ppmPin to be an OUTPUT
  digitalWrite(outPinPPM, HIGH); // turn on LED

     // Setup timer
  TCCR1A = B00110001; // Compare register B used in mode '3'
  TCCR1B = B00010010; // WGM13 and CS11 set to 1
  TCCR1C = B00000000; // All set to 0
  TIMSK1 = B00000010; // Interrupt on compare B
  TIFR1  = B00000010; // Interrupt on compare B
  OCR1A = 22000; // 22mS PPM output refresh
  OCR1B = 1000;

  // initialize the digital LED pin as an output
  // should be sequential, numbering up, lowest pin sets offsets
  lpins[0] = 2; // initialize D2 for output to LEDs
  lpins[1] = 3; // initialize D3 for output to LEDs
  lpins[2] = 4; // initialize D4 for output to LEDs

  sequenceon(); // Does one loop of the cylon effect
  
  // initialize the digital switch pins as an output
  sPins[0] = 10; // initialize D10 for output for keypress
  sPins[1] = 11; // initialize D11 for output for keypress
  sPins[2] = 12; // initialize D12 for output for keypress

//  Serial.begin(9600);      // turn on Serial Port

}

void ppmoutput() { // PPM output sub

 // Channel 1 - Analog 1
  digitalWrite(outPinPPM, LOW);
  delayMicroseconds(Fixed_uS);    // Hold
  digitalWrite(outPinPPM, HIGH);
  delayMicroseconds(A1_uS);  // Hold for A1_uS microseconds 

 // Channel 2 - Analog 2
  digitalWrite(outPinPPM, LOW);
  delayMicroseconds(Fixed_uS);    // Hold
  digitalWrite(outPinPPM, HIGH);
  delayMicroseconds(A2_uS);  // Hold for A2_uS microseconds 

 // Channel 3 - Analog 3
  digitalWrite(outPinPPM, LOW);
  delayMicroseconds(Fixed_uS);    // Hold
  digitalWrite(outPinPPM, HIGH);
  delayMicroseconds(A3_uS);  // Hold for A3_uS microseconds 

// Channel 4 - Switch 1 3-way
  digitalWrite(outPinPPM, LOW);
  delayMicroseconds(Fixed_uS);    // Hold
  digitalWrite(outPinPPM, HIGH);
  delayMicroseconds(sw1_uS);     // Hold for sw1_uS microseconds 

// Channel 5 - Switch 2 3-way
  digitalWrite(outPinPPM, LOW);
  delayMicroseconds(Fixed_uS);    // Hold
  digitalWrite(outPinPPM, HIGH);
  delayMicroseconds(sw2_uS);     // Hold for sw2_uS microseconds 

// Channel 6 - Switch 3 3-way
  digitalWrite(outPinPPM, LOW);
  delayMicroseconds(Fixed_uS);    // Hold
  digitalWrite(outPinPPM, HIGH);
  delayMicroseconds(sw3_uS);     // Hold for sw3_uS microseconds 

 // Channel 7 - Switch Matrix
  digitalWrite(outPinPPM, LOW);
  delayMicroseconds(Fixed_uS);    // Hold
  digitalWrite(outPinPPM, HIGH);
  delayMicroseconds(M1_uS);     // Hold for M1_uS microseconds 

/* // Channel 8 - Switch 5
  digitalWrite(outPinPPM, LOW);
  delayMicroseconds(Fixed_uS);    // Hold
  digitalWrite(outPinPPM, HIGH);
  delayMicroseconds(sw4_uS);     // Hold for sw4_uS microseconds 
*/
 // Synchro pulse
  digitalWrite(outPinPPM, LOW);
  delayMicroseconds(Fixed_uS);    // Hold
  digitalWrite(outPinPPM, HIGH);  // Start Synchro pulse

}


void loop() {

// logic to read each analog input

 // Read analog pins
   AI_Raw_A1 = analogRead(potPin_A1); // Analog Pot 1
   AI_Raw_A2 = analogRead(potPin_A2); // Analog Pot 2
   AI_Raw_A3 = analogRead(potPin_A3); // Rotary switch Pot 3
   
 // Map analog inputs to PPM rates for each of the channels
 //  compensate for analog 0-1023 vs expected 750-1700
   A1_uS = AI_Raw_A1 * adjustRng + pulseMin;
   A2_uS = AI_Raw_A2 * adjustRng + pulseMin;
   A3_uS = AI_Raw_A3 * adjustRng + pulseMin;

 // Check limits
  if (A1_uS <= pulseMin) A1_uS = pulseMin;   // Min
  if (A1_uS >= pulseMax) A1_uS = pulseMax;   // Max   
  if (A2_uS <= pulseMin) A2_uS = pulseMin;   // Min
  if (A2_uS >= pulseMax) A2_uS = pulseMax;   // Max 
  if (A3_uS <= pulseMin) A3_uS = pulseMin;   // Min
  if (A3_uS >= pulseMax) A3_uS = pulseMax;   // Max 

  if (digitalRead(swPin_D1) != 1) { // 3-way Switch 1
           sw1_uS = pulseMin;
  } else {
           if (digitalRead(swPin_D2) != 1 ) {
            sw1_uS = pulseMax;
  }        else {
            sw1_uS = pulseMid;
            }
  }
  if (digitalRead(swPin_D3) != 1) { // 3-way Switch 2
           sw2_uS = pulseMin;
  } else {
           if (digitalRead(swPin_D4) != 1 ) {
            sw2_uS = pulseMax;
  }        else {
            sw2_uS = pulseMid;
            }
  }
  if (digitalRead(swPin_D5) != 1) { // 3-way Switch 3  
           sw3_uS = pulseMin;
  //} else {
  //        if (digitalRead(swPin_D6) != 1) {   
  //         sw3_uS = pulseMax;
  }       else {
           sw3_uS = pulseMid;
           }
/*   Use the fm# variables to assign a switch mode to sw3 if desired           
    if ((A3_uS) > fm1 ) {
      swState = 1;
    }
    if ((A3_uS) > fm2 ) {
      swState = 2;
    }
    if ((A3_uS) > fm3 ) {
      swState = 3;
    }
    if ((A3_uS) > fm4 ) {
      swState = 4;
    }
    if ((A3_uS) > fm5 ) {
      swState = 5;
    }
    if ((A3_uS) > fm6 ) {
      swState = 6;
    }
*/
//    Serial.print(A3_uS);
    // get switch value from matrix
    // output PPM stream to match the relative 750-1700uS to APM 1100-1900
    // APM wants 1=1165, 2=1295, 3=1425, 4=1555, 5=1685, 6=1815
    // so we use 1=826, 2=980, 3=1140, 4=1290, 5=1445, 6=1595
    swState = readMatrix(); // get switch value from matrix
    switch (swState){
      case 1:
        M1_uS = 826;
      break;
      case 2:
        M1_uS = 980;
      break;
      case 3:
        M1_uS = 1140;
      break;
      case 4:
        M1_uS = 1290;
      break;
      case 5:
        M1_uS = 1445;
      break;
      case 6:
        M1_uS = 1595;
      break;
    }
        turnon(swState-1);

//    Serial.print(" ");
//    Serial.print(swState);
//    Serial.print(" ");
//    Serial.println(M1_uS);
    lastswState = swState;
    if (swState != lastswState) {
      turnon(swState-1); // need to adjust because the ledPins matrix is 0-5, not 1-6
    }
}

void turnon(int led) // This turns on a certain led from the list of leds
{
  int pospin = ledPins[led][0] + lpins[0]; 
  int negpin = ledPins[led][1] + lpins[0];
  
  alloff();
  pinMode (pospin, OUTPUT);
  digitalWrite (pospin, HIGH);
  pinMode (negpin, OUTPUT);
  digitalWrite (negpin, LOW);
}

void alloff() // This turns all the LED's off
{
  for(int i = 0; i < 4; i++)
  {
    pinMode (lpins[i], INPUT);
  }
}

void sequenceon() // This handles the cylon effect
{
  for(int i = 0; i < 6; i++)
  {
    turnon(i);
    delay(runspeed);   
  }
  for(int n = 6; n > 0; n--)
  {
    turnon(n-1);
    delay(runspeed);   
  }
}

int readMatrix() // returns the last switch pressed as number from 1-6
{
    // read the matrix for pinA switches
    pinMode (sPins[1], INPUT_PULLUP);
    pinMode (sPins[2], INPUT_PULLUP);
    pinMode (sPins[0], OUTPUT); // set pinA to output, LOW
    digitalWrite (sPins[0], LOW);
    sPinB = digitalRead (sPins[1]); // if pinB is low SW2
    sPinC = digitalRead (sPins[2]); // if pinC is low SW5
    if (sPinB == LOW) {
      swMode = 2;
    }
    if (sPinC == LOW) {
      swMode = 5;
    }

    // read the matrix for pinB switches
    pinMode (sPins[0], INPUT_PULLUP);
    pinMode (sPins[1], OUTPUT); // set pinB to output, LOW
    digitalWrite (sPins[1], LOW);
    sPinA = digitalRead (sPins[0]); // if pinA is low SW4
    sPinC = digitalRead (sPins[2]); // if pinC is low SW1
    if (sPinA == LOW) {
      swMode = 4;
    }
    if (sPinC == LOW) {
      swMode = 1;
    }
    // read the matrix for pinC switches
    pinMode (sPins[1], INPUT_PULLUP);
    pinMode (sPins[2], OUTPUT); // set pinC to output, LOW
    digitalWrite (sPins[2], LOW);
    sPinA = digitalRead (sPins[0]); // if pinA is low SW6
    sPinB = digitalRead (sPins[1]); // if pinB is low SW3
    if (sPinA == LOW) {
      swMode = 6;
    }
    if (sPinB == LOW) {
      swMode = 3;
    }

    return swMode;
}