r
This commit is contained in:
parent
5f8419afad
commit
77301633a9
226
src/main.cpp
226
src/main.cpp
|
|
@ -300,19 +300,43 @@ void autonomous() {
|
|||
if (autonpick == 0) { // Auton #1, used for adding tribal to goal
|
||||
float x = -2.7;
|
||||
drive->moveDistanceAsync(x * 1_ft);
|
||||
pros::delay(2500);
|
||||
drive->moveDistance(0.5_ft);
|
||||
double yaw = IMU.get_heading();
|
||||
std::printf("yaw: %f\n", yaw);
|
||||
// double turn = -92 - yaw;
|
||||
double turn = 45 - yaw;
|
||||
std:
|
||||
printf("turn: %f\n", turn);
|
||||
drive->setMaxVelocity(300);
|
||||
drive->turnAngle(turn * 1_deg);
|
||||
drive->moveDistanceAsync(4.1_ft);
|
||||
pros::delay(3000);
|
||||
pros::delay(1500);
|
||||
drive->moveDistance(0.7_ft);
|
||||
pros::delay(100);
|
||||
|
||||
pros::Distance distforward(17);
|
||||
|
||||
// double yaw = IMU.get_heading();
|
||||
// std::printf("yaw: %f\n", yaw);
|
||||
// double turn = -92 - yaw;
|
||||
// double turn = 53 - yaw;
|
||||
// std:: printf("turn: %f\n", turn);
|
||||
// drive->setMaxVelocity(300);
|
||||
// drive->turnAngle(turn * 1_deg);
|
||||
bool imuholdarray[10];
|
||||
|
||||
while (!is_held(imuholdarray, (abs(65.0 - IMU.get_rotation()) < 1))) {
|
||||
drive->getModel()->rotate((65.0 - IMU.get_rotation()) * 0.008);
|
||||
pros::delay(10);
|
||||
}
|
||||
drive->stop();
|
||||
|
||||
drive->getModel()->stop();
|
||||
while (distforward.get() > 150) {
|
||||
drive->getModel()->curvature(12000,
|
||||
(IMU.get_rotation() - 65.0) * -0.04);
|
||||
|
||||
// printf("%i\n", distforward.get());
|
||||
pros::delay(10);
|
||||
}
|
||||
|
||||
drive->getModel()->stop();
|
||||
|
||||
pros::delay(700);
|
||||
piston.set_value(true);
|
||||
drive->setMaxVelocity(150);
|
||||
drive->getModel()->setBrakeMode(okapi::AbstractMotor::brakeMode::hold);
|
||||
drive->moveDistance(5_in);
|
||||
} else if (autonpick == 1 ||
|
||||
autonpick ==
|
||||
2) { // Auton #2, Used for removing triball from match loader
|
||||
|
|
@ -323,13 +347,34 @@ void autonomous() {
|
|||
pros::delay(1000);
|
||||
piston.set_value(false);
|
||||
// drive->moveDistance(-1.7_ft);
|
||||
drive->turnAngle(115_deg);
|
||||
drive->moveDistance(3.5_ft);
|
||||
drive->turnAngle(95_deg);
|
||||
//drive->turnAngle(115_deg);
|
||||
bool imuholdarray[10];
|
||||
while (!is_held(imuholdarray,
|
||||
(abs(115.0 - IMU.get_rotation()) < 3))) {
|
||||
drive->getModel()->rotate((115.0 - IMU.get_rotation()) *
|
||||
0.004);
|
||||
pros::delay(10);
|
||||
}
|
||||
|
||||
drive->getModel()->stop();
|
||||
|
||||
drive->moveDistance(3.69_ft);
|
||||
|
||||
//drive->turnAngle(95_deg);
|
||||
|
||||
while (!is_held(imuholdarray,
|
||||
(abs(225.0 - IMU.get_rotation()) < 3))) {
|
||||
drive->getModel()->rotate((225.0 - IMU.get_rotation()) *
|
||||
0.004);
|
||||
pros::delay(10);
|
||||
}
|
||||
|
||||
drive->getModel()->stop();
|
||||
|
||||
drive->setMaxVelocity(50);
|
||||
drive->moveDistanceAsync(2_ft);
|
||||
pros::delay(2500);
|
||||
piston.set_value(true);
|
||||
drive->moveDistanceAsync(1.7_ft);
|
||||
pros::delay(2500);
|
||||
drive->moveDistance(4_in);
|
||||
}
|
||||
|
||||
|
|
@ -602,12 +647,13 @@ void autonomous() {
|
|||
|
||||
drive->setMaxVelocity(600);
|
||||
|
||||
// while (!is_held(imuholdarray, (abs(100.0 - IMU.get_rotation()) < 1))) {
|
||||
// while (!is_held(imuholdarray, (abs(100.0 - IMU.get_rotation()) < 1)))
|
||||
// {
|
||||
// drive->getModel()->rotate((100.0 - IMU.get_rotation()) * 0.008);
|
||||
// pros::delay(10);
|
||||
// }
|
||||
|
||||
drive->turnAngle(100_deg);
|
||||
drive->turnAngle(100_deg);
|
||||
|
||||
drive->getModel()->stop();
|
||||
|
||||
|
|
@ -615,18 +661,19 @@ void autonomous() {
|
|||
|
||||
drive->moveDistanceAsync(2_ft);
|
||||
|
||||
pros::delay(1000);
|
||||
pros::delay(700);
|
||||
|
||||
drive->moveDistance(-1_ft);
|
||||
|
||||
while (!is_held(imuholdarray, (abs(-110.0 - IMU.get_rotation()) < 3))) {
|
||||
drive->getModel()->rotate((-110.0 - IMU.get_rotation()) * 0.008);
|
||||
drive->getModel()->rotate((-110.0 - IMU.get_rotation()) * 0.004);
|
||||
pros::delay(10);
|
||||
}
|
||||
|
||||
drive->getModel()->stop();
|
||||
while (distforward.get() > 300) {
|
||||
drive->getModel()->curvature(12000, (IMU.get_rotation()+110) * -0.04);
|
||||
drive->getModel()->stop();
|
||||
while (distforward.get() > 300) {
|
||||
drive->getModel()->curvature(12000,
|
||||
(IMU.get_rotation() + 110) * -0.04);
|
||||
|
||||
// printf("%i\n", distforward.get());
|
||||
pros::delay(10);
|
||||
|
|
@ -635,44 +682,133 @@ void autonomous() {
|
|||
funnypiston.set_value(true);
|
||||
pros::delay(500);
|
||||
|
||||
drive->moveDistance(-1_ft);
|
||||
drive->moveDistance(-1_ft);
|
||||
|
||||
while (!is_held(imuholdarray, (abs(75.0 - IMU.get_rotation()) < 3))) {
|
||||
drive->getModel()->rotate((75.0 - IMU.get_rotation()) * 0.008);
|
||||
while (!is_held(imuholdarray, (abs(85.0 - IMU.get_rotation()) < 3))) {
|
||||
drive->getModel()->rotate((85.0 - IMU.get_rotation()) * 0.004); // 98
|
||||
pros::delay(10);
|
||||
}
|
||||
|
||||
drive->getModel()->stop();
|
||||
drive->getModel()->stop();
|
||||
|
||||
funnypiston.set_value(false);
|
||||
funnypiston.set_value(false);
|
||||
|
||||
drive->moveDistanceAsync(3_ft);
|
||||
drive->moveDistanceAsync(3_ft);
|
||||
|
||||
pros::delay(1000);
|
||||
pros::delay(700);
|
||||
|
||||
drive->moveDistance(-1_ft);
|
||||
drive->moveDistance(-1_ft);
|
||||
|
||||
while (!is_held(imuholdarray, (abs(-70.0 - IMU.get_rotation()) < 3))) {
|
||||
drive->getModel()->rotate((-70.0 - IMU.get_rotation()) * 0.008);
|
||||
while (!is_held(imuholdarray, (abs(-70.0 - IMU.get_rotation()) < 3))) {
|
||||
drive->getModel()->rotate((-70.0 - IMU.get_rotation()) * 0.004);
|
||||
pros::delay(10);
|
||||
}
|
||||
|
||||
drive->getModel()->stop();
|
||||
drive->getModel()->stop();
|
||||
|
||||
drive->moveDistance(1_ft);
|
||||
while (distforward.get() > 300) {
|
||||
drive->getModel()->curvature(12000,
|
||||
(IMU.get_rotation() + 70.0) * -0.04);
|
||||
|
||||
funnypiston.set_value(true);
|
||||
pros::delay(300);
|
||||
// printf("%i\n", distforward.get());
|
||||
pros::delay(10);
|
||||
}
|
||||
drive->getModel()->stop();
|
||||
|
||||
drive->turnAngle(180_deg);
|
||||
funnypiston.set_value(false);
|
||||
funnypiston.set_value(true);
|
||||
pros::delay(300);
|
||||
|
||||
drive->moveDistanceAsync(3_ft);
|
||||
pros::delay(1000);
|
||||
drive->moveDistanceAsync(-1_ft);
|
||||
drive->turnAngle(180_deg);
|
||||
funnypiston.set_value(false);
|
||||
|
||||
drive->moveDistanceAsync(3_ft);
|
||||
pros::delay(700);
|
||||
drive->moveDistanceAsync(-1_ft);
|
||||
|
||||
// piston.set_value(false);
|
||||
}
|
||||
if (autonpick == 10) {
|
||||
|
||||
piston.set_value(true);
|
||||
pros::delay(300);
|
||||
piston.set_value(false);
|
||||
pros::Distance distforward(17);
|
||||
// drive->moveDistanceAsync(500_ft);
|
||||
drive->getModel()->setBrakeMode(okapi::AbstractMotor::brakeMode::hold);
|
||||
// piston.set_value(true);
|
||||
// NEED TO WRTIE IMU CENTER CODE
|
||||
// update IMU each cycle to get true rotation
|
||||
while (distforward.get() > 300) {
|
||||
drive->getModel()->curvature(12000, IMU.get_rotation() * -0.04);
|
||||
|
||||
// printf("%i\n", distforward.get());
|
||||
pros::delay(10);
|
||||
}
|
||||
drive->stop();
|
||||
funnypiston.set_value(true);
|
||||
pros::delay(500);
|
||||
|
||||
// drive->moveDistance(-1_ft);
|
||||
bool imuholdarray[10];
|
||||
while (!is_held(imuholdarray, (abs(130.0 - IMU.get_rotation()) < 3))) {
|
||||
drive->getModel()->rotate((130.0 - IMU.get_rotation()) * 0.004);
|
||||
pros::delay(10);
|
||||
}
|
||||
|
||||
drive->getModel()->stop();
|
||||
pros::delay(100);
|
||||
|
||||
drive->moveDistance(-0.5_ft);
|
||||
|
||||
funnypiston.set_value(false);
|
||||
piston.set_value(true);
|
||||
|
||||
drive->moveDistanceAsync(2.5_ft);
|
||||
pros::delay(1000);
|
||||
piston.set_value(false);
|
||||
drive->moveDistance(-1_ft);
|
||||
pros::delay(200);
|
||||
|
||||
while (!is_held(imuholdarray, (abs(-82.0 - IMU.get_rotation()) < 3))) {
|
||||
drive->getModel()->rotate((-82.0 - IMU.get_rotation()) * 0.004);
|
||||
pros::delay(10);
|
||||
}
|
||||
|
||||
drive->getModel()->stop();
|
||||
|
||||
while (distforward.get() > 300) {
|
||||
drive->getModel()->curvature(12000,
|
||||
(IMU.get_rotation() + 90.0) * -0.04);
|
||||
|
||||
// printf("%i\n", distforward.get());
|
||||
pros::delay(10);
|
||||
}
|
||||
funnypiston.set_value(true);
|
||||
drive->getModel()->stop();
|
||||
pros::delay(700);
|
||||
|
||||
while (!is_held(imuholdarray, (abs(165.0 - IMU.get_rotation()) < 3))) {
|
||||
drive->getModel()->rotate((165.0 - IMU.get_rotation()) * 0.004);
|
||||
pros::delay(10);
|
||||
}
|
||||
|
||||
drive->getModel()->stop();
|
||||
|
||||
pros::delay(200);
|
||||
|
||||
drive->moveDistance(3_ft);
|
||||
|
||||
drive->turnAngle(-110_deg);
|
||||
pros::delay(200);
|
||||
|
||||
funnypiston.set_value(false);
|
||||
piston.set_value(true);
|
||||
|
||||
drive->moveDistanceAsync(3_ft);
|
||||
pros::delay(1200);
|
||||
drive->moveDistance(-1_ft);
|
||||
piston.set_value(false);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -1084,3 +1220,7 @@ void opcontrol() {
|
|||
pros::delay(10);
|
||||
}
|
||||
}
|
||||
// code sukz lol
|
||||
// moaning == goofy
|
||||
// housecats mid
|
||||
// t team better
|
||||
|
|
|
|||
Loading…
Reference in a new issue