Slightly more sane, using IterativeRobot and DriveTrain.

This commit is contained in:
Jason Cox 2016-01-28 16:47:25 -05:00
parent 3d0cb6be07
commit 6d903af729

View File

@ -12,8 +12,10 @@ class Robot : public IterativeRobot {
CANTalon r2_drive;
CANTalon l1_drive;
CANTalon l2_drive;
RobotDrive drivetrain1;
RobotDrive drivetrain2;
Joystick m_stick;
Joystick lstick;
// update every 0.01 seconds/10 milliseconds.
// The talon only receives control packets every 10ms.
@ -25,17 +27,28 @@ public:
r2_drive(2), // interface to change the device number on the talons.
l1_drive(3),
l2_drive(4),
m_stick(0)
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);
//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()
@ -46,12 +59,18 @@ public:
l1_drive.Enable();
l2_drive.Enable();
}
void TeleopPeriodic()
{
printf("Teleop Periodic Start.");
drive->Drive(&m_stick);
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.");
}
/**
@ -66,7 +85,7 @@ public:
}
}*/
TankDrive *drive;
//TankDrive *drive;
};
START_ROBOT_CLASS(Robot)