InventorX/pleasehelp
1
0
Fork 0
Code Issues Pull requests Packages Projects Releases Wiki Activity

ra

Browse source
This commit is contained in:
InventorXtreme 2024-01-27 08:44:49 -05:00
parent 00cf4c6d83
commit 96232c9d78
2 changed files with 259 additions and 195 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

42
docs/auton.org Normal file
View file

@ -0,0 +1,42 @@
#+OPTIONS: toc:nil
#+LATEX_HEADER: \usepackage{nopageno}
#+STARTUP: inlineimages
* Autonomous Map
0. [@0] AUTON 0
Auton pushes one tribal into goal, then turns and drives towards the bar and deploys flaps to touch.
+ Opposite Side
+ Total Score: 5?
+ 2/3 of auton win point
1. AUTON 1
Deploys wings to knock triball out of match load zone, then drives forward
+ Close Side
+ Total Score +2?
+ 1/3 of auton win point
2. AUTON 2
Deploys wings to knock triball out of match load zone, then drives forward. Retracts wings, and drives towards the bar before redeploying wings to touch the bar.
+ Close Side
+ Total Score +2?
+ 2/3 of auton win point
+ Sub-auton of auton 1
3. AUTON 3
Uses triball detection distance sensor to shoot 44 triballs over to the other side. Then retracts catapult, moves forward, and pushes triballs under goal.
+ Should be setup 30 degress left of straight
+ Skills auton
+ Currently non functional due to no shooter
4. AUTON 4
Then retracts catapult, moves forward, and pushes triballs under goal.
+ Currently non functional due to no shooter
+ Can be run exclusively
+ Setup should be 30 degrees left of straight
5. AUTON 5
Drives forward grabbing triball in center of field. Then pulls triball back, rotates right, and pushes two triballs towards our side. Then reverses and knocks triball into goal.
+ Opposing side
+ Scores 9 points
+ Can counter other teams
6. AUTON 6
Push auton. Moves 2ft backward then 0.5ft forward.
+ Utility auton

246
src/main.cpp
View file

@ -9,6 +9,7 @@
/** /**
* You should add more #includes here * You should add more #includes here
*/ */
#include "display/lv_misc/lv_task.h"
#include "okapi/api.hpp" #include "okapi/api.hpp"
#include "okapi/api/device/motor/abstractMotor.hpp" #include "okapi/api/device/motor/abstractMotor.hpp"
#include "okapi/api/units/QLength.hpp" #include "okapi/api/units/QLength.hpp"
@ -23,6 +24,9 @@
#include <bits/stdc++.h> #include <bits/stdc++.h>
// 1 2 3 5 //left
// 7 8 9 10 //right
int autonpick = 0; int autonpick = 0;
int automod = 0; int automod = 0;
lv_res_t sa0(lv_obj_t *btn) { lv_res_t sa0(lv_obj_t *btn) {
@ -86,6 +90,7 @@ void shiftarray(bool *arr, bool ap) {
bool is_tapped(bool *log, bool ap) { bool is_tapped(bool *log, bool ap) {
shiftarray(log, ap); shiftarray(log, ap);
if (log[0] == true && log[9] == false && log[1] == false) { if (log[0] == true && log[9] == false && log[1] == false) {
printf("a: %d\n", log[0]); printf("a: %d\n", log[0]);
return true; return true;
} else { } else {
@ -201,20 +206,21 @@ void autonomous() {
fclose(usd_file_read); // always close files when you're done with them fclose(usd_file_read); // always close files when you're done with them
autonpick = num; autonpick = num;
// int autonpick = 3; // int autonpick = 3;
std::shared_ptr<ChassisController> drive =
ChassisControllerBuilder()
.withMotors(
{-1, -2, -3}, std::shared_ptr<ChassisController> drive =
{ // -5 is
5, ChassisControllerBuilder()
6, .withMotors({-1, -2, -3, -5}, {7, 8, 9, 10})
7, .withDimensions({AbstractMotor::gearset::blue, (48.0 / 36.0)},
}) {{3.25_in, 11.2_in}, imev5BlueTPR})
.withDimensions({AbstractMotor::gearset::blue, (60.0 / 48.0)},
{{3.25_in, 12_in}, imev5BlueTPR})
.build(); .build();
pros::ADIDigitalOut funnypiston(3);
funnypiston.set_value(false);
pros::ADIDigitalOut upflap(8);
upflap.set_value(false);
pros::Imu IMU(19); pros::Imu IMU(19);
IMU.reset(true); IMU.reset(true);
IMU.tare(); IMU.tare();
@ -229,38 +235,46 @@ void autonomous() {
pros::ADIDigitalOut piston(2); pros::ADIDigitalOut piston(2);
if (autonpick == 0) { // Auton #1, used for adding tribal to goal if (autonpick == 0) { // Auton #1, used for adding tribal to goal
float x = -3.2; float x = -2.7;
drive->moveDistanceAsync(x * 1_ft); drive->moveDistanceAsync(x * 1_ft);
pros::delay(2500); pros::delay(2500);
drive->moveDistance(1_ft); drive->moveDistance(0.5_ft);
double yaw = IMU.get_heading(); double yaw = IMU.get_heading();
std::printf("%f", yaw); std::printf("yaw: %f\n", yaw);
double turn = -92 - yaw; // double turn = -92 - yaw;
double turn = 45 - yaw;
std:
printf("turn: %f\n", turn);
drive->setMaxVelocity(300); drive->setMaxVelocity(300);
drive->turnAngle(turn * 1_deg); drive->turnAngle(turn * 1_deg);
drive->moveDistanceAsync(-4.1_ft); drive->moveDistanceAsync(4.1_ft);
pros::delay(3000); pros::delay(3000);
piston.set_value(true); piston.set_value(true);
} else if (autonpick == 1 || } else if (autonpick == 1 ||
autonpick == autonpick ==
2) { // Auton #2, Used for removing triball from match loader 2) { // Auton #2, Used for removing triball from match loader
piston.set_value(true); piston.set_value(true);
drive->moveDistance(1_ft);
if (autonpick == 2) { if (autonpick == 2) {
drive->setMaxVelocity(300); drive->setMaxVelocity(300);
pros::delay(1000); pros::delay(1000);
piston.set_value(false); piston.set_value(false);
drive->moveDistance(-1.7_ft); // drive->moveDistance(-1.7_ft);
drive->turnAngle(87_deg); drive->turnAngle(115_deg);
drive->moveDistance(-3.5_ft); drive->moveDistance(3.5_ft);
drive->turnAngle(95_deg); drive->turnAngle(95_deg);
drive->moveDistance(-2_in); drive->setMaxVelocity(50);
drive->moveDistanceAsync(2_ft);
pros::delay(2500);
piston.set_value(true); piston.set_value(true);
drive->moveDistance(-4_in); drive->moveDistance(4_in);
} }
// drive->turnAngle(45_deg); // drive->turnAngle(45_deg);
} else if (autonpick == 3) { } else if (autonpick == 3) {
upflap.set_value(true);
bool reqtopull = false; bool reqtopull = false;
bool good[10]; bool good[10];
pros::Distance dist(13); // fixed pros::Distance dist(13); // fixed
@ -274,8 +288,6 @@ void autonomous() {
while (counter < 44) { while (counter < 44) {
if (is_held(good, dist.get() < 115)) { if (is_held(good, dist.get() < 115)) {
reqtopull = true; reqtopull = true;
} }
@ -285,8 +297,6 @@ void autonomous() {
} }
pros::lcd::set_text(7, std::to_string(counter)); pros::lcd::set_text(7, std::to_string(counter));
// if (pros::millis() % 2000 <= 10){ // if (pros::millis() % 2000 <= 10){
// reqtopull = true; // reqtopull = true;
// } // }
@ -301,22 +311,19 @@ void autonomous() {
} }
pull.moveRelative( pull.moveRelative(
(900 - (900 -
temp), // pull.moveRelative((1800 - temp), // pull.moveRelative((1800
//-
//std::fmod(pull.getPosition(), 1800)), //std::fmod(pull.getPosition(), 1800)),
10000000); 10000000);
reqtopull = false; reqtopull = false;
} }
pros::delay(10); pros::delay(10);
} }
} }
if (autonpick == 3 || autonpick == 8) { if (autonpick == 3 || autonpick == 4) {
pros::delay(500); pros::delay(500);
pros::Distance distcal(20); pros::Distance distcal(20);
bool good = false; bool good = false;
@ -332,7 +339,6 @@ void autonomous() {
pull.moveVoltage(0); pull.moveVoltage(0);
pull.tarePosition(); pull.tarePosition();
} }
} }
drive->setMaxVelocity(300); drive->setMaxVelocity(300);
drive->turnAngle(30_deg); drive->turnAngle(30_deg);
@ -340,7 +346,6 @@ void autonomous() {
drive->moveDistance(-7.1_ft); drive->moveDistance(-7.1_ft);
pros::delay(500); pros::delay(500);
double yaw = IMU.get_heading(); double yaw = IMU.get_heading();
@ -356,7 +361,6 @@ void autonomous() {
piston.set_value(true); piston.set_value(true);
// drive->turnAngle(90_deg); // drive->turnAngle(90_deg);
yaw = IMU.get_heading(); yaw = IMU.get_heading();
turn = 30 - yaw; turn = 30 - yaw;
@ -366,28 +370,70 @@ void autonomous() {
drive->turnAngle(90_deg); drive->turnAngle(90_deg);
drive->moveDistance(-4_ft); drive->moveDistance(-4_ft);
}
if (autonpick == -1) {
drive->turnAngle(90_deg);
}
if (autonpick == 4) {
float x = sqrt(8);
drive->moveDistance(x * -1_ft);
pros::delay(500);
drive->moveDistance(0.5_ft);
} }
if (autonpick == 5) { if (autonpick == 5) {
pull.moveRelative(900, 1000000); 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() > 320) {
drive->getModel()->curvature(12000, IMU.get_rotation() * -0.04);
// printf("%i\n", distforward.get());
pros::delay(10);
}
// piston.set_value(false);
drive->stop();
funnypiston.set_value(true);
pros::delay(500);
auto starttime = Timer().millis();
while (Timer().millis() < starttime + 1.5_s ) {
drive->getModel()->curvature(-12000 , IMU.get_rotation() * -0.04);
// printf("%i\n", distforward.get());
pros::delay(10);
}
// piston.set_value(false);
drive->stop();
funnypiston.set_value(true);
pros::delay(500);
drive->moveDistance(1_in);
// drive->turnAngle(15_deg);
//drive->moveDistance(-3.8_ft);
//drive->turnAngle(90_deg);
bool imuholdarray[10];
while (!is_held(imuholdarray, (abs(68.0 - IMU.get_rotation()) < 1)) ) {
drive->getModel()->rotate((68.0 - IMU.get_rotation()) * 0.008 );
pros::delay(10);
}
drive->getModel()->stop();
funnypiston.set_value(false);
drive->moveDistance(2_ft);
drive->moveDistance(-2_ft);
funnypiston.set_value(true);
drive->moveDistance(2_ft);
drive->moveDistance(-2_ft);
funnypiston.set_value(false);
drive->turnAngle(45_deg);
drive->moveDistance(-2_ft);
drive->moveDistance(1_ft);
} }
if (autonpick == 6) { if (autonpick == 6) {
drive->moveDistance(-1.95_ft); drive->setMaxVelocity(600);
drive->moveDistance(1_ft); float x = 2;
drive->moveDistance(x * -1_ft);
pros::delay(500);
drive->moveDistance(0.5_ft);
} }
} }
@ -411,16 +457,11 @@ void opcontrol() {
// pros::delay(100); // pros::delay(100);
std::shared_ptr<ChassisController> drive = std::shared_ptr<ChassisController> drive =
ChassisControllerBuilder() ChassisControllerBuilder()
.withMotors( .withMotors({-1, -2, -3, -5}, {7, 8, 9, 10})
{
-5,
-6,
-7,
},
{1, 2, 3})
// Green gearset, 4 in wheel diam, 11.5 in wheel track // Green gearset, 4 in wheel diam, 11.5 in wheel track
.withDimensions({AbstractMotor::gearset::blue, (48.0 / 36.0)}, .withDimensions({AbstractMotor::gearset::blue, (48.0 / 36.0)},
{{3.25_in, 12_in}, imev5BlueTPR}) {{3.25_in, 11.2_in}, imev5BlueTPR})
.build(); .build();
Controller controller; Controller controller;
@ -430,25 +471,20 @@ void opcontrol() {
pros::ADIDigitalOut funnypiston(3); pros::ADIDigitalOut funnypiston(3);
pros::ADIDigitalOut upflap(8); pros::ADIDigitalOut upflap(8);
Motor leftMotor1(-5); Motor leftMotor1(-1);
Motor leftMotor2(-6); Motor leftMotor2(-2);
Motor leftMotor3(-7); Motor leftMotor3(-3);
Motor leftMotor4(-5);
Motor rightMotor1(1);
Motor rightMotor2(2);
Motor rightMotor3(3);
Motor rightMotor1(7);
Motor rightMotor2(8);
Motor rightMotor3(9);
Motor rightMotor4(10);
// lift on Ltrig // lift on Ltrig
// intake on A // intake on A
leftMotor1.setBrakeMode(okapi::AbstractMotor::brakeMode::brake); drive->getModel()->setBrakeMode(okapi::AbstractMotor::brakeMode::brake);
leftMotor2.setBrakeMode(okapi::AbstractMotor::brakeMode::brake);
leftMotor3.setBrakeMode(okapi::AbstractMotor::brakeMode::brake);
rightMotor1.setBrakeMode(okapi::AbstractMotor::brakeMode::brake);
rightMotor2.setBrakeMode(okapi::AbstractMotor::brakeMode::brake);
rightMotor3.setBrakeMode(okapi::AbstractMotor::brakeMode::brake);
bool flipped = false; bool flipped = false;
@ -469,10 +505,10 @@ void opcontrol() {
// MotorGroup pull = MotorGroup({pull1, pull2}); // MotorGroup pull = MotorGroup({pull1, pull2});
flywheel.setBrakeMode(okapi::AbstractMotor::brakeMode::coast); flywheel.setBrakeMode(okapi::AbstractMotor::brakeMode::coast);
flywheel.setGearing(okapi::AbstractMotor::gearset::blue); //should be red for cata flywheel.setGearing(
okapi::AbstractMotor::gearset::blue); // should be red for cata
pull.setBrakeMode(okapi::AbstractMotor::brakeMode::coast); pull.setBrakeMode(okapi::AbstractMotor::brakeMode::coast);
lift.setBrakeMode(okapi::AbstractMotor::brakeMode::hold); lift.setBrakeMode(okapi::AbstractMotor::brakeMode::hold);
ControllerButton R2(ControllerDigital::R2); ControllerButton R2(ControllerDigital::R2);
@ -490,7 +526,6 @@ void opcontrol() {
ControllerButton Abutton(ControllerDigital::A); ControllerButton Abutton(ControllerDigital::A);
ControllerButton uparrow(ControllerDigital::up); ControllerButton uparrow(ControllerDigital::up);
ControllerButton Xbutton(ControllerDigital::X); ControllerButton Xbutton(ControllerDigital::X);
ControllerButton downarrow(ControllerDigital::down); ControllerButton downarrow(ControllerDigital::down);
@ -562,7 +597,6 @@ void opcontrol() {
barr[i] = false; barr[i] = false;
} }
bool funnypistonstatus = false; bool funnypistonstatus = false;
auto IMU = okapi::IMU(19, IMUAxes::z); auto IMU = okapi::IMU(19, IMUAxes::z);
bool reqtoloop = false; bool reqtoloop = false;
@ -589,13 +623,13 @@ void opcontrol() {
// every 10 ms. // every 10 ms.
float lsum = leftMotor1.getTemperature() + leftMotor2.getTemperature() + float lsum = leftMotor1.getTemperature() + leftMotor2.getTemperature() +
leftMotor3.getTemperature(); leftMotor3.getTemperature() + leftMotor4.getTemperature();
float lavg = lsum / 3; float lavg = lsum / 4;
float rsum = rightMotor1.getTemperature() + float rsum =
rightMotor2.getTemperature() + rightMotor1.getTemperature() + rightMotor2.getTemperature() +
rightMotor3.getTemperature(); rightMotor3.getTemperature() + rightMotor4.getTemperature();
float ravg = rsum / 3; float ravg = rsum / 4;
if (lavg > 150) { if (lavg > 150) {
lavg = -9; lavg = -9;
@ -692,28 +726,27 @@ void opcontrol() {
funnypiston.set_value(funnypistonstatus); funnypiston.set_value(funnypistonstatus);
pros::lcd::set_text(1, std::to_string(tcount)); pros::lcd::set_text(1, std::to_string(tcount));
if (!flipperstatus) { if (flipperstatus) {
pros::lcd::set_text(0, "flip f flippcmdreceive"); pros::lcd::set_text(0, "flip f flippcmdreceive");
drive = ChassisControllerBuilder() drive = ChassisControllerBuilder()
.withMotors({-5, -6, -7}, {1, 2, 3}) .withMotors({-7, -8, -9, -10}, {1, 2, 3, 5})
.withDimensions( .withDimensions(
{AbstractMotor::gearset::blue, (48.0 / 36.0)}, {AbstractMotor::gearset::blue, (48.0 / 36.0)},
{{3.25_in, 12_in}, imev5BlueTPR}) {{3.25_in, 11.2_in}, imev5BlueTPR})
.build();
flipped = false;
}
if (flipperstatus) {
pros::lcd::set_text(0, "flip b flippcmdreceive");
drive = ChassisControllerBuilder()
.withMotors({-1, -2, -3}, {5, 6, 7})
.withDimensions(
{AbstractMotor::gearset::blue, (48.0 / 36.0)},
{{3.25_in, 12_in}, imev5BlueTPR})
.build(); .build();
flipped = true; flipped = true;
} }
if (!flipperstatus) {
pros::lcd::set_text(0, "flip b flippcmdreceive");
drive = ChassisControllerBuilder()
.withMotors({-1, -2, -3, -5}, {7, 8, 9, 10})
.withDimensions(
{AbstractMotor::gearset::blue, (48.0 / 36.0)},
{{3.25_in, 11.2_in}, imev5BlueTPR})
.build();
flipped = false;
}
// if (R1.isPressed()) { // if (R1.isPressed()) {
// flywheel.moveVoltage(-12000); // flywheel.moveVoltage(-12000);
@ -722,8 +755,6 @@ void opcontrol() {
// flywheel.moveVoltage(0); // flywheel.moveVoltage(0);
// } // }
// if (fly.isPressed()) { // if (fly.isPressed()) {
// pullback = true; // pullback = true;
@ -750,8 +781,9 @@ void opcontrol() {
} }
pull.moveRelative( pull.moveRelative(
(900 - (900 -
temp), // pull.moveRelative((1800 - temp), // pull.moveRelative((1800
// std::fmod(pull.getPosition(), 1800)), //- std::fmod(pull.getPosition(),
// 1800)),
10000000); 10000000);
reqtoloop = false; reqtoloop = false;
@ -767,33 +799,23 @@ void opcontrol() {
} }
// if (dist.get() < 40 && // if (dist.get() < 40 &&
// (std::abs(pull.getPosition() - pull.getTargetPosition()) < 5)) { // (std::abs(pull.getPosition() - pull.getTargetPosition()) <
// pull.moveVoltage(0); // 5)) { pull.moveVoltage(0);
// } else { // } else {
// } // }
pros::lcd::set_text(4, std::to_string(pull.getTargetPosition())); pros::lcd::set_text(4, std::to_string(pull.getTargetPosition()));
// pros::lcd::set_text(5, std::to_string(IMU.get())); // pros::lcd::set_text(5, std::to_string(IMU.get()));
pros::lcd::set_text(5, std::to_string(pull.getPosition())); pros::lcd::set_text(5, std::to_string(pull.getPosition()));
controller.setText( controller.setText(
0, 0, 0, 0,
std::to_string(static_cast<int>(pull.getTemperature())) +
std::to_string(static_cast<int>(lavg)) + std::to_string(static_cast<int>(lavg)) +
std::to_string(static_cast<int>(ravg)) + std::to_string(static_cast<int>(ravg)) +
std::to_string(static_cast<int>(lift.getTemperature())) std::to_string(static_cast<int>(lift.getTemperature()))
); );
pros::delay(10); pros::delay(10);
} }