Teensy LC Throttle Quadrant

[kurt-olsson]

Almost final code, there are some tips and tricks here for rounding / NUllzones and not writing to X-plane if not needed to.


const int PIN_FLAPS = 21;
const int PIN_ENGINE_LEVER_1 = 18;
const int PIN_ENGINE_LEVER_2 = 19;

const int PIN_MOTOR_CONTROLLER_SPEED_LEVER_1 = 3; // PWM, do analogWrite
const int PIN_MOTOR_CONTROLLER_DIRECTION_1_LEVER_1 = 4;  //Motor 1 direction
const int PIN_MOTOR_CONTROLLER_DIRECTION_2_LEVER_1 = 6;  //Motor 2 direction

const int PIN_MOTOR_CONTROLLER_SPEED_LEVER_2 = 9; // PWM, do analogWrite
const int PIN_MOTOR_CONTROLLER_DIRECTION_1_LEVER_2 = 10;  //Motor 1 direction
const int PIN_MOTOR_CONTROLLER_DIRECTION_2_LEVER_2 = 20;  //Motor 2 direction

const int PIN_PARKING_BRAKE_PUSH_TO_TEST = 23;
const int PIN_PARKING_BRAKE_ON_OFF = 22;

const int PIN_PARKING_BRAKE_SWITCH = 15;

const int PIN_STAB_TRIM_MAIN_SELECT = -1;
const int PIN_STAB_TRIM_AUTO_PILOT = -1;

float throttle1 = 0;
float throttle2 = 0;

float flaps = 0;

const int NrOfDigitsInConversion = 2;
const int AverageIterations = 80;
const float ThrottleLeverNullZone = 0.02;
FlightSimFloat throttle1Position;
FlightSimFloat throttle2Position;
FlightSimFloat flapsRequestSettings;
FlightSimFloat autothrottleArmPosition;
FlightSimFloat parkingBrakeStatus;
FlightSimFloat parkingBrake;

void setup() {
  flapsRequestSettings = XPlaneRef("laminar/B738/flt_ctrls/flap_lever");
  throttle1Position = XPlaneRef("sim/cockpit2/engine/actuators/throttle_ratio[0]");
  throttle2Position = XPlaneRef("sim/cockpit2/engine/actuators/throttle_ratio[1]");
  autothrottleArmPosition = XPlaneRef("laminar/B738/autopilot/autothrottle_arm_pos");
  parkingBrakeStatus = XPlaneRef("laminar/B738/annunciator/parking_brake");
  parkingBrake = XPlaneRef("laminar/B738/parking_brake_pos");
 
  //Serial.begin(9600);
  analogReadResolution(16);

  pinMode(PIN_FLAPS,INPUT_PULLUP);
  pinMode(PIN_ENGINE_LEVER_2,INPUT_PULLUP);
  pinMode(PIN_ENGINE_LEVER_1,INPUT_PULLUP);
 
  pinMode(PIN_STAB_TRIM_MAIN_SELECT, INPUT_PULLUP);
  pinMode(PIN_STAB_TRIM_AUTO_PILOT, INPUT_PULLUP);

  pinMode(PIN_PARKING_BRAKE_PUSH_TO_TEST, OUTPUT);
  pinMode(PIN_PARKING_BRAKE_ON_OFF, OUTPUT);

  pinMode(PIN_MOTOR_CONTROLLER_SPEED_LEVER_1, OUTPUT);
  pinMode(PIN_MOTOR_CONTROLLER_DIRECTION_1_LEVER_1, OUTPUT);
  pinMode(PIN_MOTOR_CONTROLLER_DIRECTION_2_LEVER_1, OUTPUT);

  pinMode(PIN_MOTOR_CONTROLLER_SPEED_LEVER_2, OUTPUT);
  pinMode(PIN_MOTOR_CONTROLLER_DIRECTION_1_LEVER_2, OUTPUT);
  pinMode(PIN_MOTOR_CONTROLLER_DIRECTION_2_LEVER_2, OUTPUT);

  pinMode(PIN_PARKING_BRAKE_SWITCH, INPUT_PULLUP);
}

void loop() {

  //Test code autothrottle movement
  //-------------------------------------------------------
  /*analogWrite(PIN_MOTOR_CONTROLLER_SPEED,0);
  //Decrease power
  digitalWrite(PIN_MOTOR_CONTROLLER_DIRECTION_1,HIGH); 
  digitalWrite(PIN_MOTOR_CONTROLLER_DIRECTION_2,LOW);
  //Increase power
  digitalWrite(PIN_MOTOR_CONTROLLER_DIRECTION_1,HIGH); 
  digitalWrite(PIN_MOTOR_CONTROLLER_DIRECTION_2,LOW);*/
  //-------------------------------------------------------
 

  FlightSim.update();

  int parkingBrakeValue = digitalRead(PIN_PARKING_BRAKE_SWITCH) == 0 ? 1 : 0;
  if (parkingBrake != parkingBrakeValue)
     parkingBrake = parkingBrakeValue;
     
  digitalWrite(PIN_PARKING_BRAKE_ON_OFF,parkingBrakeStatus);  //ON OFF
  digitalWrite(PIN_PARKING_BRAKE_PUSH_TO_TEST,HIGH); //PUSH TO TEST
 
  // THROTTLES -------------------------------------
  //Throttle logic
  //--------------------------------------------------------------
  //Get average values for stable analog reading / this is min repeated 80 times for stable values (teeny is fast Hurray!)
  int throttleValue1Avg = 0;  //todo break out function.
  for(int i = 0; i < AverageIterations;i++) {
    throttleValue1Avg += analogRead(PIN_ENGINE_LEVER_1);
  }
  throttleValue1Avg = throttleValue1Avg / AverageIterations;

  int throttleValue2Avg = 0;
  for(int i = 0; i < AverageIterations;i++) {
    throttleValue2Avg += analogRead(PIN_ENGINE_LEVER_2);
  }
  throttleValue2Avg = throttleValue2Avg / AverageIterations;

  throttle1 = abs(mapfloat(throttleValue1Avg,2250,40,0.0,1.0));
  throttle2 = abs(mapfloat(throttleValue2Avg,2250,40,0.0,1.0));

  //Now round to only two digits so that we dont update the throttle position too much,
  //this causes the Teensy to stop working when sending too much info in the buffer.
  //this is also fixed with a total of 120ms delay.
 
  float throttle1Rounded = String(throttle1,NrOfDigitsInConversion).toFloat();
  float throttle2Rounded = String(throttle2,NrOfDigitsInConversion).toFloat();
   
  //Manual mode, no Autothrottle engaged in X-Plane
  if (autothrottleArmPosition == 0) {
    analogWrite(PIN_MOTOR_CONTROLLER_SPEED_LEVER_1,0);
    analogWrite(PIN_MOTOR_CONTROLLER_SPEED_LEVER_1,0);
    //dont write values if they are the same, keep data minimum!
    if (throttle1Position != throttle1Rounded)
      throttle1Position = throttle1Rounded;
 
    if (throttle2Position != throttle2Rounded)
      throttle2Position = throttle2Rounded;
   
  }  //Autothrottle engaged, follow the set throttle position.
  else if (autothrottleArmPosition == 1) {
    //Lever 1 logic   TODO: Break out to function.
    float delta_throttle_1 = throttle1Position - throttle1;
    //only move if the difference is bigger than Nullzone in lever position
    if (abs(delta_throttle_1) > ThrottleLeverNullZone) {  //0.02
        //manual throttle needs to catch up
        if (delta_throttle_1 > 0) {
           analogWrite(PIN_MOTOR_CONTROLLER_SPEED_LEVER_1,255);
           digitalWrite(PIN_MOTOR_CONTROLLER_DIRECTION_1_LEVER_1,LOW); 
           digitalWrite(PIN_MOTOR_CONTROLLER_DIRECTION_2_LEVER_1,HIGH);
        } //manual throttle needs to chill down
        else if (delta_throttle_1 < 0) {
          analogWrite(PIN_MOTOR_CONTROLLER_SPEED_LEVER_1,255);
          digitalWrite(PIN_MOTOR_CONTROLLER_DIRECTION_1_LEVER_1,HIGH); 
          digitalWrite(PIN_MOTOR_CONTROLLER_DIRECTION_2_LEVER_1,LOW);
        }
      //only move is there is a bigger seperation between lever and set position
    }
    else
    {
      //Make sure to stop the engine if movement is not needed, otherwise it will loop.
      analogWrite(PIN_MOTOR_CONTROLLER_SPEED_LEVER_1,0);
    }

    //Lever 2 logic   TODO: Break out to function.
    float delta_throttle_2 = throttle2Position - throttle2;
    //only move if the difference is bigger than Nullzone in lever position
    if (abs(delta_throttle_2) > ThrottleLeverNullZone) {
        //manual throttle needs to catch up
        if (delta_throttle_2 > 0) {
           analogWrite(PIN_MOTOR_CONTROLLER_SPEED_LEVER_2,255);
           digitalWrite(PIN_MOTOR_CONTROLLER_DIRECTION_1_LEVER_2,HIGH); 
           digitalWrite(PIN_MOTOR_CONTROLLER_DIRECTION_2_LEVER_2,LOW);
        } //manual throttle needs to chill down
        else if (delta_throttle_2 < 0) {
          analogWrite(PIN_MOTOR_CONTROLLER_SPEED_LEVER_2,255);
          digitalWrite(PIN_MOTOR_CONTROLLER_DIRECTION_1_LEVER_2,LOW); 
          digitalWrite(PIN_MOTOR_CONTROLLER_DIRECTION_2_LEVER_2,HIGH);
        }
      //only move is there is a bigger seperation between lever and set position
    }
    else
    {
      //Make sure to stop the engine if movement is not needed, otherwise it will loop.
      analogWrite(PIN_MOTOR_CONTROLLER_SPEED_LEVER_2,0);
    }

   
   
  }
 
  //FLAPS -----------------------------------------

  flaps = getFlapsPositionValue(analogRead(PIN_FLAPS));
 
  if (flapsRequestSettings != flaps)
      flapsRequestSettings = flaps;
 
  //FLAPS 40 DONT WORK, THIS IS DUE TO POTENTIOMETER THAT HAS TO LITTLE SPAN, 30 AND 40 GIVES THE SAME VALUES
 
  delay(50);
}

float getFlapsPositionValue(int flaps_analog_value) {
  if (flaps_analog_value > 0 && flaps_analog_value < 500) { return 0.00; }      //4 flaps 0
  if (flaps_analog_value > 500 && flaps_analog_value < 3000) { return 0.125; }   //700 flaps 1
  if (flaps_analog_value > 3000 && flaps_analog_value < 5200) { return 0.25; }    //4500 flaps 2
  if (flaps_analog_value > 5200 && flaps_analog_value < 7000) { return 0.375; }   //6000 flaps 5
  if (flaps_analog_value > 7000 && flaps_analog_value < 9200) { return 0.50; }    //8500 flaps 10
  if (flaps_analog_value > 9200 && flaps_analog_value < 10500) { return 0.625; }   //10000 flaps 15
  if (flaps_analog_value > 10500 && flaps_analog_value < 12000) { return 0.75; }    //11400 flaps 25
  if (flaps_analog_value > 12000 && flaps_analog_value < 14000) { return 0.875; }   //13100 flaps 30
  if (flaps_analog_value > 14000 && flaps_analog_value < 15000) { return 1.00; }    //15000 flaps 40 ?? DONT WORK DUE TO POT NOT REACHING VALUE
 
  return 0.00;
}

float mapfloat(float x, float in_min, float in_max, float out_min, float out_max)
{
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

[Joe Lavery]

You can't see me Kurt but I'm bowing my head while tipping my hat to the master....

[ElStino737]

Definitely appreciate these teensy code posts.
I will really start building my own project in a year or so and these code examples really will help me a lot  and probably anyone trying to do the same.

[RayS]

Awesome work!

Regarding the Flaps40 issue: You should be able to re-map the pot output of flaps_analog_value to something within range?

[kurt-olsson]

Thanks for the nice comments.
More sketches is coming.