void tug(int to,int deg,int spe){
/*
to = timeout
deg = degres you want to turn + for right - for left
spe = speed
*/
//resets all sensors / integers
int deg2 = 0;
Brain.Timer.reset();
Inertial.calibrate();
Inertial.setHeading(0, degrees);
if(deg < 0){
//for going left you need to find the distence from 360
deg2 = deg - 360;
FrontL.spin(directionType::fwd, spe,velocityUnits::pct);
BackL.spin(directionType::fwd, spe,velocityUnits::pct);
FrontR.spin(directionType::fwd, spe,velocityUnits::pct);
BackR.spin(directionType::fwd, spe,velocityUnits::pct);
wait(90,msec);
//loop till the gyro states that is withen the deadzone
while(to > Brain.Timer.value() && (Inertial.heading(degrees) > (deg2 + 4) || Inertial.heading(degrees) < (deg2 - 4))){
Controller1.Screen.clearScreen();
Controller1.Screen.print(Inertial.heading(degrees));
wait(60,msec);
// telling the robot to ether turn left or right to get to where it needs to go
if(Inertial.heading(degrees) < (deg2 - 1)){
//turn right
FrontL.spin(directionType::rev, spe,velocityUnits::pct);
BackL.spin(directionType::rev, spe,velocityUnits::pct);
FrontR.spin(directionType::rev, spe,velocityUnits::pct);
BackR.spin(directionType::rev, spe,velocityUnits::pct);
}else if(Inertial.heading(degrees) > (deg2 + 1)){
//turn left
FrontL.spin(directionType::fwd, spe,velocityUnits::pct);
BackL.spin(directionType::fwd, spe,velocityUnits::pct);
FrontR.spin(directionType::fwd, spe,velocityUnits::pct);
BackR.spin(directionType::fwd, spe,velocityUnits::pct);
}
wait(50,msec);
}
}else if(deg > 0){
while(to > Brain.Timer.value() && (Inertial.heading(degrees) > (deg2 + 2) || Inertial.heading(degrees) < (deg2 - 2))){
if(Inertial.heading(degrees) < (deg - 1)){
//turn right
FrontL.spin(directionType::fwd, spe,velocityUnits::pct);
BackL.spin(directionType::fwd, spe,velocityUnits::pct);
FrontR.spin(directionType::fwd, spe,velocityUnits::pct);
BackR.spin(directionType::fwd, spe,velocityUnits::pct);
}else if(Inertial.heading(degrees) > (deg + 1)){
//turn right
FrontL.spin(directionType::rev, spe,velocityUnits::pct);
BackL.spin(directionType::rev, spe,velocityUnits::pct);
FrontR.spin(directionType::rev, spe,velocityUnits::pct);
BackR.spin(directionType::rev, spe,velocityUnits::pct);
}
}
}
}
So recently I have had a struggle with being able to get my inertia sensor to work with turns with the code I currently am showing yo my robot does not move at all would anyone be able to help/work with me to figure out this problem so could use this at my next competition . Or if anyone thinks it would be easier to use the three wire encoders that I use for everything else