파파울프의 일상 웹 저장소

// Thrustmaster Cougar TQS USB Project

#include "Joystick.h"
#include <Wire.h>
#include <Adafruit_ADS1X15.h>
#include <EEPROM.h>

// Create Joystick
Joystick_ Joystick(JOYSTICK_DEFAULT_REPORT_ID, 
  JOYSTICK_TYPE_JOYSTICK, 12, 0,
  true, true, false, true, true, false,
  false, true, false, false, false);

// Set to true to test "Auto Send" mode or false to test "Manual Send" mode.
Adafruit_ADS1115 ads1115;  /* Use this for the 16-bit version */

struct strCalibrationData {
  // 각종 축 MIN 값
  unsigned int minX;
  unsigned int minY;
  unsigned int minAnt;
  unsigned int minRng;
  unsigned int minThtl;
  
  // 각종 축 MAX 값
  unsigned int maxX;
  unsigned int maxY;
  unsigned int maxAnt;
  unsigned int maxRng;
  unsigned int maxThtl;
  
  // 각종 축 Center 값 (미사용)
  unsigned int centerX;
  unsigned int centerY;
  unsigned int centerAnt;
  unsigned int centerRng;
  unsigned int centerThtl;
};

// analog 측정값
unsigned int xAxis           = 0;    // 미니스틱X축
unsigned int yAxis           = 0;    // 미니스틱Y축
unsigned int antAxis         = 0;    // Ant. Elevation 축
unsigned int rngAxis         = 0;    // Range 축
unsigned int throttleAxis    = 0;    // 스로틀축

// 각종 축 Mapping 값
unsigned int mapXAxis        = 0;
unsigned int mapYAxis        = 0;
unsigned int mapAntAxis      = 0;
unsigned int mapRngAxis      = 0;
unsigned int mapThrottleAxis = 0;

// 아두이노 ADC 핀번호
int xPin            = A3;
int yPin            = 10;
int antPin          = A1;
int rngPin          = A0;
int throttlePin     = A2;

// 아두이노 버튼 핀번호
int  btnX1          = 4;
int  btnX2          = 5;
int  btnX3          = 6;
int  btnX4          = 7;

// 아두이노 Pulldown 출력 핀번호
int btnY1           = 8;
int btnY2           = 9;
int btnY3           = 14;

// 필터용 변수
float EMA_ax         = 0.6;
int   EMA_sx         = 0;

float EMA_ay         = 0.6;
int   EMA_sy         = 0;

float EMA_aAnt       = 0.6;
int   EMA_sAnt       = 0;

float EMA_aRng       = 0.6;
int   EMA_sRng       = 0;

float EMA_at         = 0.6;
int   EMA_st         = 0;

// 버튼 현재 값
int currButtonStateX[10] = {0,0,0,0,0,0,0,0,0,0};

// EEPROM 번지수
int addr            = 0;

// 시간측정용
unsigned long currentTime = 0;
boolean  isPressed        = false;
boolean  isSetupEnded     = false;
boolean  Startup          = true;
  
strCalibrationData caliData = {
  // 각종 축 MIN 값
  //142, 131, 0, 0, 137, 696, 664, 749, 749, 681, 0, 0, 0, 0, 0
  0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
};

 
void setup() {
  Joystick.begin();

  if (!ads1115.begin()) {
    Serial.println("Failed to initialize ADS.");
    while (1);
  }

  // Set Range Values
  Joystick.setXAxisRange(0, 32767);
  Joystick.setYAxisRange(0, 32767);
  Joystick.setRxAxisRange(0, 32767);
  Joystick.setRyAxisRange(0, 32767);
  Joystick.setThrottleRange(0, 32767);

  //ads1115.setGain(GAIN_TWOTHIRDS);
  ads1115.setGain(GAIN_ONE);
  ads1115.setDataRate(RATE_ADS1115_860SPS); // 860 samples per second

  EMA_sx    = analogRead(xPin);
  EMA_sy    = analogRead(yPin);
  EMA_sAnt  = analogRead(antPin);
  EMA_sRng  = analogRead(rngPin);
  EMA_st    = analogRead(throttlePin);

  // Initialize Button Pins
  pinMode(btnX1, INPUT);
  pinMode(btnX2, INPUT);
  pinMode(btnX3, INPUT);
  pinMode(btnX4, INPUT);
  pinMode(btnY1, OUTPUT);
  pinMode(btnY2, OUTPUT);
  pinMode(btnY3, OUTPUT);

  Serial.begin(9600);

  EEPROM.get(0, caliData);

  Serial.print(caliData.minX);
  Serial.print(" : ");
  Serial.print(caliData.centerX);
  Serial.print(" : ");
  Serial.print(caliData.maxX);
  Serial.print(" / ");
  Serial.print(caliData.minY);
  Serial.print(" : ");
  Serial.print(caliData.centerY);
  Serial.print(" : ");
  Serial.print(caliData.maxY);
  Serial.print(" / ");
  Serial.print(caliData.minAnt);
  Serial.print(" : ");
  Serial.print(caliData.maxAnt);
  Serial.print(" / ");
  Serial.print(caliData.minRng);
  Serial.print(" : ");
  Serial.print(caliData.maxRng);
  Serial.print(" / ");
  Serial.print(caliData.minThtl);
  Serial.print(" : ");
  Serial.print(caliData.maxThtl);
  Serial.println(" / ");
}

void loop() {

  // Analog축 처리
  analogAxisProcess();

  // 버튼 처리
  buttonMatrixProcess();

  // T6, T3 버튼 5초 입력시 캘리브레이션 시작
  if (currButtonStateX[8] == 0 && currButtonStateX[1] == 0)
  {
    if (isPressed == false)
    {
      isPressed = true;
      currentTime = millis();

      Serial.println(currentTime);
    }

    if (millis() - currentTime > 5000)
    {
      if (isSetupEnded == false)
      {
        Serial.println("SETUP");
        Calibration();
      }
    }
  }
  else if (currButtonStateX[8] == 0 && currButtonStateX[0] == 0)
  {
    if (isPressed == false)
    {
      isPressed = true;
      currentTime = millis();

      Serial.println(currentTime);
    }

    if (millis() - currentTime > 5000)
    {
      if (isSetupEnded == false)
      {
        Serial.println("RESET");
        Reset();
      }
    }
  }
  else
  {
    isPressed    = false;
    isSetupEnded = false;
    currentTime = millis();
  }

  //delay(100);
}

void Calibration()
{
  delay(100);
  Serial.println("--- START SAVE Cougar TQS Calibration DATA ---");

  // Serial.println(EEPROM.length());
  addr = 0;

  Serial.print(caliData.minX);
  Serial.print(" : ");
  Serial.print(caliData.maxX);
  Serial.print(" / ");
  Serial.print(caliData.minY);
  Serial.print(" : ");
  Serial.print(caliData.maxY);
  Serial.print(" / ");
  Serial.print(caliData.minAnt);
  Serial.print(" : ");
  Serial.print(caliData.maxAnt);
  Serial.print(" / ");
  Serial.print(caliData.minRng);
  Serial.print(" : ");
  Serial.print(caliData.maxRng);
  Serial.print(" / ");
  Serial.print(caliData.minThtl);
  Serial.print(" : ");
  Serial.print(caliData.maxThtl);
  Serial.println(" / ");

  EEPROM.put(addr, caliData);
  
//  EEPROM.write( 0, (byte) minX>>8);
//  EEPROM.write( 1, (byte) minX);
//  EEPROM.write( 2, (byte) maxX>>8);
//  EEPROM.write( 3, (byte) maxX);
//  EEPROM.write( 4, (byte) minY>>8);
//  EEPROM.write( 5, (byte) minY);
//  EEPROM.write( 6, (byte) maxY>>8);
//  EEPROM.write( 7, (byte) maxY);
//  EEPROM.write( 8, (byte) minAnt>>8);
//  EEPROM.write( 9, (byte) minAnt);
//  EEPROM.write(10, (byte) maxAnt>>8);
//  EEPROM.write(11, (byte) maxAnt);
//  EEPROM.write(12, (byte) minRng>>8);
//  EEPROM.write(13, (byte) minRng);
//  EEPROM.write(14, (byte) maxRng>>8);
//  EEPROM.write(15, (byte) maxRng);
//  EEPROM.write(16, (byte) minThtl>>8);
//  EEPROM.write(17, (byte) minThtl);
//  EEPROM.write(18, (byte) maxThtl>>8);
//  EEPROM.write(19, (byte) maxThtl);

  //while (addr < EEPROM.length())
//  while (addr < 40)
//  {
//    Serial.print(addr);
//    Serial.print("\t");
//    Serial.print(EEPROM.read(addr));
//    Serial.println();
//    addr++;
//  }
  
  isSetupEnded    = true;
  
  Serial.println("--- END SAVE Cougar TQS Calibration DATA ---");
}

void Reset()
{
  delay(100);
  Serial.println("--- START SAVE Cougar TQS Initial DATA ---");

  // Serial.println(EEPROM.length());
  addr = 0;

  caliData.minX    += 4000;
  caliData.maxX    -= 4000;
  caliData.minY    += 4000;
  caliData.maxY    -= 4000;
  caliData.minAnt  += 4000;
  caliData.maxAnt  -= 4000;
  caliData.minRng  += 400;
  caliData.maxRng  -= 400;
  caliData.minThtl += 4000;
  caliData.maxThtl -= 4000;

  Serial.print(caliData.minX);
  Serial.print(" : ");
  Serial.print(caliData.maxX);
  Serial.print(" / ");
  Serial.print(caliData.minY);
  Serial.print(" : ");
  Serial.print(caliData.maxY);
  Serial.print(" / ");
  Serial.print(caliData.minAnt);
  Serial.print(" : ");
  Serial.print(caliData.maxAnt);
  Serial.print(" / ");
  Serial.print(caliData.minRng);
  Serial.print(" : ");
  Serial.print(caliData.maxRng);
  Serial.print(" / ");
  Serial.print(caliData.minThtl);
  Serial.print(" : ");
  Serial.print(caliData.maxThtl);
  Serial.println(" / ");

  EEPROM.put(addr, caliData);
  
//  EEPROM.write( 0, (byte) minX>>8);
//  EEPROM.write( 1, (byte) minX);
//  EEPROM.write( 2, (byte) maxX>>8);
//  EEPROM.write( 3, (byte) maxX);
//  EEPROM.write( 4, (byte) minY>>8);
//  EEPROM.write( 5, (byte) minY);
//  EEPROM.write( 6, (byte) maxY>>8);
//  EEPROM.write( 7, (byte) maxY);
//  EEPROM.write( 8, (byte) minAnt>>8);
//  EEPROM.write( 9, (byte) minAnt);
//  EEPROM.write(10, (byte) maxAnt>>8);
//  EEPROM.write(11, (byte) maxAnt);
//  EEPROM.write(12, (byte) minRng>>8);
//  EEPROM.write(13, (byte) minRng);
//  EEPROM.write(14, (byte) maxRng>>8);
//  EEPROM.write(15, (byte) maxRng);
//  EEPROM.write(16, (byte) minThtl>>8);
//  EEPROM.write(17, (byte) minThtl);
//  EEPROM.write(18, (byte) maxThtl>>8);
//  EEPROM.write(19, (byte) maxThtl);

  //while (addr < EEPROM.length())
//  while (addr < 40)
//  {
//    Serial.print(addr);
//    Serial.print("\t");
//    Serial.print(EEPROM.read(addr));
//    Serial.println();
//    addr++;
//  }
  
  isSetupEnded    = true;
  
  Serial.println("--- END SAVE Cougar TQS Initial DATA ---");
}

void analogAxisProcess()
{
  //xAxis           = analogRead(xPin);
  //yAxis           = analogRead(yPin);
  //antAxis         = analogRead(antPin);
  rngAxis         = analogRead(rngPin);
  //throttleAxis    = analogRead(throttlePin);

  // Use ADS1115 
  throttleAxis    = ads1115.readADC_SingleEnded(0);
  xAxis           = ads1115.readADC_SingleEnded(2);
  yAxis           = ads1115.readADC_SingleEnded(1);
  //antAxis         = ads1115.readADC_SingleEnded(3);
  antAxis = twosComplement(ads1115.readADC_SingleEnded(3));
  
//  Serial.print("X:");
//  Serial.print(xAxis);
//  Serial.print("  Y:");
//  Serial.print(yAxis);
//  Serial.print("  ANT:");
//  Serial.print(antAxis);
//  Serial.print("  RNG:");
//  Serial.print(mapRngAxis);
//  Serial.print("  Throttle:");
//  Serial.print(throttleAxis);
//  Serial.println();

  if (caliData.minX    > xAxis)        caliData.minX    = xAxis;        caliData.centerX = caliData.minX + ((caliData.maxX - caliData.minX) / 2);
  if (caliData.minY    > yAxis)        caliData.minY    = yAxis;        caliData.centerY = caliData.minY + ((caliData.maxY - caliData.minY) / 2);
  if (caliData.minAnt  > antAxis)      caliData.minAnt  = antAxis;
  if (caliData.minRng  > rngAxis)      caliData.minRng  = rngAxis;
  if (caliData.minThtl > throttleAxis) caliData.minThtl = throttleAxis;
  
  if (caliData.maxX    < xAxis)        caliData.maxX    = xAxis;
  if (caliData.maxY    < yAxis)        caliData.maxY    = yAxis;
  if (caliData.maxAnt  < antAxis)      caliData.maxAnt  = antAxis;
  if (caliData.maxRng  < rngAxis)      caliData.maxRng  = rngAxis;
  if (caliData.maxThtl < throttleAxis) caliData.maxThtl = throttleAxis;

  EMA_sx   = (EMA_ax   * xAxis)        + ((1 - EMA_ax  ) * EMA_sx  );
  EMA_sy   = (EMA_ay   * yAxis)        + ((1 - EMA_ay  ) * EMA_sy  );
  EMA_sAnt = (EMA_aAnt * antAxis)      + ((1 - EMA_aAnt) * EMA_sAnt);
  EMA_sRng = (EMA_aRng * rngAxis)      + ((1 - EMA_aRng) * EMA_sRng);
  EMA_st   = (EMA_at   * throttleAxis) + ((1 - EMA_at  ) * EMA_st  );
  
  mapXAxis        = map(EMA_sx  , caliData.minX  ,  caliData.maxX  ,    32767, 0);
  mapYAxis        = map(EMA_sy  , caliData.minY  ,  caliData.maxY  ,    32767, 0);
  mapAntAxis      = map(EMA_sAnt, caliData.minAnt,  caliData.maxAnt,    0, 32767);
  mapRngAxis      = map(EMA_sRng, caliData.minRng,  caliData.maxRng,    0, 32767);
  mapThrottleAxis = map(EMA_st  , caliData.minThtl, caliData.maxThtl,   0, 32767);

  Joystick.setXAxis(mapXAxis);
  Joystick.setYAxis(mapYAxis);
  Joystick.setRxAxis(mapAntAxis);
  Joystick.setRyAxis(mapRngAxis);
  Joystick.setThrottle(mapThrottleAxis);

//  Serial.print("X:");
//  Serial.print(mapXAxis);
//  Serial.print("  Y:");
//  Serial.println(yAxis);
//  Serial.print("  ANT:");
//  Serial.print(mapAntAxis);
//  Serial.print("  RNG:");
//  Serial.print(mapRngAxis);
//  Serial.print("  Throttle:");
//  Serial.print(mapThrottleAxis);
//  Serial.print(" ||| ");
//  Serial.print(caliData.minX);
//  Serial.print(" : ");
//  Serial.print(caliData.maxX);
//  Serial.print(" / ");
//  Serial.print(caliData.minY);
//  Serial.print(" : ");
//  Serial.print(caliData.maxY);
//  Serial.print(" / ");
//  Serial.print(caliData.minAnt);
//  Serial.print(" : ");
//  Serial.print(caliData.maxAnt);
//  Serial.print(" / ");
//  Serial.print(caliData.minRng);
//  Serial.print(" : ");
//  Serial.print(caliData.maxRng);
//  Serial.print(" / ");
//  Serial.print(caliData.minThtl);
//  Serial.print(" : ");
//  Serial.print(caliData.maxThtl);
//  Serial.println(" / ");
}

void buttonMatrixProcess()
{
  digitalWrite(btnY1, LOW);
  delay(1);
  currButtonStateX[0] = digitalRead(btnX1);
  currButtonStateX[1] = digitalRead(btnX2);
  currButtonStateX[2] = digitalRead(btnX3);
  currButtonStateX[3] = digitalRead(btnX4);
  digitalWrite(btnY1, HIGH);

  digitalWrite(btnY2, LOW);
  delay(1);
  currButtonStateX[4] = digitalRead(btnX1);
  currButtonStateX[5] = digitalRead(btnX2);
  currButtonStateX[6] = digitalRead(btnX3);
  currButtonStateX[7] = digitalRead(btnX4);
  digitalWrite(btnY2, HIGH);

  digitalWrite(btnY3, LOW);
  delay(1);
  currButtonStateX[8] = digitalRead(btnX1);
  currButtonStateX[9] = digitalRead(btnX2);
  digitalWrite(btnY3, HIGH);

  // T1 CURSOR ENABLE
  if (currButtonStateX[9] == 0)
  {
    Joystick.setButton(0, true);
  }
  else
  {
    Joystick.setButton(0, false);
  }
  
  // T2 UP VHF
  if (currButtonStateX[1] == 0)
  {
    Joystick.setButton(1, true);
  }
  else
  {
    Joystick.setButton(1, false);
  }

  // T3 DOWN UHF
  if (currButtonStateX[0] == 0)
  {
    Joystick.setButton(2, true);
  }
  else
  {
    Joystick.setButton(2, false);
  }

  // T4 RIGHT IFF OUT
  if (currButtonStateX[3] == 0)
  {
    Joystick.setButton(3, true);
  }
  else
  {
    Joystick.setButton(3, false);
  }

  // T5 LEFT IFF IN
  if (currButtonStateX[2] == 0)
  {
    Joystick.setButton(4, true);
  }
  else
  {
    Joystick.setButton(4, false);
  }

  // T6 UNCAGE
  if (currButtonStateX[8] == 0)
  {
    Joystick.setButton(5, true);
  }
  else
  {
    Joystick.setButton(5, false);
  }

  // T7 DIGFIGHT OVERRIDE
  if (currButtonStateX[4] == 0)
  {
    Joystick.setButton(6, true);
  }
  else
  {
    Joystick.setButton(6, false);
  }

  // T8 MRM OVERRIDE
  if (currButtonStateX[5] == 0)
  {
    Joystick.setButton(7, true);
  }
  else
  {
    Joystick.setButton(7, false);
  }

  // T9 OPEN AIRBRAKE
  if (currButtonStateX[6] == 0)
  {
    Joystick.setButton(8, true);
  }
  else
  {
    Joystick.setButton(8, false);
  }

  // T10 CLOSE AIRBRAKE
  if (currButtonStateX[7] == 0)
  {
    Joystick.setButton(9, true);
  }
  else
  {
    Joystick.setButton(9, false);
  }

  // T11 NAV MODE
  if ((currButtonStateX[4] == 1) && (currButtonStateX[5] == 1))
  {
    Joystick.setButton(10, true);
  }
  else
  {
    Joystick.setButton(10, false);
  }

  // T12 MID AIRBRAKE
  if ((currButtonStateX[6] == 1) && (currButtonStateX[7] == 1))
  {
    Joystick.setButton(11, true);
  }
  else
  {
    Joystick.setButton(11, false);
  }

//  Serial.print("BTN : ");
//  Serial.print(currButtonStateX[0]);
//  Serial.print(currButtonStateX[1]);
//  Serial.print(currButtonStateX[2]);
//  Serial.print(currButtonStateX[3]);
//  Serial.print(currButtonStateX[4]);
//  Serial.print(currButtonStateX[5]);
//  Serial.print(currButtonStateX[6]);
//  Serial.print(currButtonStateX[7]);
//  Serial.print(currButtonStateX[8]);
//  Serial.print(currButtonStateX[9]);
//  Serial.println(" ");
}

unsigned int twosComplement(unsigned int value)
{
  if (value >= 0x8000){
    //Serial.println("OVER");

    value = 0;
  }
  return value;
}