here
This commit is contained in:
parent
820658e2e3
commit
efe1fe6bcd
146
src/main.cpp
146
src/main.cpp
|
|
@ -16,6 +16,7 @@
|
|||
#include "okapi/impl/device/controllerUtil.hpp"
|
||||
#include "pros/adi.h"
|
||||
#include "pros/adi.hpp"
|
||||
#include "pros/distance.hpp"
|
||||
#include "pros/llemu.hpp"
|
||||
#include "pros/motors.h"
|
||||
#include "pros/rtos.hpp"
|
||||
|
|
@ -92,6 +93,16 @@ bool is_tapped(bool *log, bool ap) {
|
|||
}
|
||||
};
|
||||
|
||||
bool is_tapped_short(bool *log, bool ap) {
|
||||
shiftarray(log, ap);
|
||||
if (log[0] == true && log[1] == false) {
|
||||
printf("a: %d\n", log[0]);
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
};
|
||||
|
||||
bool is_held(bool *log, bool ap) {
|
||||
shiftarray(log, ap);
|
||||
if (log[0] == true && log[1] == true && log[2] == true && log[3] == true &&
|
||||
|
|
@ -253,15 +264,29 @@ void autonomous() {
|
|||
bool reqtopull = false;
|
||||
bool good[10];
|
||||
pros::Distance dist(13); // fixed
|
||||
int counter = 0;
|
||||
pull.tarePosition();
|
||||
printf("hereasdsadfasdfsdf\n");
|
||||
bool reqtopullarr[10];
|
||||
for (int i = 0; i<10; i++){
|
||||
reqtopullarr[i] = false;
|
||||
}
|
||||
|
||||
while (true) {
|
||||
while (counter<44) {
|
||||
|
||||
if (is_held(good, dist.get() < 70)) {
|
||||
|
||||
|
||||
if (is_held(good, dist.get() < 115)) {
|
||||
reqtopull = true;
|
||||
}
|
||||
|
||||
if(is_tapped_short(reqtopullarr,reqtopull)) {
|
||||
counter = counter + 1;
|
||||
}
|
||||
pros::lcd::set_text(7,std::to_string(counter));
|
||||
|
||||
|
||||
|
||||
// if (pros::millis() % 2000 <= 10){
|
||||
// reqtopull = true;
|
||||
// }
|
||||
|
|
@ -281,6 +306,7 @@ void autonomous() {
|
|||
|
||||
10000000);
|
||||
reqtopull = false;
|
||||
|
||||
}
|
||||
|
||||
pros::delay(10);
|
||||
|
|
@ -291,20 +317,57 @@ void autonomous() {
|
|||
}
|
||||
|
||||
if (autonpick == 3 || autonpick == 8) {
|
||||
pros::delay(500);
|
||||
pros::Distance distcal(20);
|
||||
bool good = false;
|
||||
while (!good) {
|
||||
if (!(distcal.get() < 35)) {
|
||||
// pull.moveVoltage(-12000);
|
||||
pull.moveVoltage(12000);
|
||||
|
||||
} else {
|
||||
pullback = false;
|
||||
ready = true;
|
||||
good = true;
|
||||
pull.moveVoltage(0);
|
||||
pull.tarePosition();
|
||||
}
|
||||
|
||||
}
|
||||
drive->setMaxVelocity(300);
|
||||
drive->moveDistance(-7_ft);
|
||||
drive->turnAngle(-60_deg);
|
||||
|
||||
drive->turnAngle(30_deg);
|
||||
drive->setMaxVelocity(600);
|
||||
//piston.set_value(true);
|
||||
drive->moveDistanceAsync(-5_ft);
|
||||
pros::delay(1200);
|
||||
//piston.set_value(false);
|
||||
|
||||
drive->moveDistance(0.7_ft);
|
||||
drive->moveDistance(-7.1_ft);
|
||||
|
||||
|
||||
pros::delay(500);
|
||||
|
||||
double yaw = IMU.get_heading();
|
||||
std::printf("%f\n", yaw);
|
||||
double turn = -60 - yaw;
|
||||
|
||||
drive->setMaxVelocity(300);
|
||||
drive->turnAngle(turn * 1_deg);
|
||||
|
||||
pros::delay(300);
|
||||
drive->moveDistanceAsync(-5_ft);
|
||||
pros::delay(2000);
|
||||
|
||||
piston.set_value(true);
|
||||
|
||||
|
||||
//drive->turnAngle(90_deg);
|
||||
yaw = IMU.get_heading();
|
||||
turn = 30 - yaw;
|
||||
drive->turnAngle(turn*1_deg);
|
||||
|
||||
drive->moveDistance(-3_ft);
|
||||
|
||||
drive->turnAngle(90_deg);
|
||||
drive->moveDistance(-4_ft);
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -321,6 +384,11 @@ void autonomous() {
|
|||
if (autonpick == 5) {
|
||||
pull.moveRelative(900, 1000000);
|
||||
}
|
||||
|
||||
if (autonpick==6) {
|
||||
drive->moveDistance(-1.95_ft);
|
||||
drive->moveDistance(1_ft);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -359,8 +427,8 @@ void opcontrol() {
|
|||
|
||||
pros::ADIDigitalIn limit(1);
|
||||
pros::ADIDigitalOut piston(2);
|
||||
pros::ADIDigitalOut upflap(3);
|
||||
pros::ADIDigitalOut funnypiston(8);
|
||||
pros::ADIDigitalOut funnypiston(3);
|
||||
pros::ADIDigitalOut upflap(8);
|
||||
|
||||
Motor leftMotor1(-5);
|
||||
Motor leftMotor2(-6);
|
||||
|
|
@ -370,6 +438,9 @@ void opcontrol() {
|
|||
Motor rightMotor2(2);
|
||||
Motor rightMotor3(3);
|
||||
|
||||
//lift on Ltrig
|
||||
|
||||
// intake on A
|
||||
|
||||
leftMotor1.setBrakeMode(okapi::AbstractMotor::brakeMode::brake);
|
||||
leftMotor2.setBrakeMode(okapi::AbstractMotor::brakeMode::brake);
|
||||
|
|
@ -386,7 +457,7 @@ void opcontrol() {
|
|||
Motor pull(12);
|
||||
//pull.setReversed(true);
|
||||
|
||||
Motor lift(16);
|
||||
Motor lift(18);
|
||||
|
||||
// Motor pull2(11);
|
||||
|
||||
|
|
@ -419,6 +490,7 @@ void opcontrol() {
|
|||
|
||||
ControllerButton Abutton(ControllerDigital::A);
|
||||
|
||||
|
||||
ControllerButton uparrow(ControllerDigital::up);
|
||||
ControllerButton Xbutton(ControllerDigital::X);
|
||||
ControllerButton downarrow(ControllerDigital::down);
|
||||
|
|
@ -475,6 +547,12 @@ void opcontrol() {
|
|||
modeswap[i] = false;
|
||||
}
|
||||
|
||||
bool barr[10];
|
||||
for (int i = 0; i < 10; i++){
|
||||
barr[i] = false;
|
||||
}
|
||||
|
||||
|
||||
bool funnypistonstatus = false;
|
||||
auto IMU = okapi::IMU(19, IMUAxes::z);
|
||||
bool reqtoloop = false;
|
||||
|
|
@ -488,6 +566,9 @@ void opcontrol() {
|
|||
|
||||
pros::Distance dististriball(11); // fixed
|
||||
|
||||
bool intakebool;
|
||||
intakebool = false;
|
||||
|
||||
while (true) {
|
||||
// Tank drive with left and right sticks.
|
||||
drive->getModel()->tank(controller.getAnalog(ControllerAnalog::leftY),
|
||||
|
|
@ -545,11 +626,11 @@ void opcontrol() {
|
|||
tcount++;
|
||||
}
|
||||
|
||||
if (is_tapped(funnyarr, leftarrow.isPressed())) {
|
||||
if (is_tapped(funnyarr, downarrow.isPressed())) {
|
||||
funnypistonstatus = !funnypistonstatus;
|
||||
}
|
||||
|
||||
if (is_tapped(y1arr, Abutton.isPressed())) {
|
||||
if (is_tapped(y1arr, Bbutton.isPressed())) {
|
||||
upflapstatus = !upflapstatus;
|
||||
}
|
||||
|
||||
|
|
@ -646,16 +727,16 @@ void opcontrol() {
|
|||
// pros::lcd::set_text(5, "hit while not");
|
||||
// }
|
||||
|
||||
if (is_tapped(loopmodearr, downarrow.isPressed())) {
|
||||
// if (is_tapped(loopmodearr, downarrow.isPressed())) {
|
||||
// lmode = !lmode;
|
||||
lmode = false;
|
||||
}
|
||||
// //lmode = false;
|
||||
// }
|
||||
|
||||
// printf("%i",dist.get());
|
||||
if (is_held(distarr, dist.get() < 70) && lmode) {
|
||||
reqtoloop = true;
|
||||
// printf("raaaaaaaaaaaaaa");
|
||||
}
|
||||
// // printf("%i",dist.get());
|
||||
// if (is_held(distarr, dististriball.get() < 70) && lmode) {
|
||||
// reqtoloop = true;
|
||||
// // printf("raaaaaaaaaaaaaa");
|
||||
// }
|
||||
|
||||
if (is_tapped(r1arr, R1.isPressed()) || reqtoloop == true) {
|
||||
float temp = std::fmod(pull.getPosition(), 900);
|
||||
|
|
@ -691,25 +772,12 @@ void opcontrol() {
|
|||
|
||||
pros::lcd::set_text(4, std::to_string(pull.getTargetPosition()));
|
||||
|
||||
if (Bbutton.isPressed()) {
|
||||
pull.moveVoltage(12000);
|
||||
}
|
||||
if (uparrow.isPressed()) {
|
||||
pull.moveVoltage(0);
|
||||
}
|
||||
|
||||
|
||||
|
||||
//pros::lcd::set_text(5, std::to_string(IMU.get()));
|
||||
|
||||
// if (L2.isPressed()) {
|
||||
// intake.moveVoltage(-12000);
|
||||
// testMotor.moveVoltage(-12000);
|
||||
// } else if (L1.isPressed()) {
|
||||
// intake.moveVoltage(12000);
|
||||
// testMotor.moveVoltage(-12000);
|
||||
// } else {
|
||||
// intake.moveVoltage(0);
|
||||
// testMotor.moveVoltage(0);
|
||||
// }
|
||||
|
||||
|
||||
pros::lcd::set_text(5,std::to_string(pull.getPosition()));
|
||||
|
||||
|
|
|
|||
Loading…
Reference in a new issue