Driving during Autonomous

I’m having trouble developing a code in which the robot will simply drive forward until it reaches the center peg during autonomous. I currently have two options but this is mostly due to me trying different code. I have rewritten the values but cannot figure out how to make the robot drive straight! This seems to be a simple task but no one on the team has much experience with programming. With what I have now, choosing option 1 in the SmartDashboard results in the robot spinning in one place while option 2 does nothing. Here is what I have written thus far for autonomous:

void RobotInit()
	    {
	        SmartDashboard::PutNumber("auton", 2);
	    }


	    void Autonomous()
	    {
	        autonomous = SmartDashboard::GetNumber("auton", 0);

	        if(autonomous == 1)
	                {
	                    cout << "ONE!";
	                    left_0.Set(leftPolarity*LinearValueEstimator(-0.5, 0.0));
	                    right_0.Set(leftPolarity*LinearValueEstimator(0.5, 0.0));
	                    left_1.Set(leftPolarity*LinearValueEstimator(-0.5, 0.0));
	                    right_1.Set(leftPolarity*LinearValueEstimator(0.5, 0.0));
	                    Wait(2);
	                    left_0.StopMotor();
	                    left_1.StopMotor();
	                    right_0.StopMotor();
	                    right_1.StopMotor();

	                }
	                else if(autonomous == 2)
	                {
	                    cout << "TWO!";
	                    left_0.Set(leftPolarity*LinearValueEstimator(-0.50, 0.0));
	                    right_0.Set(leftPolarity*LinearValueEstimator(0.50, 0.0));
	                    left_1.Set(leftPolarity*LinearValueEstimator(-0.50, 0.0));
	                    right_1.Set(leftPolarity*LinearValueEstimator(0.50, 0.0));
	                    Wait(2.2);
	                    left_0.StopMotor();
	                    left_1.StopMotor();
	                    right_0.StopMotor();
	                    right_1.StopMotor();
	        }
	        else{cout << "NONE!";}
	    }

Please let me know what can be done to fix this code! :slight_smile:

The behavior you describe is that left/right motors are spinning in the same direction (spin turn) or against each other (stopped).

From only the code, it’s not quite clear how the motors are wired, and this may be the issue. I would start with this. If left_0 and left_1 aren’t quite the motors you think they are, it would exhibit some of the problems you report. Wring out the wiring unless you are certain.

It is not evident how the motors are mechanically ganged together. The assumption is that they are both driving in the same direction–but we cannot see this from just inspection of the code. If there is an additional gear on one of the motors, one would be flipped.

In order to simplify the code, I would remove any additional variables being passed to the Set() commands. Pass in a simple 1.0 and -1.0 for the motor speeds instead of variables and additional methods. I would expect this to be the most likely cause of any problems–that what is being passed into the Set() is not quite what you think. Or print out your inputs.

And it is not necessary to drive both motors for initial testing. This can be very useful during bring-up and verifying that a motor is working. It is perfectly OK to do a Set(1.0) on only one of the left or right motors at a time for short periods to ensure it is working in the correct direction.

It is a lot easier to bring up functionality a little piece at a time to gain confidence in the system as opposed to all four motors at one time. This is generally a good practice for bringing up any new subsystem.

Hope this helps give you some ideas.