What type of drive are you using? (Tank, Mechanum, Swerve)
Are analog encoders attached to all the wheels?
Are you using Talon SRX or another motor controller?
Do you have a Gyro sensor on your robot (It is easier to use for rotating to angle).
Making senseless computers do
intelligent real world things
is NOT easy. SO MAKE IT FUN!