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

406
src/main.cpp
View file

@ -9,6 +9,7 @@
/**
* You should add more #includes here
*/
#include "display/lv_misc/lv_task.h"
#include "okapi/api.hpp"
#include "okapi/api/device/motor/abstractMotor.hpp"
#include "okapi/api/units/QLength.hpp"
@ -23,6 +24,9 @@
#include <bits/stdc++.h>
// 1 2 3 5 //left
// 7 8 9 10 //right
int autonpick = 0;
int automod = 0;
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) {
shiftarray(log, ap);
if (log[0] == true && log[9] == false && log[1] == false) {
printf("a: %d\n", log[0]);
return true;
} else {
@ -201,20 +206,21 @@ void autonomous() {
fclose(usd_file_read); // always close files when you're done with them
autonpick = num;
// int autonpick = 3;
std::shared_ptr<ChassisController> drive =
ChassisControllerBuilder()
.withMotors(
{-1, -2, -3},
{
5,
6,
7,
})
.withDimensions({AbstractMotor::gearset::blue, (60.0 / 48.0)},
{{3.25_in, 12_in}, imev5BlueTPR})
std::shared_ptr<ChassisController> drive =
// -5 is
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();
pros::ADIDigitalOut funnypiston(3);
funnypiston.set_value(false);
pros::ADIDigitalOut upflap(8);
upflap.set_value(false);
pros::Imu IMU(19);
IMU.reset(true);
IMU.tare();
@ -229,71 +235,75 @@ void autonomous() {
pros::ADIDigitalOut piston(2);
if (autonpick == 0) { // Auton #1, used for adding tribal to goal
float x = -3.2;
float x = -2.7;
drive->moveDistanceAsync(x * 1_ft);
pros::delay(2500);
drive->moveDistance(1_ft);
drive->moveDistance(0.5_ft);
double yaw = IMU.get_heading();
std::printf("%f", yaw);
double turn = -92 - yaw;
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);
drive->moveDistanceAsync(4.1_ft);
pros::delay(3000);
piston.set_value(true);
} else if (autonpick == 1 ||
autonpick ==
2) { // Auton #2, Used for removing triball from match loader
piston.set_value(true);
drive->moveDistance(1_ft);
if (autonpick == 2) {
drive->setMaxVelocity(300);
pros::delay(1000);
piston.set_value(false);
drive->moveDistance(-1.7_ft);
drive->turnAngle(87_deg);
drive->moveDistance(-3.5_ft);
// drive->moveDistance(-1.7_ft);
drive->turnAngle(115_deg);
drive->moveDistance(3.5_ft);
drive->turnAngle(95_deg);
drive->moveDistance(-2_in);
drive->setMaxVelocity(50);
drive->moveDistanceAsync(2_ft);
pros::delay(2500);
piston.set_value(true);
drive->moveDistance(-4_in);
drive->moveDistance(4_in);
}
// drive->turnAngle(45_deg);
} else if (autonpick == 3) {
upflap.set_value(true);
bool reqtopull = false;
bool good[10];
pros::Distance dist(13); // fixed
int counter = 0;
int counter = 0;
pull.tarePosition();
printf("hereasdsadfasdfsdf\n");
bool reqtopullarr[10];
for (int i = 0; i<10; i++){
reqtopullarr[i] = false;
}
bool reqtopullarr[10];
for (int i = 0; i < 10; i++) {
reqtopullarr[i] = false;
}
while (counter<44) {
while (counter < 44) {
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 (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;
// }
// if (pros::millis() % 2000 <= 10){
// reqtopull = true;
// }
if (reqtopull) {
// pull.moveRelative((1800 - std::fmod(pull.getPosition(),
// 1800)),
// 1000);
// 1000);
// reqtopull = false;
float temp = std::fmod(pull.getPosition(), 900);
if ((900 - temp) < 150) {
@ -301,93 +311,129 @@ void autonomous() {
}
pull.moveRelative(
(900 -
temp), // pull.moveRelative((1800 -
// std::fmod(pull.getPosition(), 1800)),
temp), // pull.moveRelative((1800
//-
//std::fmod(pull.getPosition(), 1800)),
10000000);
reqtopull = false;
}
pros::delay(10);
}
}
}
if (autonpick == 3 || autonpick == 8) {
pros::delay(500);
pros::Distance distcal(20);
bool good = false;
while (!good) {
if (!(distcal.get() < 35)) {
if (autonpick == 3 || autonpick == 4) {
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;
good = true;
pull.moveVoltage(0);
pull.tarePosition();
}
}
drive->setMaxVelocity(300);
drive->turnAngle(30_deg);
drive->setMaxVelocity(600);
}
drive->setMaxVelocity(300);
drive->turnAngle(30_deg);
drive->setMaxVelocity(600);
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();
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);
pros::delay(300);
drive->moveDistanceAsync(-5_ft);
pros::delay(2000);
piston.set_value(true);
piston.set_value(true);
// drive->turnAngle(90_deg);
yaw = IMU.get_heading();
turn = 30 - yaw;
drive->turnAngle(turn * 1_deg);
//drive->turnAngle(90_deg);
yaw = IMU.get_heading();
turn = 30 - yaw;
drive->turnAngle(turn*1_deg);
drive->moveDistance(-3_ft);
drive->moveDistance(-3_ft);
drive->turnAngle(90_deg);
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);
drive->moveDistance(-4_ft);
}
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) {
drive->moveDistance(-1.95_ft);
drive->moveDistance(1_ft);
if (autonpick == 6) {
drive->setMaxVelocity(600);
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);
std::shared_ptr<ChassisController> drive =
ChassisControllerBuilder()
.withMotors(
{
-5,
-6,
-7,
},
{1, 2, 3})
.withMotors({-1, -2, -3, -5}, {7, 8, 9, 10})
// Green gearset, 4 in wheel diam, 11.5 in wheel track
.withDimensions({AbstractMotor::gearset::blue, (48.0 / 36.0)},
{{3.25_in, 12_in}, imev5BlueTPR})
{{3.25_in, 11.2_in}, imev5BlueTPR})
.build();
Controller controller;
@ -430,32 +471,27 @@ void opcontrol() {
pros::ADIDigitalOut funnypiston(3);
pros::ADIDigitalOut upflap(8);
Motor leftMotor1(-5);
Motor leftMotor2(-6);
Motor leftMotor3(-7);
Motor leftMotor1(-1);
Motor leftMotor2(-2);
Motor leftMotor3(-3);
Motor leftMotor4(-5);
Motor rightMotor1(1);
Motor rightMotor2(2);
Motor rightMotor3(3);
//lift on Ltrig
Motor rightMotor1(7);
Motor rightMotor2(8);
Motor rightMotor3(9);
Motor rightMotor4(10);
// lift on Ltrig
// intake on A
leftMotor1.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);
drive->getModel()->setBrakeMode(okapi::AbstractMotor::brakeMode::brake);
bool flipped = false;
Motor flywheel(15);
Motor pull(12);
//pull.setReversed(true);
// pull.setReversed(true);
Motor lift(18);
@ -469,10 +505,10 @@ void opcontrol() {
// MotorGroup pull = MotorGroup({pull1, pull2});
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);
lift.setBrakeMode(okapi::AbstractMotor::brakeMode::hold);
ControllerButton R2(ControllerDigital::R2);
@ -490,7 +526,6 @@ void opcontrol() {
ControllerButton Abutton(ControllerDigital::A);
ControllerButton uparrow(ControllerDigital::up);
ControllerButton Xbutton(ControllerDigital::X);
ControllerButton downarrow(ControllerDigital::down);
@ -506,13 +541,13 @@ void opcontrol() {
}
bool l1toggle[10];
for (int i=0; i<10; i++) {
l1toggle[i] = false;
for (int i = 0; i < 10; i++) {
l1toggle[i] = false;
}
bool l2toggle[10];
for (int i=0; i < 10; i++) {
l2toggle[i] = false;
for (int i = 0; i < 10; i++) {
l2toggle[i] = false;
}
bool pullback = false;
@ -558,11 +593,10 @@ void opcontrol() {
}
bool barr[10];
for (int i = 0; i < 10; i++){
barr[i] = false;
for (int i = 0; i < 10; i++) {
barr[i] = false;
}
bool funnypistonstatus = false;
auto IMU = okapi::IMU(19, IMUAxes::z);
bool reqtoloop = false;
@ -589,20 +623,20 @@ void opcontrol() {
// every 10 ms.
float lsum = leftMotor1.getTemperature() + leftMotor2.getTemperature() +
leftMotor3.getTemperature();
float lavg = lsum / 3;
leftMotor3.getTemperature() + leftMotor4.getTemperature();
float lavg = lsum / 4;
float rsum = rightMotor1.getTemperature() +
rightMotor2.getTemperature() +
rightMotor3.getTemperature();
float ravg = rsum / 3;
float rsum =
rightMotor1.getTemperature() + rightMotor2.getTemperature() +
rightMotor3.getTemperature() + rightMotor4.getTemperature();
float ravg = rsum / 4;
if (lavg > 150) {
lavg = -9;
}
if (ravg >150) {
ravg = -9;
}
if (lavg > 150) {
lavg = -9;
}
if (ravg > 150) {
ravg = -9;
}
if (pullback) {
if (!(dist.get() < 35)) {
@ -626,9 +660,9 @@ void opcontrol() {
tcount++;
}
if(is_tapped(l1toggle, L1.isPressed() )) {
flapsOn = !flapsOn;
}
if (is_tapped(l1toggle, L1.isPressed())) {
flapsOn = !flapsOn;
}
if (is_tapped(manpullarr,
(uparrow.isPressed() && Xbutton.isPressed()))) {
@ -673,7 +707,7 @@ void opcontrol() {
tempaselect = tempaselect - 1;
}
if (is_tapped(a, Abutton.isPressed())) {
// write to file;
// write to file;
FILE *autofile = fopen("/usd/auto.txt", "w");
fprintf(autofile, "%d", tempaselect);
fclose(autofile);
@ -692,66 +726,64 @@ void opcontrol() {
funnypiston.set_value(funnypistonstatus);
pros::lcd::set_text(1, std::to_string(tcount));
if (!flipperstatus) {
if (flipperstatus) {
pros::lcd::set_text(0, "flip f flippcmdreceive");
drive = ChassisControllerBuilder()
.withMotors({-5, -6, -7}, {1, 2, 3})
.withMotors({-7, -8, -9, -10}, {1, 2, 3, 5})
.withDimensions(
{AbstractMotor::gearset::blue, (48.0 / 36.0)},
{{3.25_in, 12_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})
{{3.25_in, 11.2_in}, imev5BlueTPR})
.build();
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()) {
// flywheel.moveVoltage(-12000);
// }
// else {
// flywheel.moveVoltage(0);
// }
// if (R1.isPressed()) {
// flywheel.moveVoltage(-12000);
// }
// else {
// flywheel.moveVoltage(0);
// }
// if (fly.isPressed()) {
// pullback = true;
// pullback = true;
// pros::lcd::set_text(5, "hit while not");
// pros::lcd::set_text(5, "hit while not");
// }
// if (is_tapped(loopmodearr, downarrow.isPressed())) {
// lmode = !lmode;
// //lmode = false;
// lmode = !lmode;
// //lmode = false;
// }
// // printf("%i",dist.get());
// if (is_held(distarr, dististriball.get() < 70) && lmode) {
// reqtoloop = true;
// // printf("raaaaaaaaaaaaaa");
// reqtoloop = true;
// // printf("raaaaaaaaaaaaaa");
// }
if (is_tapped(r1arr, R1.isPressed()) || reqtoloop == true) {
float temp = std::fmod(pull.getPosition(), 900);
pros::lcd::set_text(6,std::to_string(temp));
pros::lcd::set_text(6, std::to_string(temp));
if ((900 - temp) < 150) {
temp = -(900 - temp);
pros::lcd::set_text(7,std::to_string(temp));
pros::lcd::set_text(7, std::to_string(temp));
}
pull.moveRelative(
(900 -
temp), // pull.moveRelative((1800 -
// std::fmod(pull.getPosition(), 1800)),
temp), // pull.moveRelative((1800
//- std::fmod(pull.getPosition(),
// 1800)),
10000000);
reqtoloop = false;
@ -767,34 +799,24 @@ void opcontrol() {
}
// if (dist.get() < 40 &&
// (std::abs(pull.getPosition() - pull.getTargetPosition()) < 5)) {
// pull.moveVoltage(0);
// (std::abs(pull.getPosition() - pull.getTargetPosition()) <
// 5)) { pull.moveVoltage(0);
// } else {
// }
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(
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>(lift.getTemperature()))
std::to_string(static_cast<int>(lift.getTemperature()))
);
);
pros::delay(10);
}
}