#include "main.h"
/* Documentation */
// https://ez-robotics.github.io/EZ-Template/
// Chassis constructor, edit accordingly
ez::Drive chassis(
// These are your drive motors, the first motor is used for sensing!
{-1, -2, 8}, // Left Chassis Ports, (use negative numbers for reversed motors!)
{4, 5, -6}, // Right Chassis Ports (use negative numbers for reversed motors!)
7, // IMU (inertial) port
3.25, // Wheel Diameter (Remember, 4" wheels without screw holes are actually 4.125!)
360 // Wheel RPM
);
/**
* 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()
{
ez::ez_template_print();
pros::delay(500); // Stop the user from doing anything while legacy ports configure
// Configure your chassis controls
// Enables modifying the controller curve with buttons on the joysticks
chassis.opcontrol_curve_buttons_toggle(true);
// Sets the active brake kP. We recommend ~2. 0 will disable.
chassis.opcontrol_drive_activebrake_set(2);
// Defaults for curve. If using tank, only the first parameter is used.
// (Comment this line out if you have an SD card!)
chassis.opcontrol_curve_default_set(0, 0);
// Set the constants using the function defined in autons.cpp
default_constants();
// Autonomous Selector
ez::as::auton_selector.autons_add(
{
Auton("Draw Right-Handed Square\nPLEASE DONT ACTUALLY RUN THIS DURING COMPETITION", draw_square),
Auton("RED Left Side\n\nSetup on 3rd from left\n\nBack lined up with inner forward edge\n\nWall riders lined up with inner left edge", red_left),
Auton("RED Right Side\n\nSetup on 2nd from right\n\nBack lined up with inner forward edge\n\nWall riders lined up with inner left edge", red_right),
Auton("BLUE Right Side\n\nSetup on 3nd from right\n\nBack lined up with inner forward edge\n\nWall riders lined up with inner right edge", blue_right),
Auton("RED Right Side\n\nSetup on 2nd from left\n\nBack lined up with inner forward edge\n\nWall riders lined up with inner right edge", blue_left)
}
);
// Initialize chassis and auton selector
chassis.initialize();
ez::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()
{
// Add your code here
}
/**
* 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()
{
// Add your code here
}
/**
* 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
// Calls selected auton from autonomous selector
ez::as::auton_selector.selected_auton_call();
}
/**
* 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 neutralscore(){
chassis.drive_angle_set(0);
while(FrontDistance.get()>230){
chassis.pid_drive_set(3,70);
chassis.pid_wait_quick();
pros::delay(300);
}
}
void alliancescore(){
chassis.drive_angle_set(0);
while(FrontDistance.get()>230){
chassis.pid_drive_set(3,70);
chassis.pid_wait_quick();
pros::delay(300);
}
}
void opcontrol()
{
// This is preference to what you like to drive on
// MOTOR_BRAKE_HOLD (recommended), MOTOR_BRAKE_BRAKE, MOTOR_BRAKE_COAST
pros::motor_brake_mode_e_t driver_preference_brake = MOTOR_BRAKE_COAST;
// chassis.opcontrol_tank(); // Tank control
chassis.opcontrol_arcade_standard(ez::SPLIT); // Standard split arcade USE THIS
// chassis.opcontrol_arcade_standard(ez::SINGLE); // Standard single arcade
// chassis.opcontrol_arcade_flipped(ez::SPLIT); // Flipped split arcade
// chassis.opcontrol_arcade_flipped(ez::SINGLE); // Flipped single arcade
chassis.drive_brake_set(driver_preference_brake);
bool clampstate=0;
Clamp.set_value(false);
bool lifterstate;
Lifter.set_value(false);
Arm1.move_velocity(0);
Arm2.move_velocity(0);
Arm1.set_brake_mode(MOTOR_BRAKE_HOLD);
Arm2.set_brake_mode(MOTOR_BRAKE_HOLD);
Arm1.tare_position();
Arm2.tare_position();
Arm1.set_encoder_units(MOTOR_ENCODER_DEGREES);
Arm2.set_encoder_units(MOTOR_ENCODER_DEGREES);
int armstate = 0;
int armtarget = 1;
// 1 = neutral stake
// 0 = alliance stake
while (true)
{
// Intake Control
if (master.get_digital(pros::E_CONTROLLER_DIGITAL_R1)){
Intake.move_velocity(200);
}
else if (master.get_digital(pros::E_CONTROLLER_DIGITAL_R2)){
Intake.move_velocity(-200);
}
else{
Intake.move_velocity(0);
}
// Arm Auto Control
if (master.get_digital_new_press(pros::E_CONTROLLER_DIGITAL_L2)){
if (armstate==1){
armstate=-1;
}
else if(armstate=-1){
armstate=1;
}
}
if (armstate==1){
if (armtarget==0){
if (Arm1.get_position()>=470){
Arm1.move_velocity(0);
Arm2.move_velocity(0);
}
else{
Arm1.move_velocity(200);
Arm2.move_velocity(-200);
}
}
else{
if (Arm1.get_position()>=670){
Arm1.move_velocity(0);
Arm2.move_velocity(0);
}
else{
Arm1.move_velocity(200);
Arm2.move_velocity(-200);
}
}
}
if (armstate==-1){
if (Arm1.get_position()<=0){
Arm1.move_velocity(0);
Arm2.move_velocity(0);
}
else{
Arm1.move_velocity(-100);
Arm2.move_velocity(100);
}
}
// Clamp Control
if (master.get_digital_new_press(pros::E_CONTROLLER_DIGITAL_L1)){
if (clampstate==1){
clampstate=0;
}
else{
clampstate=1;
}
Clamp.set_value(clampstate);
}
// Arm Maunual Mode Control
if (master.get_digital_new_press(pros::E_CONTROLLER_DIGITAL_UP)){
if (armtarget==1){
armtarget=0;
}
else{
armtarget=1;
}
}
// Arm Align Automatic Control
if (master.get_digital_new_press(pros::E_CONTROLLER_DIGITAL_DOWN)){
if (armtarget==1){
neutralscore();
}
else{
alliancescore();
}
}
// Lifter Control
if (master.get_digital_new_press(pros::E_CONTROLLER_DIGITAL_X)){
if (lifterstate==1){
lifterstate=0;
}
else{
lifterstate=1;
}
Lifter.set_value(lifterstate);
}
//TESTING ONLY
if (master.get_digital(DIGITAL_B) && master.get_digital(DIGITAL_DOWN)){
autonomous();
}
// This is used for timer calculations! Keep this ez::util::DELAY_TIME
pros::delay(ez::util::DELAY_TIME);
}
}
Main file ^^^
#include "main.h"
/* Documentation */
// https://ez-robotics.github.io/EZ-Template/
/**
* Sets the speed you drive, turn, and swing at
* during autonomous. Values range from 0-127.
* I suggest against going above 100, as it will
* burn out your motors very quickly.
*/
const int DRIVE_SPEED = 90;
const int TURN_SPEED = 90;
const int SWING_SPEED = 90;
// PID Constants
// Adjust accordingly, read documentation for more information
//
void default_constants() {
chassis.pid_heading_constants_set(5, 0, 20);
chassis.pid_drive_constants_set(20, 0, 100);
chassis.pid_turn_constants_set(3, 0.05, 20, 15);
chassis.pid_swing_constants_set(6, 0, 65);
chassis.pid_turn_exit_condition_set(80_ms, 3_deg, 250_ms, 7_deg, 500_ms, 500_ms);
chassis.pid_swing_exit_condition_set(80_ms, 3_deg, 250_ms, 7_deg, 500_ms, 500_ms);
chassis.pid_drive_exit_condition_set(80_ms, 1_in, 250_ms, 3_in, 500_ms, 500_ms);
chassis.pid_turn_chain_constant_set(3_deg);
chassis.pid_swing_chain_constant_set(5_deg);
chassis.pid_drive_chain_constant_set(3_in);
chassis.slew_drive_constants_set(7_in, 80);
}
// WRITE ACTUAL FUNCTIONS HERE
// Add your autonomous functions here
void deploy(){
bool clampstate=0;
Clamp.set_value(false);
Lifter.set_value(1);
Arm1.move_velocity(0);
Arm2.move_velocity(0);
Arm1.set_brake_mode(MOTOR_BRAKE_HOLD);
Arm2.set_brake_mode(MOTOR_BRAKE_HOLD);
Arm1.tare_position();
Arm2.tare_position();
Arm1.set_encoder_units(MOTOR_ENCODER_DEGREES);
Arm2.set_encoder_units(MOTOR_ENCODER_DEGREES);
}
void draw_square(){
for(int i=0;i<4;i++){
chassis.pid_drive_set(24,100);
chassis.pid_wait();
chassis.pid_turn_relative_set(90,70);
chassis.pid_wait();
}
}
void red_left(){
chassis.pid_drive_set(-22,100);
chassis.pid_wait();
chassis.pid_turn_relative_set(-50,70);
chassis.pid_wait();
Clamp.set_value(0);
chassis.pid_drive_set(-14,70);
chassis.pid_wait();
Clamp.set_value(1);
Intake.move_velocity(200);
Lifter.set_value(1);
chassis.pid_drive_set(26,100);
chassis.pid_wait();
Lifter.set_value(0);
pros::delay(750);
chassis.pid_drive_set(-5,100);
chassis.pid_wait();
chassis.pid_turn_set(-90,70);
chassis.pid_wait();
chassis.pid_drive_set(-40,100);
chassis.pid_wait();
chassis.pid_turn_relative_set(-100,70);
chassis.pid_wait();
chassis.pid_drive_set(27,100);
chassis.pid_wait();
pros::delay(250);
chassis.pid_drive_set(-10,100);
chassis.pid_wait();
}
void red_right(){
chassis.pid_drive_set(-22,100);
chassis.pid_wait();
chassis.pid_turn_relative_set(-50,70);
chassis.pid_wait();
Clamp.set_value(0);
chassis.pid_drive_set(-14,70);
chassis.pid_wait();
Clamp.set_value(1);
chassis.pid_turn_set(-90,70);
chassis.pid_wait();
Intake.move_velocity(200);
chassis.pid_drive_set(20,100);
chassis.pid_wait();
pros::delay(500);
chassis.pid_drive_set(-8,70);
chassis.pid_wait();
pros::delay(500);
Intake.move_velocity(0);
}
void blue_right(){
chassis.pid_drive_set(-22,100);
chassis.pid_wait();
chassis.pid_turn_relative_set(50,70);
chassis.pid_wait();
Clamp.set_value(0);
chassis.pid_drive_set(-14,70);
chassis.pid_wait();
Clamp.set_value(1);
Intake.move_velocity(200);
Lifter.set_value(1);
chassis.pid_drive_set(26,100);
chassis.pid_wait();
Lifter.set_value(0);
pros::delay(750);
chassis.pid_drive_set(-5,100);
chassis.pid_wait();
chassis.pid_turn_set(90,70);
chassis.pid_wait();
chassis.pid_drive_set(-40,100);
chassis.pid_wait();
chassis.pid_turn_relative_set(100,70);
chassis.pid_wait();
chassis.pid_drive_set(27,100);
chassis.pid_wait();
pros::delay(250);
chassis.pid_drive_set(-10,100);
chassis.pid_wait();
}
void blue_left(){
chassis.pid_drive_set(-22,100);
chassis.pid_wait();
chassis.pid_turn_relative_set(50,70);
chassis.pid_wait();
Clamp.set_value(0);
chassis.pid_drive_set(-14,70);
chassis.pid_wait();
Clamp.set_value(1);
chassis.pid_turn_set(90,70);
chassis.pid_wait();
Intake.move_velocity(200);
chassis.pid_drive_set(20,100);
chassis.pid_wait();
pros::delay(500);
chassis.pid_drive_set(-8,70);
chassis.pid_wait();
pros::delay(500);
Intake.move_velocity(0);
}
Autonomous functions ^^^
Sorry for not including code in my original post, I was somewhat panicked and didn’t think before posting.