The following code is a basic Arcade Drive style of code that uses the FlySky i6 hobby controller. The radio controller is not naturally mated to the Arduino coding language, however, the radio transmitter that comes with the controller can be used with the Arduino platform with just a few simple coding workarounds. Additionally, you will notice that this code does not require any additional libraries in order to work. The code is written in a class format which allows simple upfront coding, with the advantage of more usability inside the specific classes.
NOTE 1: THIS IS STILL VERSION 0.1 OF THE CODE AS OF THE MOMENT IT HAS NOT BEEN TESTED WITH AN ACTUAL BOT (as my team has yet to give me a bot to code)
NOTE 2: FURTHER CODE UPDATES WILL BE POSTED AT THIS LOCATION, SO THE CODE YOU SEE NOW MAY NOT BE THE CODE YOU SEE NEXT.
NOTE 3: THIS CODE UTILIZES A H-BRIDGE MOTOR CONTROLLER
/*
2019 Beetleweight Class Robot Code
Tri-C
Version: 0.1 Testing
Dan Jira
3/1/2019
jiratheman@gmail.com
dan-jira.blogspot.com
*/
class TestCode
{
public:
static void Setup();
static void Loop();
private:
// Member Functions
static void ArcadeDriveFSi6();
static void TankDriveFSi6();
static void SetMotorValues(double leftOutput, double rightOutput);
// Constants
// For Controller
static const bool USE_FS_I6_CONTROLLER = true;
static const unsigned FS_I6_SWITCH_THRESHOLD_VALUE = 1500;
static const unsigned long PULSE_IN_TIMEOUT_US = 50000;
// For H-Bridge
static const unsigned int H_BRIDGE_ENA = 3;
static const unsigned int H_BRIDGE_IN1 = 5;
static const unsigned int H_BRIDGE_IN2 = 6;
static const unsigned int H_BRIDGE_IN3 = 7;
static const unsigned int H_BRIDGE_IN4 = 8;
static const unsigned int H_BRIDGE_ENB = 9;
// For Unknown (commented out for now)
//static const unsigned int NANO_MOVE_LEFT_PIN = 10;
//static const unsigned int NANO_MOVE_RIGHT_PIN = 11;
// For Controller Channel Input Pins (need Mega)
static const unsigned int CH1_INPUT_PIN = 48;
static const unsigned int CH2_INPUT_PIN = 49;
static const unsigned int CH3_INPUT_PIN = 50;
static const unsigned int CH4_INPUT_PIN = 51;
static const unsigned int CH5_INPUT_PIN = 52;
static const unsigned int CH6_INPUT_PIN = 53;
};
//////////////////////////////////////////////////////////////////////
///
/// Method: Setup
///
/// Details: The Arduino initialization function called during
/// controller start up.
//////////////////////////////////////////////////////////////////////
void setup()
{
TestCode::Setup();
}
//////////////////////////////////////////////////////////////////////
///
/// Method: Loop
///
/// Details: Keeps the Arduino running, uses called code and
/// functions.
//////////////////////////////////////////////////////////////////////
void loop()
{
TestCode::Loop();
}
///////////////////////////////////////////////////////////////////////
///
/// Method: Setup
///
/// Description: Initialize TestCode function
///
///////////////////////////////////////////////////////////////////////
void TestCode::Setup()
{
// Final declaration of variables
const unsigned int ONE_SECOND_DELAY_MS = 1000;
// Begin Serial monitor @ 115200baud
Serial.begin(115200);
Serial.println("Test Code.");
// Check for controller
if (USE_FS_I6_CONTROLLER)
{
// Set pins for controller inputs
pinMode(CH1_INPUT_PIN, INPUT);
pinMode(CH2_INPUT_PIN, INPUT);
pinMode(CH3_INPUT_PIN, INPUT);
pinMode(CH4_INPUT_PIN, INPUT);
pinMode(CH5_INPUT_PIN, INPUT);
pinMode(CH6_INPUT_PIN, INPUT);
Serial.println("FlySky i6 Controller.");
} else {
// Warn for no controller
Serial.println("No controller configured.");
}
// H-Bridge outputs
pinMode(H_BRIDGE_ENA, OUTPUT);
pinMode(H_BRIDGE_IN1, OUTPUT);
pinMode(H_BRIDGE_IN2, OUTPUT);
pinMode(H_BRIDGE_IN3, OUTPUT);
pinMode(H_BRIDGE_IN4, OUTPUT);
pinMode(H_BRIDGE_ENB, OUTPUT);
// Commented out until I learn more
//pinMode(NANO_MOVE_LEFT_PIN, INPUT);
//pinMode(NANO_MOVE_RIGHT_PIN, INPUT);
}
///////////////////////////////////////////////////////////////////////
///
/// Method: Loop
///
/// Description: Test Code main loop
///
///////////////////////////////////////////////////////////////////////
void TestCode::Loop()
{
// I think this is for extra sensors we aren't using
// May be deleted
//static bool bWasAuto = false;
//static bool bWasManual = true;
//static bool bReadCenter = true;
// Select preferred driving mode by commenting out the unused one
if (USE_FS_I6_CONTROLLER)
{
ArcadeDriveFSi6();
//TankDriveFSi6();
} else {
// If no controller warn user through SM
Serial.println("No Controller Connected!");
}
}
///////////////////////////////////////////////////////////////////////
///
/// Method: ArcadeDriveFSi6
///
/// Description: Arcade drive style with FSi6
///
///////////////////////////////////////////////////////////////////////
void TestCode::ArcadeDriveFSi6()
{
// FSi6 Axis Inputs
// X = Ch1
// Y = Ch2
// 2000
// |
// 1000 --------- 2000
// |
// 1000
//
// Base Values (yAxis may need to be CH3)
double xAxis = pulseIn(CH1_INPUT_PIN, HIGH, PULSE_IN_TIMEOUT_US);
double yAxis = pulseIn(CH2_INPUT_PIN, HIGH, PULSE_IN_TIMEOUT_US);
// Serial Stuff for testing purposes
int c1 = pulseIn(CH1_INPUT_PIN, HIGH, PULSE_IN_TIMEOUT_US);
int c2 = pulseIn(CH2_INPUT_PIN, HIGH, PULSE_IN_TIMEOUT_US);
int c3 = pulseIn(CH3_INPUT_PIN, HIGH, PULSE_IN_TIMEOUT_US);
int c4 = pulseIn(CH4_INPUT_PIN, HIGH, PULSE_IN_TIMEOUT_US);
int c5 = pulseIn(CH5_INPUT_PIN, HIGH, PULSE_IN_TIMEOUT_US);
int c6 = pulseIn(CH6_INPUT_PIN, HIGH, PULSE_IN_TIMEOUT_US);
Serial.println(c1);
Serial.println(c2);
Serial.println(c3);
Serial.println(c4);
Serial.println(c5);
Serial.println(c6);
Serial.println();
delay(500);
return;
// How to pause controller
if ((xAxis == 0.0) || (yAxis == 0.0))
{
// Controller Off
return;
}
// Normalize numbers to make H-Bridge Happy
xAxis = ((xAxis - 1500.0)/(500.0));
yAxis = ((yAxis - 1500.0)/(500.0));
// Calculate motor outputs
double leftOutput = xAxis + yAxis;
double rightOutput = xAxis + yAxis;
SetMotorValues(leftOutput, rightOutput);
// Even more things for SM debugging
Serial.println("L, R");
Serial.print(leftOutput);
Serial.println(", ");
Serial.print(rightOutput);
delay(500);
}
///////////////////////////////////////////////////////////////////////
///
/// Method: TankDriveFSi6
///
/// Description: Tank drive style with FSi6
///
///////////////////////////////////////////////////////////////////////
void TestCode::TankDriveFSi6()
{
// Coming Soon... Maybe
}
///////////////////////////////////////////////////////////////////////
///
/// Method: SetMotorValues
///
/// Description: Sends motor vals to H-Bridge
///
///////////////////////////////////////////////////////////////////////
void TestCode::SetMotorValues(double leftOutput, double rightOutput)
{
// Output trimmed to -1/+1 to make H-Bridge even happier
leftOutput = (abs(leftOutput) > .10) ? 1.0 : leftOutput;
leftOutput = (abs(leftOutput) < .10) ? -1.0 : leftOutput;
rightOutput = (abs(rightOutput) > .10) ? 1.0 : rightOutput;
rightOutput = (abs(rightOutput) < .10) ? -1.0 : rightOutput;
// Limit output to be at least 10% power
leftOutput = (abs(leftOutput) < .10) ? 0.0 : leftOutput;
rightOutput = (abs(rightOutput) < .10) ? 0.0 : rightOutput;
// Scale up for analog reading and output
leftOutput *= 255.0;
rightOutput *= 255.0;
// Set H-Bridge IN pins to motor directions
// NOTE: set up for 2 motors per side?
// LEFT MOTOR(S)
if (leftOutput >= 0.0)
{
// Do stuff when left motor active
digitalWrite(H_BRIDGE_IN1, HIGH);
digitalWrite(H_BRIDGE_IN2, LOW);
// SM for Debug
Serial.println("1H, 2L");
} else {
// Do stuff when right motor active
digitalWrite(H_BRIDGE_IN1, LOW);
digitalWrite(H_BRIDGE_IN2, HIGH);
// SM for Debug
Serial.println("1L, 2H");
}
// RIGHT MOTOR(S)
if (rightOutput >= 0.0)
{
digitalWrite(H_BRIDGE_IN3, HIGH);
digitalWrite(H_BRIDGE_IN4, LOW);
// SM for Debug
Serial.println("3H, 4L");
} else {
digitalWrite(H_BRIDGE_IN3, LOW);
digitalWrite(H_BRIDGE_IN4, HIGH);
// SM for Debug
Serial.println("3L, 4H");
}
// Analog Pulses don't like negatives
// prolly too complicated heh silly computer
leftOutput = abs( leftOutput);
rightOutput = abs(rightOutput);
// Setup the H-Bridge to handle forward/reverse controls
analogWrite(H_BRIDGE_ENA, leftOutput);
analogWrite(H_BRIDGE_ENB, rightOutput);
// More SM stuff for Debugging
Serial.println("Left Output: " );
Serial.print( leftOutput);
Serial.println("Right Output: ");
Serial.print(rightOutput);
delay(500);
}
No comments:
Post a Comment