/** Singleton instance and entry into while loop that runs the desired program*/
public static void Main()
{
/* Tracking gamepad buttons states for single press captures*/
bool LastBtn2 = false;
bool LastBtn3 = false;
/* Forword/Backward Scalor */
const float kScalarX = 0.50f;
/* Left/Right Scalor */
const float kScalarY = 0.50f;
/* Turning Scalor */
const float kScalarTwist = 0.30f;
/* Ramp Rate */
const float kVoltageRampSec = 0.2f;
/* Configure Talons to operate in percentage VBus mode, and Ramp Up Voltage*/
foreach (CTRE.TalonSrx temp in Talons)
{
temp.SetControlMode(TalonSrx.ControlMode.kPercentVbus);
temp.SetVoltageRampRate(12.0f / kVoltageRampSec);
}
/* Clear DisplayModule */
LCDDisplay.ClearSprites();
/* Fill the screen with the target object (Top half magenta, bottom half green) */
LCDDisplay.AddRectSprite(CTRE.HERO.Module.DisplayModule.Color.Magenta, 0, 0, 160 / 2, 128);
LCDDisplay.AddRectSprite(CTRE.HERO.Module.DisplayModule.Color.Green, 160 / 2, 0, 160 / 2, 128);
while (true)
{
/* Keep robot enabled if gamepad is connected and in 'D' mode */
if (GamePad.GetConnectionStatus() == CTRE.UsbDeviceConnection.Connected)
{
CTRE.Watchdog.Feed();
}
/* Allow user to enable/disable Display Module backlight
* Button 2 (A) Disables the backlight
* Button 3 (B) Enables the backlight */
bool Btn2 = GamePad.GetButton(2);
bool Btn3 = GamePad.GetButton(3);
if (Btn2 & !LastBtn2)
{
LCDDisplay.BacklightEnable = false;
}
else if (Btn3 & !LastBtn3)
{
LCDDisplay.BacklightEnable = true;
}
LastBtn2 = Btn2;
LastBtn3 = Btn3;
/* Regular mecanum drive that is scaled and Gamepad joysticks have been adjusted */
float X = GamePad.GetAxis(0);
/* Invert gamepad so forward is truly forward */
float Y = -1 * GamePad.GetAxis(1);
float Twist = GamePad.GetAxis(2);
MecanumDrive(Y * kScalarY, X * kScalarX, Twist * kScalarTwist);
}
}
public Display()
{
cd_connected = new ChangeDetector();
cd_enabled = new ChangeDetector();
cd_voltage = new ChangeDetector();
cd_limit = new ChangeDetector();
cd_barrelStatus = new ChangeDetector[Shooter.NUMBER_OF_BARRELS];
cd_barrelPSI = new ChangeDetector[Shooter.NUMBER_OF_BARRELS];
for (int i = 0; i < Shooter.NUMBER_OF_BARRELS; i++)
{
cd_barrelStatus[i] = new ChangeDetector();
cd_barrelPSI[i] = new ChangeDetector();
}
DisplayModule _displayModule = new DisplayModule(CTRE.HERO.IO.Port1, DisplayModule.OrientationType.Portrait);
/* lets pick a font */
Font _bigFont = Properties.Resources.GetFont(Properties.Resources.FontResources.Freesans_bold_18);
int size = 20;
int f_y = 0;
_labelConnected = _displayModule.AddLabelSprite(_bigFont, DisplayModule.Color.White, 0, f_y, 128, size);
f_y += size;
_labelEnabled = _displayModule.AddLabelSprite(_bigFont, DisplayModule.Color.White, 0, f_y, 128, size);
f_y += size;
_labelVoltage = _displayModule.AddLabelSprite(_bigFont, DisplayModule.Color.White, 0, f_y, 128, size);
f_y += size;
_label_limit = _displayModule.AddLabelSprite(_bigFont, DisplayModule.Color.White, 0, f_y, 128, size);
_labelBarrelStatus = new DisplayModule.LabelSprite[Shooter.NUMBER_OF_BARRELS];
_labelBarrelPSI = new DisplayModule.LabelSprite[Shooter.NUMBER_OF_BARRELS];
f_y = 80;
_labelBarrelStatus[0] = _displayModule.AddLabelSprite(_bigFont, DisplayModule.Color.White, 0, f_y, 64, size);
_labelBarrelStatus[1] = _displayModule.AddLabelSprite(_bigFont, DisplayModule.Color.White, 64, f_y, 64, size);
f_y += size;
_labelBarrelPSI[0] = _displayModule.AddLabelSprite(_bigFont, DisplayModule.Color.White, 0, f_y, 64, size);
_labelBarrelPSI[1] = _displayModule.AddLabelSprite(_bigFont, DisplayModule.Color.White, 64, f_y, 64, size);
if (false)
{
// if we are Landscape, then DisplayHeight is the X screen dimension, and DisplayWidth is the Y dimension
for (int x = 0; x < _displayModule.DisplayWidth; x += 16)
{
_displayModule.AddRectSprite(DisplayModule.Color.White, x, 0, 1, _displayModule.DisplayHeight);
}
for (int y = 0; y < _displayModule.DisplayHeight; y += 16)
{
_displayModule.AddRectSprite(DisplayModule.Color.White, 0, y, _displayModule.DisplayWidth, 1);
}
}
RSL_1 = new OutputPort(CTRE.HERO.IO.Port5.Pin6, false);
}
public VerticalGauge(DisplayModule displayModule, int x, int y, int height, int width, DisplayModule.Color topCol, DisplayModule.Color btmCol)
{
_displayModule = displayModule;
_x = x;
_y = y;
_height = height;
_width = width;
_top = 0;
_btm = height;
_topCol = topCol;
_btmCol = btmCol;
_backRect = _displayModule.AddRectSprite(DisplayModule.Color.White, x, _y, width + 2, _top + _btm + 5);
_topRect = _displayModule.AddRectSprite(_topCol, _x + 1, _y + 1, _width, _top + 1);
_btmRect = _displayModule.AddRectSprite(_btmCol, _x + 1, _y + 1 + _top + 1, _width, _btm + 1);
}
public HorizGauge(DisplayModule displayModule, int x, int y, int height, int width, DisplayModule.Color topCol, DisplayModule.Color btmCol)
{
_displayModule = displayModule;
_x = x;
_y = y;
_height = height;
_width = width;
_left = 0;
_rght = width;
_leftCol = topCol;
_rghtCol = btmCol;
_backRect = _displayModule.AddRectSprite(DisplayModule.Color.White, x, _y, _left + _rght + 4, _height + 2);
_leftRect = _displayModule.AddRectSprite(_leftCol, _x + 1, _y + 1, _left + 1, _height);
_rghtRect = _displayModule.AddRectSprite(_rghtCol, _x + 1 + _left + 1, _y + 1, _rght + 1, _height);
}
public static void Main()
{
// Game Controller
GameController gamepad = new GameController(UsbHostDevice.GetInstance());
// NinaB Font
Font ninaB = Properties.Resources.GetFont(Properties.Resources.FontResources.NinaB);
// Initializing a display module: DisplayModule(port, orientation)
DisplayModule displayModule = new DisplayModule(CTRE.HERO.IO.Port8, DisplayModule.OrientationType.Landscape);
while (true)
{
// Connect the game controller first so that the sprites show up
if (gamepad.GetConnectionStatus() == UsbDeviceConnection.Connected)
{
// Erases everything on the display
displayModule.Clear();
// Adding labels: [Display Module Name].AddLabelSprite(font, colour, x_pos, y_pos, width, height)
DisplayModule.LabelSprite title = displayModule.AddLabelSprite(ninaB, DisplayModule.Color.White, 27, 17, 120, 15);
DisplayModule.LabelSprite x_label = displayModule.AddLabelSprite(ninaB, DisplayModule.Color.White, 80, 65, 80, 15);
DisplayModule.LabelSprite y_label = displayModule.AddLabelSprite(ninaB, DisplayModule.Color.White, 80, 85, 80, 15);
// Adding rectangles: [Display Module Name].AddRectSprite(colour, x_pos, y_pos, width, height)
DisplayModule.RectSprite x_rect = displayModule.AddRectSprite(DisplayModule.Color.White, 20, 55, 18, 55);
DisplayModule.RectSprite y_rect = displayModule.AddRectSprite(DisplayModule.Color.White, 47, 55, 18, 55);
// Everything gets cleared when the game controller is unplugged
while (gamepad.GetConnectionStatus() == UsbDeviceConnection.Connected)
{
// Declares and resets the joystick
double x_value = gamepad.GetAxis(0);
double y_value = -gamepad.GetAxis(1);
if (x_value < 0.05 && x_value > -0.05)
{
x_value = 0;
}
if (y_value < 0.05 && y_value > -0.05)
{
y_value = 0;
}
// Changes the color of the rectangle (x-value of the left joystick): [Rectangle Name].SetColor(colour)
if (x_value > 0.05)
{
x_rect.SetColor(DisplayModule.Color.Green);
}
else if (x_value < -0.05)
{
x_rect.SetColor(DisplayModule.Color.Red);
}
else
{
x_rect.SetColor(DisplayModule.Color.White);
}
// Changes the color of the rectangle (y-value of the left joystick): [Rectangle Name].SetColor(colour)
if (y_value > 0.05)
{
y_rect.SetColor(DisplayModule.Color.Green);
}
else if (y_value < -0.05)
{
y_rect.SetColor(DisplayModule.Color.Red);
}
else
{
y_rect.SetColor(DisplayModule.Color.White);
}
// Sets the text that the label displays: [Label Name].SetText(text: string)
title.SetText("Joystick Control");
x_label.SetText("X: " + x_value.ToString());
y_label.SetText("Y: " + y_value.ToString());
}
}
else
{
// Erases everything on the display
displayModule.Clear();
// Adding images: [Display Module Name].AddResourceImageSprite(resource_manager, img_ID, img_type, x_pos, y_pos)
DisplayModule.ResourceImageSprite image = displayModule.AddResourceImageSprite(Properties.Resources.ResourceManager, Properties.Resources.BinaryResources.img, Bitmap.BitmapImageType.Jpeg, 44, 16);
// Adding labels: [Display Module Name].AddLabelSprite(font, colour, x_pos, y_pos, width, height)
DisplayModule.LabelSprite text = displayModule.AddLabelSprite(ninaB, DisplayModule.Color.White, 36, 99, 100, 30);
// Sets the text that the label displays: [Label Name].SetText(text: string)
text.SetText("TAS Robotics");
// Keeps the image and text while the gamepad is unplugged
while (gamepad.GetConnectionStatus() == UsbDeviceConnection.NotConnected)
{
}
}
System.Threading.Thread.Sleep(100);
}
}
/** Singleton instance and entry into while loop that runs the desired program*/
public static void Main()
{
/* Tracking gamepad buttons states for single press captures*/
bool LastBtn1 = false;
bool LastBtn3 = false;
bool LastBtn2 = false;
/* Forword/Backward Scalor */
const float kScalarX = 0.50f;
/* Left/Right Scalor */
const float kScalarY = 0.50f;
/* Turning Scalor */
const float kScalarTwist = 0.30f;
/* Ramp Rate */
const float kVoltageRampSec = 0.25f;
/* Configure Talons to operate in percentage VBus mode, and Ramp Up Voltage*/
foreach (TalonSRX temp in Talons)
{
temp.Set(ControlMode.PercentOutput, 0);
temp.ConfigOpenloopRamp(kVoltageRampSec);
}
/* Clear DisplayModule */
LCDDisplay.ClearSprites();
LCDDisplay.BacklightEnable = false;
/* Fill the screen with the target object (Top half magenta, bottom half green) */
LCDDisplay.AddRectSprite(DisplayModule.Color.Magenta, 0, 0, 160 / 2, 128);
LCDDisplay.AddRectSprite(DisplayModule.Color.Green, 160 / 2, 0, 160 / 2, 128);
int colorDelay = 0;
int i = 0;
Boolean On = false;
Boolean LEDEnable = true;
while (true)
{
/* Keep robot enabled if gamepad is connected and in 'D' mode */
if (gamepad.GetConnectionStatus() == CTRE.Phoenix.UsbDeviceConnection.Connected)
{
CTRE.Phoenix.Watchdog.Feed();
}
/* Allow user to enable/disable Display Module backlight
* Button 2 (A) Disables the backlight
* Button 3 (B) Enables the backlight */
bool Btn1 = gamepad.GetButton(1);
bool Btn3 = gamepad.GetButton(3);
if (Btn1 & !LastBtn1)
{
LCDDisplay.BacklightEnable = false;
}
else if (Btn3 & !LastBtn3)
{
LCDDisplay.BacklightEnable = true;
}
LastBtn1 = Btn1;
LastBtn3 = Btn3;
/* Enable/Disable LED's */
bool Btn2 = gamepad.GetButton(2);
if (Btn2 & !LastBtn2)
{
LEDEnable = !LEDEnable;
}
LastBtn2 = Btn2;
if (gamepad.GetConnectionStatus() == CTRE.Phoenix.UsbDeviceConnection.Connected)
{
if (LEDEnable)
{
colorDelay++;
if (colorDelay >= 15)
{
_ColorSequencer.Process();
/* You can change the sequence in ColorSequencer.cs by ordering the premade colors or creating your own values */
_LEDStripController.Red = _ColorSequencer.Red;
_LEDStripController.Blue = _ColorSequencer.Blue;
_LEDStripController.Grn = _ColorSequencer.Green;
colorDelay = 0;
}
}
else
{
_LEDStripController.Red = 0;
_LEDStripController.Blue = 0;
_LEDStripController.Grn = 0;
}
}
if (gamepad.GetConnectionStatus() == CTRE.Phoenix.UsbDeviceConnection.NotConnected || _Battery.IsLow())
{
/* Flash RED ??? */
i++;
if (i >= 200)
{
On = !On;
i = 0;
}
/* Decide if strip is white or off */
if (On == true)
{
_LEDStripController.Red = 1;
_LEDStripController.Blue = 0;
_LEDStripController.Grn = 0;
}
else if (On == false)
{
_LEDStripController.Red = 0;
_LEDStripController.Blue = 0;
_LEDStripController.Grn = 0;
}
}
_LEDStripController.Process();
/* Regular mecanum drive that is scaled and Gamepad joysticks have been adjusted */
float X = gamepad.GetAxis(0);
/* Invert gamepad so forward is truly forward */
float Y = -1 * gamepad.GetAxis(1);
float Twist = gamepad.GetAxis(2);
MecanumDrive(Y * kScalarY, X * kScalarX, Twist * kScalarTwist);
}
}
public void DisplayOutput()
{
if (displayModeRefresh == false)
{
title = display.AddLabelSprite(ninaB, DisplayModule.Color.White, 34, 17, 120, 15);
label1 = display.AddLabelSprite(ninaB, DisplayModule.Color.White, 80, 56, 80, 15);
label2 = display.AddLabelSprite(ninaB, DisplayModule.Color.White, 80, 76, 80, 15);
label3 = display.AddLabelSprite(ninaB, DisplayModule.Color.White, 80, 96, 80, 15);
rect1 = display.AddRectSprite(DisplayModule.Color.White, 20, 55, 18, 55);
rect2 = display.AddRectSprite(DisplayModule.Color.White, 47, 55, 18, 55);
displayModeRefresh = true;
}
if (title == null || label1 == null || label2 == null || label3 == null || rect1 == null || rect2 == null)
{
display.Clear();
title = display.AddLabelSprite(ninaB, DisplayModule.Color.White, 34, 17, 120, 15);
label1 = display.AddLabelSprite(ninaB, DisplayModule.Color.White, 80, 55, 80, 15);
label2 = display.AddLabelSprite(ninaB, DisplayModule.Color.White, 80, 75, 80, 15);
label3 = display.AddLabelSprite(ninaB, DisplayModule.Color.White, 80, 95, 80, 15);
rect1 = display.AddRectSprite(DisplayModule.Color.White, 20, 55, 18, 55);
rect2 = display.AddRectSprite(DisplayModule.Color.White, 47, 55, 18, 55);
}
string colorStringX = "0x00";
string colorStringY = "0x00";
string redX, blueX, greenX;
string redY, blueY, greenY;
int colorIntX = (int)System.Math.Round(xAxisA * 100);
int colorIntY = (int)System.Math.Round(yAxisA * 100);
if (colorIntX > 0)
{
blueX = (100 - colorIntX).ToString("X2");
greenX = "ff";
redX = (100 - colorIntX).ToString("X2");
}
else if (colorIntX < 0)
{
blueX = (100 + colorIntX).ToString("X2");
greenX = (100 + colorIntX).ToString("X2");
redX = "ff";
}
else
{
blueX = "ff";
greenX = "ff";
redX = "ff";
}
if (colorIntY > 0)
{
blueY = (100 - colorIntY).ToString("X2");
greenY = "ff";
redY = (100 - colorIntY).ToString("X2");
}
else if (colorIntY < 0)
{
blueY = (100 + colorIntY).ToString("X2");
greenY = (100 + colorIntY).ToString("X2");
redY = "ff";
}
else
{
blueY = "ff";
greenY = "ff";
redY = "ff";
}
colorStringX = colorStringX + blueX + greenX + redX;
colorStringY = colorStringY + blueY + greenY + redY;
rect1.SetColor((DisplayModule.Color)Convert.ToInt32(colorStringX, 16));
rect2.SetColor((DisplayModule.Color)Convert.ToInt32(colorStringY, 16));
label1.SetText("X: " + xAxisA.ToString());
label2.SetText("Y: " + yAxisA.ToString());
label3.SetText("Angle: " + ((int)System.Math.Round(angle)).ToString());
title.SetText("Motor Control");
}