Code:
48void DriveBase::AutoDriveTurn(float speed, float angle){
49 while(drive_gyro->GetAngle() >= angle){
50 fl_motor->Set(speed);
51 fr_motor->Set(speed);
52 bl_motor->Set(speed);
53 br_motor->Set(speed);
54 }
55 }
This function is going to cause you problems. This is called a "tight loop" and will consume all of your CPU power while blocking your main thread. Because there is no delay the CPU is going to execute the loop as fast as it possibly can. It will likely take the CPU to 100% and then start dropping control packets from the driver station. On top of that while you are in the loop none of your other commands or code in the main thread will be running.
The better way to do this that won't cause you problems is to use a command.
Code:
32 class AutoDriveTurnCommand() {
33 private:
34 double mSpeed;
35 double mAngle;
36 public:
37 AutoDriveTurnCommand(double speed, double angle) {
38 mSpeed = speed;
39 mAngle = angle;
40 }
41 void Initialize() {
42 Robot::drivebase->DriveTank(speed, speed);
43 }
44 void Execute() {}
45 bool IsFinished() {
46 return Robot::drivebase->GetGyroAngle() < mAngle;
47 }
48 void End() {
49 Robot::drivebase->DriveTank(0, 0);
50 }
51 void Interrupted() {
52 End();
53 }
54 };
For this to work you will need to define a function in DriveBase called GetGyroAngle that returns the angle of the gyro.