#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);
}
