92 lines
2.2 KiB
C++
92 lines
2.2 KiB
C++
#include "WPILib.h"
|
|
#include "TankDrive.h"
|
|
|
|
/**
|
|
* This sample shows how to use the new CANTalon to just run a motor in a basic
|
|
* throttle mode, in the same manner as you might control a traditional PWM
|
|
* controlled motor.
|
|
*
|
|
*/
|
|
class Robot : public IterativeRobot {
|
|
CANTalon r1_drive;
|
|
CANTalon r2_drive;
|
|
CANTalon l1_drive;
|
|
CANTalon l2_drive;
|
|
RobotDrive drivetrain1;
|
|
RobotDrive drivetrain2;
|
|
|
|
Joystick lstick;
|
|
|
|
// update every 0.01 seconds/10 milliseconds.
|
|
// The talon only receives control packets every 10ms.
|
|
//double kUpdatePeriod = 0.010;
|
|
|
|
public:
|
|
Robot()
|
|
: r1_drive(1), // Initialize the Talon as device 1. Use the roboRIO web
|
|
r2_drive(2), // interface to change the device number on the talons.
|
|
l1_drive(3),
|
|
l2_drive(4),
|
|
drivetrain1(l1_drive, r1_drive),
|
|
drivetrain2(l2_drive, r2_drive),
|
|
lstick(0)
|
|
{
|
|
printf("Initializing Robot...");
|
|
printf("Initializing TankDrive");
|
|
//drive = new TankDrive(&l1_drive, &l2_drive, &r1_drive, &r2_drive);
|
|
printf("Ready!");
|
|
//l1_drive.SetInverted(true);
|
|
//l2_drive.SetInverted(true);
|
|
//l1_drive.Set(0, 0);
|
|
//l2_drive.Set(0, 0);
|
|
//r1_drive.Set(0, 1);
|
|
//r2_drive.Set(0, 1);
|
|
}
|
|
|
|
void DisabledInit()
|
|
{
|
|
r1_drive.SetSafetyEnabled(false);
|
|
r2_drive.SetSafetyEnabled(false);
|
|
l1_drive.SetSafetyEnabled(false);
|
|
l2_drive.SetSafetyEnabled(false);
|
|
}
|
|
|
|
void TeleopInit()
|
|
{
|
|
printf("Starting Teleop mode");
|
|
r1_drive.Enable();
|
|
r2_drive.Enable();
|
|
l1_drive.Enable();
|
|
l2_drive.Enable();
|
|
|
|
|
|
|
|
}
|
|
|
|
void TeleopPeriodic()
|
|
{
|
|
printf("Teleop Periodic Start.");
|
|
float speed = lstick.GetY() * (lstick.GetThrottle());
|
|
float rot = (-1) * lstick.GetTwist();
|
|
|
|
drivetrain1.ArcadeDrive(speed, rot, false);
|
|
drivetrain2.ArcadeDrive(speed, rot, false);
|
|
printf("~Teleop Periodic End.");
|
|
}
|
|
/**
|
|
* Runs the motor from the output of a Joystick.
|
|
*
|
|
void OperatorControl() {
|
|
while (IsOperatorControl() && IsEnabled()) {
|
|
// Do Drive.
|
|
std::cerr << "In Operator Control";
|
|
drive->Drive(&m_stick);
|
|
Wait(kUpdatePeriod); // Wait a bit so that the loop doesn't lock everything up.
|
|
}
|
|
}*/
|
|
|
|
//TankDrive *drive;
|
|
};
|
|
|
|
START_ROBOT_CLASS(Robot)
|