//static AnalogInput potentiometer0 = new AnalogInput(CTRE.HERO.IO.Port1.Analog_Pin5);
public static void Main()
{
//double analogRead0;
/* simple counter to print and watch using the debugger */
//int counter = 0;
/* loop forever */
//static System.IO.Ports.SerialPort _uart
// System.IO.Ports.SerialPort(CTRE.HERO.IO.Port1.UART, 115200);
CTRE.Phoenix.Controller.GameController myGamepad = new
CTRE.Phoenix.Controller.GameController(new CTRE.Phoenix.UsbHostDevice(0));
CTRE.Phoenix.MotorControl.CAN.VictorSPX motor0 = new
CTRE.Phoenix.MotorControl.CAN.VictorSPX(0);
CTRE.Phoenix.MotorControl.CAN.VictorSPX motor1 = new
CTRE.Phoenix.MotorControl.CAN.VictorSPX(1);
CTRE.Phoenix.MotorControl.CAN.VictorSPX motor2 = new
CTRE.Phoenix.MotorControl.CAN.VictorSPX(2);
CTRE.Phoenix.MotorControl.CAN.TalonSRX motor3 = new
CTRE.Phoenix.MotorControl.CAN.TalonSRX(3);
while (true)
{
/* print the three analog inputs as three columns */
//analogRead0 = potentiometer0.Read();
//Debug.Print("Motor 0 position: " + analogRead0);
if (myGamepad.GetConnectionStatus() == CTRE.Phoenix.UsbDeviceConnection.Connected)
//if ((analogRead0 > 0)&&(analogRead0 < .400))
{
Debug.Print("axis:" + myGamepad.GetAxis(1));
motor1.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, myGamepad.GetAxis(1) / 2);
motor2.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, myGamepad.GetAxis(2) / 1.33);
//motor3.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, 1);
//motor1.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput,.5);
//analogRead0 = potentiometer0.Read();
//Debug.Print("Motor 0 position: " + analogRead0);
CTRE.Phoenix.Watchdog.Feed();
//if ((analogRead0 > .570) && (analogRead0 < .575))
// {
//motor1.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, 1);
//}
//else
//{
// motor1.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, 0);
//}
//CTRE.Phoenix.Watchdog.Feed();
}
//Debug.Print("Counter Value: " + counter);
/* increment counter */
// ++counter; /* try to land a breakpoint here and hover over 'counter' to see it's current value. Or add it to the Watch Tab */
/* wait a bit */
System.Threading.Thread.Sleep(10);
}
}
public static void Main()
{
/* Start Init */
//robotInit();
CTRE.Phoenix.MotorControl.CAN.TalonSRX driveTalon1 = new CTRE.Phoenix.MotorControl.CAN.TalonSRX(0);
CTRE.Phoenix.MotorControl.CAN.TalonSRX driveTalon2 = new CTRE.Phoenix.MotorControl.CAN.TalonSRX(0);
CTRE.Phoenix.Controller.GameController gamepad =
new CTRE.Phoenix.Controller.GameController(new CTRE.Phoenix.UsbHostDevice(0));
CTRE.Phoenix.Controller.GameControllerValues gv = new CTRE.Phoenix.Controller.GameControllerValues();
gamepad.GetAllValues(ref gv);
Intake intake = new Intake();
Conveyor conveyor = new Conveyor();
Climber climber = new Climber();
Drive drive = new Drive(driveTalon1, driveTalon2, gamepad);
TeleopStateMachine tsm = new TeleopStateMachine(intake, conveyor, climber, gamepad);
/* Start Auton */
/* Start Teleop */
/* simple counter to print and watch using the debugger */
int counter = 0;
/* loop forever */
while (true)
{
/* print the three analog inputs as three columns */
Debug.Print("Counter Value: " + counter);
/* increment counter */
++counter; /* try to land a breakpoint here and hover over 'counter' to see it's current value. Or add it to the Watch Tab */
/* wait a bit */
System.Threading.Thread.Sleep(100);
//TeleopStateMachine.moveRight();
}
}
public static void Main()
{
/* simple counter to print and watch using the debugger */
//int counter = 0;
// Controller Object
CTRE.Phoenix.Controller.GameController myGamepad = new CTRE.Phoenix.Controller.GameController(new CTRE.Phoenix.UsbHostDevice(0));
// Talon Object
CTRE.Phoenix.MotorControl.CAN.TalonSRX myTalon = new CTRE.Phoenix.MotorControl.CAN.TalonSRX(55);
/* loop forever */
while (true)
{
if (myGamepad.GetConnectionStatus() == CTRE.Phoenix.UsbDeviceConnection.Connected)
{
Debug.Print("Axis 0:" + myGamepad.GetAxis(0));
Debug.Print("Axis 1:" + myGamepad.GetAxis(1));
Debug.Print("Axis 2:" + myGamepad.GetAxis(2));
Debug.Print("Axis 5:" + myGamepad.GetAxis(5));
// pass axis value to talon
myTalon.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, myGamepad.GetAxis(1));
CTRE.Phoenix.Watchdog.Feed();
}
/* print the three analog inputs as three columns */
//Debug.Print("Counter Value: " + counter);
/* increment counter */
//++counter; /* try to land a breakpoint here and hover over 'counter' to see it's current value. Or add it to the Watch Tab */
/* wait a bit */
System.Threading.Thread.Sleep(10);
}
}
public Conveyor()
{
CTRE.Phoenix.MotorControl.CAN.TalonSRX conveyorTalon = new CTRE.Phoenix.MotorControl.CAN.TalonSRX(conveyorTalonID);
}
public Battery(CTRE.Phoenix.MotorControl.CAN.TalonSRX talon)
{
_talon = talon;
}
public Intake()
{
CTRE.Phoenix.MotorControl.CAN.TalonSRX intakeTalon1 = new CTRE.Phoenix.MotorControl.CAN.TalonSRX(intakeTalon1ID);
CTRE.Phoenix.MotorControl.CAN.TalonSRX intakeTalon2 = new CTRE.Phoenix.MotorControl.CAN.TalonSRX(intakeTalon2ID);
}
/** entry point of the application */
public static void Main()
{
/* Here, we initialize and set the direction of each Talon (is 100 backwards or forwards?) */
for (int i = 0; i < myTalon.Length; i++)
{
int curTalonID = i + talon_id_offset;
myTalon[i] = new CTRE.Phoenix.MotorControl.CAN.TalonSRX(curTalonID);
/* The Talons of ID 2 and 4 are the left wheel motors, must invert them! */
if (curTalonID == 2 + talon_id_offset || curTalonID == 4 + talon_id_offset)
{
myTalon[i].SetInverted(true);
}
}
/* temporary array */
byte[] scratch = new byte[PKG_LENGTH * 2];
_uart = new System.IO.Ports.SerialPort(CTRE.HERO.IO.Port1.UART, 115200);
_uart.Open();
byte[] packet = new byte[PKG_LENGTH - 2];
// current packet
byte[] curPacket = new byte[PKG_LENGTH - 2];
// dont actually need to assign the check sum
for (int i = 0; i < curPacket.Length - 1; i++)
{
curPacket[i] = offset;
}
while (true)
{
/* read bytes out of uart */
if (_uart.BytesToRead > 0)
{
int readCnt = _uart.Read(_rx, 0, CalcRemainingCap());
for (int i = 0; i < readCnt; ++i)
{
PushByte(_rx[i]);
}
}
// a complete packet is guaranteed to exist in the buffer if buffer length > 2 * pkg length
if (_txCnt >= PKG_LENGTH * 2)
{
for (int i = 0; i < scratch.Length; i++)
{
scratch[i] = PopByte();
}
bool hasMsg = false;
for (int i = 0; i < scratch.Length; i++)
{
// if headers match, copy the next PKG_LENGTH-2 bytes to packet
if (scratch[i] == H1 && scratch[i + 1] == H2 && i <= PKG_LENGTH)
{
for (int j = 0; j < packet.Length; j++)
{
// add 2 because we dont need the header
packet[j] = scratch[i + 2 + j];
}
hasMsg = true;
break;
}
}
if (hasMsg)
{
if (CheckValidity(packet))
{
// copy the valid packet to curPacket which is continuously fed to the Talons
Array.Copy(packet, curPacket, PKG_LENGTH - 2);
// Debug.Print("Valid data:" + packet[0].ToString());
}
}
}
WriteToTalon(curPacket);
System.Threading.Thread.Sleep(20);
}
}
public static void Main()
{
/* create a gamepad object */
CTRE.Phoenix.Controller.GameController myGamepad = new CTRE.Phoenix.Controller.GameController(new CTRE.Phoenix.UsbHostDevice(0));
/* create a talon, the Talon Device ID in HERO LifeBoat is zero */
CTRE.Phoenix.MotorControl.CAN.TalonSRX myTalon = new CTRE.Phoenix.MotorControl.CAN.TalonSRX(1);
CTRE.Phoenix.MotorControl.CAN.TalonSRX myTalon2 = new CTRE.Phoenix.MotorControl.CAN.TalonSRX(2);
CTRE.Phoenix.MotorControl.CAN.TalonSRX myTalon3 = new CTRE.Phoenix.MotorControl.CAN.TalonSRX(25);
float speed;
float turn;
float avg;
bool rampDown = false;
bool rampUp = false;
int c = 0;
var startTime = DateTime.UtcNow;
while (DateTime.UtcNow - startTime < TimeSpan.FromTicks(50000000))
{
myTalon3.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, 1);
Debug.Print("Test: " + c++);
/* allow motor control */
CTRE.Phoenix.Watchdog.Feed();
}
startTime = DateTime.UtcNow;
while (DateTime.UtcNow - startTime < TimeSpan.FromTicks(50000000))
{
myTalon3.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, -1);
Debug.Print("Test: " + c++);
CTRE.Phoenix.Watchdog.Feed();
}
/* wait a bit */
System.Threading.Thread.Sleep(50);
/* loop forever */
while (true)
{
/* added inside the while loop */
if (myGamepad.GetConnectionStatus() == CTRE.Phoenix.UsbDeviceConnection.Connected)
{
//If you want to change it to right bumper use get.axis(2) and (5)
speed = myGamepad.GetAxis(1);
turn = myGamepad.GetAxis(0);
avg = speed / 2 + turn / 2;
rampDown = myGamepad.GetButton(1);
rampUp = myGamepad.GetButton(2);
/* print the axis value */
//Turning is overriding the speed
Debug.Print("axis:" + myGamepad.GetAxis(0) + ", " + myGamepad.GetAxis(1) + ", " + myGamepad.GetAxis(2) + ", " + myGamepad.GetAxis(5));
Debug.Print("speed:" + speed);
Debug.Print("turn:" + turn);
Debug.Print("Button 1(DOWN): " + rampDown);
Debug.Print("Button 2(UP): " + rampUp);
/* pass axis value to talon */
// myTalon.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, speed);
// myTalon2.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, speed);
//myTalon.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, turn);
//myTalon2.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, turn * -1);
myTalon.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, avg * -1);
myTalon2.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, avg);
if (rampDown)
{
myTalon3.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, -1.0);
}
else if (rampUp)
{
myTalon3.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, 1.0);
}
else
{
myTalon3.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, 0);
}
//once button is pressed move motor specific amount of times so that the ramp rests at a 90 degree angle
/* allow motor control */
CTRE.Phoenix.Watchdog.Feed();
}
/* wait a bit */
System.Threading.Thread.Sleep(50);
}
}
