BESKAR2D2 – test board with RJ45

#include <ReefwingSBUS.h>
#define MIN_LEFT_ROLL    -255
#define MAX_LEFT_ROLL     255
#define MIN_LEFT_PITCH    -255
#define MAX_LEFT_PITCH     255
#define MIN_RIGHT_ROLL   -255
#define MAX_RIGHT_ROLL    255
#define MIN_RIGHT_PITCH   -255
#define MAX_LEFT_PITCH    255
#define MIN_SBUS          200
#define MAX_SBUS          1800
//#define SWITCH_ON         991
SBUS RX_from_RADIOLINK(Serial3);
uint16_t channels[16];
uint16_t lostFrameCtr = 0;
uint16_t sbusChannel6 = 0, sbusChannel7 = 0, sbusChannel8 = 0, sbusChannel5 = 0, rxMode = 0;
int16_t sbusChannel1 = 0, sbusChannel2 = 0, sbusChannel3 = 0, sbusChannel4 = 0;
bool failSafe;
bool lostFrame;
bool armed = false;
void setup() {
  //  Start Serial and wait for connection from computer
  Serial.begin(115200);
  while (!Serial);
  RX_from_RADIOLINK.begin();
//BLUE RJ45 WIRE A
  pinMode(26, OUTPUT);
  pinMode(27, OUTPUT);
  pinMode(28, OUTPUT);
  pinMode(29, OUTPUT);
//wHITE WIRE  RJ45 WIRE b
  pinMode(30, OUTPUT);
  pinMode(31, OUTPUT);
  pinMode(32, OUTPUT);
  pinMode(33, OUTPUT);
//BLUE RJ45 WIRE B
  pinMode(34, OUTPUT);
  pinMode(35, OUTPUT);
  pinMode(36, OUTPUT);
  pinMode(37, OUTPUT);
//WHITE RJ45 WIRE B
  pinMode(38, OUTPUT);
  pinMode(39, OUTPUT);
  pinMode(40, OUTPUT);
  pinMode(41, OUTPUT);
//GREY RJ45 WIRE A
  pinMode(45, OUTPUT);//this is capable of PWM//speed
  pinMode(49, OUTPUT);//direction
  pinMode(50, OUTPUT);//BRAKE
  pinMode(51, OUTPUT);//STOP
//GREY RJ45 WIRE B
  pinMode(44, OUTPUT);//this is capable of PWM  //SPEED
  pinMode(48, OUTPUT);//DIRECTION
  pinMode(46, OUTPUT);//BRAKE
  pinMode(47, OUTPUT);//STOP
}
void loop() {
  // Check for a valid SBUS packet from the RX_from_RADIOLINK receiver
  if (RX_from_RADIOLINK.read(&channels[0], &failSafe, &lostFrame)) {
   //Scale the joysticks to the min and max
    sbusChannel1 = map(channels[0], MIN_SBUS, MAX_SBUS, MIN_LEFT_ROLL, MAX_LEFT_ROLL);
    sbusChannel2  = map(channels[1], MIN_SBUS, MAX_SBUS,  MAX_LEFT_PITCH,MIN_LEFT_PITCH);
    sbusChannel4 = map(channels[3], MIN_SBUS, MAX_SBUS, MIN_RIGHT_PITCH, MAX_LEFT_PITCH);
    sbusChannel3 = map(channels[2], MIN_SBUS, MAX_SBUS, MIN_RIGHT_ROLL, MAX_RIGHT_ROLL);
    //  AUX Channels
    sbusChannel5 = channels[4];
    sbusChannel6     = channels[5];
    sbusChannel7    = channels[6];
    sbusChannel8    = channels[7];
    rxMode   = channels[8];
    if (lostFrame) { lostFrameCtr++; }
  //LEFT SWITCH
  switch (sbusChannel7) {
    case 200:
      Serial.print(“LS”);Serial.print(“A”);Serial.print(“\t”);
                digitalWrite(31, HIGH);
                  digitalWrite(32, LOW);
                  digitalWrite(33, LOW);
            break;
    case 1000:
      Serial.print(“LS”);Serial.print(“B”);Serial.print(“\t”);
                   digitalWrite(31, LOW);
                  digitalWrite(32,HIGH);
                  digitalWrite(33, LOW);
      break;
    case 1800:
      Serial.print(“LS”);Serial.print(“C”);Serial.print(“\t”);
      digitalWrite(31, LOW);
                  digitalWrite(32, LOW);
                  digitalWrite(33, HIGH);
      break;
    default:
     Serial.println(“Invalid value”);
  }
/////////////////////////////// LEFT VOLUME
      Serial.print(“LV”);Serial.print(sbusChannel8); Serial.print(“\t”);
////LEFT X
        Serial.print(“LX”);Serial.print(sbusChannel4); Serial.print(“\t”);
      if (sbusChannel4 >= -255 && sbusChannel4 < -50) {
    // sbusChannel is within the range from -100 to -50
                  digitalWrite(38, HIGH);
                  digitalWrite(39, HIGH);
                  digitalWrite(40, LOW);
                  digitalWrite(41, LOW);
} else if (sbusChannel4 >= -50 && sbusChannel4 < -10) {
    // sbusChannel is within the range from -50 to -10
                   digitalWrite(38, HIGH);
                  digitalWrite(39, LOW);
                  digitalWrite(40, LOW);
                  digitalWrite(41, LOW);
} else if (sbusChannel4 >= -10 && sbusChannel4 < 10) {
    // sbusChannel is within the range from -10 to 10
                  digitalWrite(38, LOW);
                  digitalWrite(39, LOW);
                  digitalWrite(40, LOW);
                  digitalWrite(41, LOW);
} else if (sbusChannel4 >= 10 && sbusChannel4 < 50) {
    // sbusChannel is within the range from 10 to 50
                  digitalWrite(38, LOW);
                  digitalWrite(39, LOW);
                  digitalWrite(40, LOW);
                  digitalWrite(41, HIGH);
} else if (sbusChannel4 >= 50 && sbusChannel4 <= 255) {
    // sbusChannel is within the range from 50 to 100
                  digitalWrite(38, LOW);
                  digitalWrite(39, LOW);
                  digitalWrite(40, HIGH);
                  digitalWrite(41, HIGH);
} else {
    // sbusChannel is outside of all of the specified ranges
}
//////////////////////LEFT Y AXIS
    Serial.print(“LY”);Serial.print(sbusChannel3); Serial.print(“\t”);
      if (sbusChannel3 >= -255 && sbusChannel3 < -50) {
    // sbusChannel is within the range from -100 to -50
                  analogWrite(45,fabs(sbusChannel3) );//speed
                  digitalWrite(49, LOW);//direction
                  digitalWrite(50, LOW);//BRAKE
                  digitalWrite(51, LOW);//STOP//
Serial.print(“RF”);
} else if (sbusChannel3 >= -50 && sbusChannel3 < -10) {
    // sbusChannel is within the range from -50 to -10
                  analogWrite(45,fabs(sbusChannel1) );//speed
                  digitalWrite(49, LOW);//direction
                  digitalWrite(50, HIGH);//BRAKE
                  digitalWrite(51, LOW);//STOP//
Serial.print(“RS”);
} else if (sbusChannel3 >= -10 && sbusChannel3 < 10) {
    // sbusChannel is within the range from -10 to 10
                   analogWrite(45,fabs(sbusChannel3) );//speed
                  digitalWrite(49, LOW);//direction
                  digitalWrite(50, HIGH);//BRAKE
                  digitalWrite(51, HIGH);//STOP//
Serial.print(“SS”);
} else if (sbusChannel3 >= 10 && sbusChannel3 < 50) {
    // sbusChannel is within the range from 10 to 50
                  analogWrite(45,fabs(sbusChannel3) );//speed
                  digitalWrite(49, HIGH);//direction
                  digitalWrite(50, HIGH);//BRAKE
                  digitalWrite(51, LOW);//STOP//
Serial.print(“FS”);
} else if (sbusChannel3 >= 50 && sbusChannel3 <= 255) {
    // sbusChannel is within the range from 50 to 100
                  analogWrite(45,fabs(sbusChannel3) );//speed
                  digitalWrite(49, HIGH);//direction
                  digitalWrite(50, LOW);//BRAKE
                  digitalWrite(51, LOW);//STOP//
Serial.print(“FF”);
} else {
    // sbusChannel is outside of all of the specified ranges
}
 Serial.print(“\t”);
 Serial.print(“\t”);
 Serial.print(“\t”);
//////////////////////////////
//RIGHT SIDE OF CONTROLLER
/////////////////////////////
////////////////////////////////
    Serial.print(“RX”);Serial.print(sbusChannel1); Serial.print(“\t”);
      if (sbusChannel1 >= -255 && sbusChannel1 < -50) {
    // sbusChannel is within the range from -100 to -50
                  digitalWrite(34, HIGH);
                  digitalWrite(35, HIGH);
                  digitalWrite(36, LOW);
                  digitalWrite(37, LOW);
} else if (sbusChannel1 >= -50 && sbusChannel1 < -10) {
    // sbusChannel is within the range from -50 to -10
                   digitalWrite(34, HIGH);
                  digitalWrite(35, LOW);
                  digitalWrite(36, LOW);
                  digitalWrite(37, LOW);
} else if (sbusChannel1 >= -10 && sbusChannel1 < 10) {
    // sbusChannel is within the range from -10 to 10
digitalWrite(34, LOW);
                  digitalWrite(35, LOW);
                  digitalWrite(36, LOW);
                  digitalWrite(37, LOW);
} else if (sbusChannel1 >= 10 && sbusChannel1 < 50) {
    // sbusChannel is within the range from 10 to 50
    digitalWrite(34, LOW);
                  digitalWrite(35, LOW);
                  digitalWrite(36, LOW);
                  digitalWrite(37, HIGH);
} else if (sbusChannel1 >= 50 && sbusChannel1 <= 255) {
    // sbusChannel is within the range from 50 to 100
    digitalWrite(34, LOW);
                  digitalWrite(35, LOW);
                  digitalWrite(36, HIGH);
                  digitalWrite(37, HIGH);
} else {
    // sbusChannel is outside of all of the specified ranges
}
///////////////////////////////////////RIGHT Y
    Serial.print(“RY”);Serial.print(sbusChannel2); Serial.print(“\t”);
      if (sbusChannel2 >= -255 && sbusChannel2 < -50) {
    // sbusChannel is within the range from -100 to -50
                  analogWrite(44,fabs(sbusChannel2) );//speed
                  digitalWrite(48, LOW);//direction
                  digitalWrite(46, LOW);//BRAKE
                  digitalWrite(47, LOW);//STOP
Serial.print(“RF”);
} else if (sbusChannel2 >= -50 && sbusChannel2 < -10) {
    // sbusChannel is within the range from -50 to -10
                   analogWrite(44,fabs(sbusChannel2) );//speed
                  digitalWrite(48, LOW);//direction
                  digitalWrite(46, HIGH);//BRAKE
                  digitalWrite(47, LOW);//STOP
Serial.print(“RS”);
} else if (sbusChannel2 >= -10 && sbusChannel2 < 10) {
    // sbusChannel is within the range from -10 to 10
                  analogWrite(44,fabs(sbusChannel2) );//speed
                  digitalWrite(48, LOW);//direction
                  digitalWrite(46, HIGH);//BRAKE
                  digitalWrite(47, HIGH);//STOP
Serial.print(“ST”);
} else if (sbusChannel2 >= 10 && sbusChannel2 < 50) {
    // sbusChannel is within the range from 10 to 50
                  analogWrite(44,fabs(sbusChannel2) );//speed
                  digitalWrite(48, HIGH);//direction
                  digitalWrite(46, LOW);//BRAKE
                  digitalWrite(47, LOW);//STOP
                Serial.print(“FS”);
} else if (sbusChannel2 >= 50 && sbusChannel2 <= 255) {
    // sbusChannel is within the range from 50 to 100
                  analogWrite(44,fabs(sbusChannel2) );//speed
                  digitalWrite(48, HIGH);//direction
                  digitalWrite(46, LOW);//BRAKE
                  digitalWrite(47, LOW);//STOP
                  Serial.print(“FF”);
} else {
    // sbusChannel is outside of all of the specified ranges
}
Serial.print(“\t”);
     //Serial.print(“RB”);Serial.print(sbusChannel6); Serial.print(“\t”);
switch (sbusChannel6) {
    case 200:
      Serial.print(“RB”);Serial.print(“O”);Serial.print(“\t”);
                  digitalWrite(26, LOW);
                  digitalWrite(30, LOW);
            break;
    case 1800:
    Serial.print(“RB”);  Serial.print(“P”);Serial.print(“\t”);
                  digitalWrite(26, HIGH);
                      digitalWrite(30, HIGH);
      break;
    default:
      Serial.print(“RB”);Serial.print(“Invalid value”);Serial.print(“\t”);
      digitalWrite(26, LOW);
      digitalWrite(30, LOW);
      break;
  }
;
  switch (sbusChannel5) {
    case 200:
      Serial.print(“RS”);Serial.print(“A”);Serial.print(“\t”);
                  digitalWrite(27, HIGH);
                  digitalWrite(28, LOW);
                  digitalWrite(29, LOW);
                  break;
    case 1000:
      Serial.print(“RS”);Serial.print(“B”);Serial.print(“\t”);
                  digitalWrite(27, LOW);
                  digitalWrite(28,HIGH);
                  digitalWrite(29, LOW);
                  break;
    case 1800:
    Serial.print(“RS”);  Serial.print(“C”);Serial.print(“\t”);
                  digitalWrite(27, LOW);
                  digitalWrite(28, LOW);
                  digitalWrite(29, HIGH);
      break;
    default:
      Serial.print(“RS”);Serial.print(“Invalid value”);Serial.print(“\t”);
                        digitalWrite(27, LOW);
                  digitalWrite(28, LOW);
                  digitalWrite(29, LOW);
  }
;
Serial.print(“\t”);
       Serial.println(rxMode);
  }
  delay(10);
}