commonwealth-robotics/steak
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Refactored everything.

Browse Source
This commit is contained in:
Jacob
2024-09-18 13:05:17 -04:00
commit 6569224de3
282 changed files with 45883 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

196
src/autons.cpp Normal file
View File

@@ -0,0 +1,196 @@
#include "main.h"
/////
// For installation, upgrading, documentations and tutorials, check out our website!
// https://ez-robotics.github.io/EZ-Template/
/////
// These are out of 127
const int DRIVE_SPEED = 110;
const int TURN_SPEED = 90;
const int SWING_SPEED = 90;
///
// Constants
///
void default_constants() {
chassis.pid_heading_constants_set(3, 0, 20);
chassis.pid_drive_constants_set(10, 0, 100);
chassis.pid_turn_constants_set(3, 0, 20);
chassis.pid_swing_constants_set(5, 0, 30);
chassis.pid_turn_exit_condition_set(300_ms, 3_deg, 500_ms, 7_deg, 750_ms, 750_ms);
chassis.pid_swing_exit_condition_set(300_ms, 3_deg, 500_ms, 7_deg, 750_ms, 750_ms);
chassis.pid_drive_exit_condition_set(300_ms, 1_in, 500_ms, 3_in, 750_ms, 750_ms);
chassis.slew_drive_constants_set(7_in, 80);
}
///
// Drive Example
///
void drive_example() {
// The first parameter is target inches
// The second parameter is max speed the robot will drive at
// The third parameter is a boolean (true or false) for enabling/disabling a slew at the start of drive motions
// for slew, only enable it when the drive distance is greater then the slew distance + a few inches
chassis.pid_drive_set(24_in, DRIVE_SPEED, true);
chassis.pid_wait();
chassis.pid_drive_set(-12_in, DRIVE_SPEED);
chassis.pid_wait();
chassis.pid_drive_set(-12_in, DRIVE_SPEED);
chassis.pid_wait();
}
///
// Turn Example
///
void turn_example() {
// The first parameter is target degrees
// The second parameter is max speed the robot will drive at
chassis.pid_turn_set(90_deg, TURN_SPEED);
chassis.pid_wait();
chassis.pid_turn_set(45_deg, TURN_SPEED);
chassis.pid_wait();
chassis.pid_turn_set(0_deg, TURN_SPEED);
chassis.pid_wait();
}
///
// Combining Turn + Drive
///
void drive_and_turn() {
chassis.pid_drive_set(24_in, DRIVE_SPEED, true);
chassis.pid_wait();
chassis.pid_turn_set(45_deg, TURN_SPEED);
chassis.pid_wait();
chassis.pid_turn_set(-45_deg, TURN_SPEED);
chassis.pid_wait();
chassis.pid_turn_set(0_deg, TURN_SPEED);
chassis.pid_wait();
chassis.pid_drive_set(-24_in, DRIVE_SPEED, true);
chassis.pid_wait();
}
///
// Wait Until and Changing Max Speed
///
void wait_until_change_speed() {
// pid_wait_until will wait until the robot gets to a desired position
// When the robot gets to 6 inches, the robot will travel the remaining distance at a max speed of 30
chassis.pid_drive_set(24_in, DRIVE_SPEED, true);
chassis.pid_wait_until(6_in);
chassis.pid_speed_max_set(30); // After driving 6 inches at DRIVE_SPEED, the robot will go the remaining distance at 30 speed
chassis.pid_wait();
chassis.pid_turn_set(45_deg, TURN_SPEED);
chassis.pid_wait();
chassis.pid_turn_set(-45_deg, TURN_SPEED);
chassis.pid_wait();
chassis.pid_turn_set(0_deg, TURN_SPEED);
chassis.pid_wait();
// When the robot gets to -6 inches, the robot will travel the remaining distance at a max speed of 30
chassis.pid_drive_set(-24_in, DRIVE_SPEED, true);
chassis.pid_wait_until(-6_in);
chassis.pid_speed_max_set(30); // After driving 6 inches at DRIVE_SPEED, the robot will go the remaining distance at 30 speed
chassis.pid_wait();
}
///
// Swing Example
///
void swing_example() {
// The first parameter is ez::LEFT_SWING or ez::RIGHT_SWING
// The second parameter is target degrees
// The third parameter is speed of the moving side of the drive
// The fourth parameter is the speed of the still side of the drive, this allows for wider arcs
chassis.pid_swing_set(ez::LEFT_SWING, 45_deg, SWING_SPEED, 45);
chassis.pid_wait();
chassis.pid_swing_set(ez::RIGHT_SWING, 0_deg, SWING_SPEED, 45);
chassis.pid_wait();
chassis.pid_swing_set(ez::RIGHT_SWING, 45_deg, SWING_SPEED, 45);
chassis.pid_wait();
chassis.pid_swing_set(ez::LEFT_SWING, 0_deg, SWING_SPEED, 45);
chassis.pid_wait();
}
///
// Auto that tests everything
///
void combining_movements() {
chassis.pid_drive_set(24_in, DRIVE_SPEED, true);
chassis.pid_wait();
chassis.pid_turn_set(45_deg, TURN_SPEED);
chassis.pid_wait();
chassis.pid_swing_set(ez::RIGHT_SWING, -45_deg, SWING_SPEED, 45);
chassis.pid_wait();
chassis.pid_turn_set(0_deg, TURN_SPEED);
chassis.pid_wait();
chassis.pid_drive_set(-24_in, DRIVE_SPEED, true);
chassis.pid_wait();
}
///
// Interference example
///
void tug(int attempts) {
for (int i = 0; i < attempts - 1; i++) {
// Attempt to drive backwards
printf("i - %i", i);
chassis.pid_drive_set(-12_in, 127);
chassis.pid_wait();
// If failsafed...
if (chassis.interfered) {
chassis.drive_sensor_reset();
chassis.pid_drive_set(-2_in, 20);
pros::delay(1000);
}
// If robot successfully drove back, return
else {
return;
}
}
}
// If there is no interference, robot will drive forward and turn 90 degrees.
// If interfered, robot will drive forward and then attempt to drive backwards.
void interfered_example() {
chassis.pid_drive_set(24_in, DRIVE_SPEED, true);
chassis.pid_wait();
if (chassis.interfered) {
tug(3);
return;
}
chassis.pid_turn_set(90_deg, TURN_SPEED);
chassis.pid_wait();
}
// . . .
// Make your own autonomous functions here!
// . . .

132
src/main.cpp Normal file
View File

@@ -0,0 +1,132 @@
#include "main.h"
using namespace ez;
Drive chassis (
{-11, -18, 20}, // Left motor ports.
{1,8 ,-10}, // Right motor ports.
7, // IMU port.
3.25, // Wheel size.
600, // Ticks per rotation of encoder.
1.6667 // External ratio.
// TODO: Add tracking wheel ports.
);
/*
Runs initialization code. This occurs as soon as the program is started.
All other competition modes are blocked by initialize; it is recommended to
keep execution time for this mode under a few seconds.
*/
void initialize() {
pros::delay(500); // Stop the user from doing anything while legacy ports
// configure.
chassis.opcontrol_curve_buttons_toggle(true); // Enables modifying the
// controller curve with
// buttons on the joysticks.
chassis.opcontrol_drive_activebrake_set(0); // Sets the active brake kP. We
// recommend 2.
chassis.opcontrol_curve_default_set(0, 0); // Defaults for curve. If using
// tank, only the first
// parameter is used. (Comment
// this line out if you have an
// SD card!).
default_constants(); // Set the drive to your own constants from autons.cpp!
#if 0
as::auton_selector.autons_add({
Auton("Example Drive\n\nDrive forward and come back.", drive_example),
Auton("Example Turn\n\nTurn 3 times.", turn_example),
Auton("Drive and Turn\n\nDrive forward, turn, come back. ", drive_and_turn),
Auton("Drive and Turn\n\nSlow down during drive.", wait_until_change_speed),
Auton("Swing Example\n\nSwing in an 'S' curve", swing_example),
Auton("Combine all 3 movements", combining_movements),
Auton("Interference\n\nAfter driving forward, robot performs differently if interfered or not.", interfered_example),
});
#endif
chassis.initialize();
as::initialize();
master.rumble(".");
}
/*
Runs while the robot is in the disabled state of Field Management System or
the VEX Competition Switch, following either autonomous or opcontrol. When
the robot is enabled, this task will exit.
*/
void disabled() { }
/*
Runs after initialize(), and before autonomous when connected to the Field
Management System or the VEX Competition Switch. This is intended for
competition-specific initialization routines, such as an autonomous
selector on the LCD.
This task will exit when the robot is enabled and autonomous or opcontrol
starts.
*/
void competition_initialize() { }
/*
Runs the user autonomous code. This function will be started in its own
task with the default priority and stack size whenever the robot is enabled
via the Field Management System or the VEX Competition Switch in the
autonomous mode. Alternatively, this function may be called in initialize
or opcontrol for non-competition testing purposes.
If the robot is disabled or communications is lost, the autonomous task
will be stopped. Re-enabling the robot will restart the task, not re-start
it from where it left off.
*/
void autonomous() {
chassis.pid_targets_reset(); // Resets PID targets to 0.
chassis.drive_imu_reset(); // Reset gyro position to 0.
chassis.drive_sensor_reset(); // Reset drive sensors to 0.
chassis.drive_brake_set(MOTOR_BRAKE_HOLD); // Set motors to hold. This
// helps autonomous consistency.
int dist = 48;
chassis.pid_drive_set(dist, 127, true);
chassis.pid_wait_until(dist - 5);
int dist2 = -(dist-6);
chassis.pid_drive_set(dist2, 127, true);
chassis.pid_wait_until(dist2 + 5);
}
/*
Runs the operator control code. This function will be started in its own
task with the default priority and stack size whenever the robot is enabled
via the Field Management System or the VEX Competition Switch in the
operator control mode.
If no competition control is connected, this function will run immediately
following initialize().
If the robot is disabled or communications is lost, the operator control
task will be stopped. Re-enabling the robot will restart the task, not
resume it from where it left off.
*/
void opcontrol() {
chassis.drive_brake_set(MOTOR_BRAKE_COAST);
//swing_example();
while (true) {
if (!pros::competition::is_connected()) {
if (master.get_digital_new_press(DIGITAL_X))
chassis.pid_tuner_toggle();
if (master.get_digital_new_press(DIGITAL_B))
autonomous();
chassis.pid_tuner_iterate(); // Allow PID Tuner to iterate.
}
chassis.opcontrol_arcade_standard(SPLIT); // Tank control.
pros::delay(util::DELAY_TIME); // This is used for timer calculations!
// Keep this util::DELAY_TIME.
}
}