Our team this year switched to using the v5 rotation sensors this year and after we switched we wrote our code and when we go to run it our robot doesn’t move our code is below and we tried with both rotations and degrees as the units of measure.
//-----------------------------------------------------------------
//For going long distances
//-----------------------------------------------------------------
void drivelong(int ti,int dist){
//RESET
Brain.Timer.reset();
double ipd = 10.21;
int dist2 = dist / ipd;
blr.setPosition(0, rev);
flr.setPosition(0, rev);
brr.setPosition(0, rev);
frr.setPosition(0, rev);
if(dist > 0){
while(ti > Brain.Timer.value() && ((blr.position(rev) + flr.position(rev) + brr.position(rev) + frr.position(rev))/4) < dist2){
int slowdown = (dist2/14) * 11.9;
int speedup = (dist2/14) * 3.2;
if(((blr.position(rev) + flr.position(rev) + brr.position(rev) + frr.position(rev))/4) < slowdown && ((blr.position(rev) + flr.position(rev) + brr.position(rev) + frr.position(rev))/4) > speedup){
left1.spin(directionType::fwd, 40,velocityUnits::pct);
left2.spin(directionType::rev, 40,velocityUnits::pct);
left3.spin(directionType::rev, 40,velocityUnits::pct);
right1.spin(directionType::rev, 40,velocityUnits::pct);
right2.spin(directionType::fwd, 40,velocityUnits::pct);
right3.spin(directionType::fwd, 40,velocityUnits::pct);
}
if(((blr.position(rev) + flr.position(rev) + brr.position(rev) + frr.position(rev))/4) > slowdown || ((blr.position(rev) + flr.position(rev) + brr.position(rev) + frr.position(rev))/4) < speedup){
left1.spin(directionType::fwd, 30,velocityUnits::pct);
left2.spin(directionType::rev, 30,velocityUnits::pct);
left3.spin(directionType::rev, 30,velocityUnits::pct);
right1.spin(directionType::rev, 30,velocityUnits::pct);
right2.spin(directionType::fwd, 30,velocityUnits::pct);
right3.spin(directionType::fwd, 30,velocityUnits::pct);
}
}
}
if ((blr.position(rev) + flr.position(rev) + brr.position(rev) + frr.position(rev))/4 <= dist2) {
left1.setStopping(brake);
left2.setStopping(brake);
left3.setStopping(brake);
right1.setStopping(brake);
right2.setStopping(brake);
right3.setStopping(brake);
left1.stop();
left2.stop();
left3.stop();
right1.stop();
right2.stop();
right3.stop();
}
}