From 6dccd05e79d5a2feccecf69959807dfdc3ba8290 Mon Sep 17 00:00:00 2001 From: Ethan Stacy Date: Sun, 4 Feb 2024 19:18:32 -0700 Subject: [PATCH] Main teleop working --- project.pros | 623 ++++++++++++++++++++++++++------------------------- src/main.cpp | 156 +++++++++---- src/ports.h | 4 +- 3 files changed, 430 insertions(+), 353 deletions(-) diff --git a/project.pros b/project.pros index f13eeb4..9450107 100644 --- a/project.pros +++ b/project.pros @@ -1,316 +1,317 @@ { - "py/object": "pros.conductor.project.Project", - "py/state": { - "project_name": "ORDGR_15", + "py/object": "pros.conductor.project.Project", + "py/state": { + "project_name": "ORDGR_15", + "target": "v5", + "templates": { + "kernel": { + "location": "/home/ethan/.config/pros/templates/kernel@3.8.0", + "metadata": { + "cold_addr": "58720256", + "cold_output": "bin/cold.package.bin", + "hot_addr": "125829120", + "hot_output": "bin/hot.package.bin", + "origin": "pros-mainline", + "output": "bin/monolith.bin" + }, + "name": "kernel", + "py/object": "pros.conductor.templates.local_template.LocalTemplate", + "supported_kernels": null, + "system_files": [ + "include/display/lv_core/lv_vdb.h", + "include/display/lv_core/lv_core.mk", + "include/display/lv_misc/lv_math.h", + "include/display/lv_objx/lv_tabview.h", + "include/display/lv_misc/lv_color.h", + "include/display/lv_hal/lv_hal_indev.h", + "include/display/lv_fonts/lv_fonts.mk", + "include/display/lv_misc/lv_symbol_def.h", + "include/display/lv_hal/lv_hal.mk", + "include/display/lv_themes/lv_theme_night.h", + "include/display/lv_draw/lv_draw_triangle.h", + "include/pros/optical.hpp", + "include/display/lv_draw/lv_draw_vbasic.h", + "include/display/lv_objx/lv_objx_templ.h", + "include/display/lv_core/lv_refr.h", + "include/pros/link.hpp", + "include/display/lv_objx/lv_btnm.h", + "include/display/lv_objx/lv_cb.h", + "firmware/v5-common.ld", + "include/pros/ext_adi.h", + "include/pros/rotation.h", + "include/display/lv_objx/lv_spinbox.h", + "include/display/lv_misc/lv_circ.h", + "include/display/lv_misc/lv_mem.h", + "include/display/lv_objx/lv_page.h", + "include/display/lv_objx/lv_ddlist.h", + "include/display/lv_core/lv_group.h", + "include/display/lvgl.h", + "include/display/lv_objx/lv_chart.h", + "include/pros/distance.h", + "include/display/lv_objx/lv_list.h", + "include/pros/vision.h", + "include/pros/misc.hpp", + "include/display/lv_draw/lv_draw.h", + "include/display/lv_objx/lv_label.h", + "include/display/lv_misc/lv_font.h", + "include/display/lv_draw/lv_draw_img.h", + "include/display/lv_misc/lv_log.h", + "include/display/lv_misc/lv_templ.h", + "include/pros/llemu.h", + "include/display/lv_objx/lv_btn.h", + "include/display/lv_fonts/lv_font_builtin.h", + "include/display/lv_objx/lv_calendar.h", + "firmware/libm.a", + "include/display/lv_conf.h", + "include/display/lv_objx/lv_sw.h", + "include/display/lv_draw/lv_draw_rect.h", + "include/display/lv_objx/lv_cont.h", + "include/pros/vision.hpp", + "include/display/lv_objx/lv_mbox.h", + "include/pros/adi.hpp", + "include/pros/imu.hpp", + "include/display/lv_objx/lv_table.h", + "include/pros/screen.hpp", + "include/display/lv_draw/lv_draw_label.h", + "include/display/lv_misc/lv_txt.h", + "include/pros/api_legacy.h", + "include/display/lv_objx/lv_lmeter.h", + "include/display/lv_themes/lv_theme_templ.h", + "include/pros/apix.h", + "include/display/lv_draw/lv_draw.mk", + "include/display/lv_themes/lv_theme_alien.h", + "include/pros/colors.hpp", + "include/display/lv_objx/lv_img.h", + "firmware/libc.a", + "include/display/lv_themes/lv_theme_zen.h", + "include/display/lv_themes/lv_theme_material.h", + "include/display/lv_draw/lv_draw_arc.h", + "include/display/lv_themes/lv_theme_mono.h", + "include/display/lv_themes/lv_themes.mk", + "include/display/lv_objx/lv_slider.h", + "include/pros/serial.h", + "include/display/lv_themes/lv_theme.h", + "include/display/README.md", + "include/display/lv_objx/lv_canvas.h", + "include/pros/misc.h", + "include/display/lv_misc/lv_fs.h", + "include/pros/rtos.h", + "include/display/lv_core/lv_indev.h", + "include/pros/motors.hpp", + "include/display/lv_core/lv_style.h", + "include/display/lv_version.h", + "include/display/lv_core/lv_lang.h", + "include/api.h", + "include/display/lv_objx/lv_gauge.h", + "include/pros/rtos.hpp", + "include/display/lv_hal/lv_hal_disp.h", + "include/pros/motors.h", + "include/display/lv_objx/lv_led.h", + "include/display/lv_draw/lv_draw_rbasic.h", + "include/display/lv_objx/lv_kb.h", + "include/display/lv_conf_checker.h", + "include/display/lv_hal/lv_hal.h", + "include/display/lv_draw/lv_draw_line.h", + "include/pros/gps.hpp", + "include/display/lv_objx/lv_objx.mk", + "include/display/lv_objx/lv_win.h", + "include/display/lv_core/lv_obj.h", + "include/display/lv_objx/lv_arc.h", + "include/pros/link.h", + "include/display/lv_objx/lv_preload.h", + "include/display/lv_misc/lv_area.h", + "include/display/lv_misc/lv_ll.h", + "include/pros/optical.h", + "include/pros/serial.hpp", + "include/pros/screen.h", + "include/display/lv_themes/lv_theme_nemo.h", + "include/pros/llemu.hpp", + "firmware/libpros.a", + "include/display/lv_misc/lv_gc.h", + "include/display/lv_misc/lv_anim.h", + "include/display/lv_objx/lv_line.h", + "include/pros/distance.hpp", + "include/pros/rotation.hpp", + "include/pros/error.h", + "include/display/lv_objx/lv_tileview.h", + "include/pros/gps.h", + "include/display/lv_misc/lv_task.h", + "include/pros/imu.h", + "firmware/v5-hot.ld", + "include/display/lv_misc/lv_misc.mk", + "include/pros/colors.h", + "common.mk", + "include/display/lv_objx/lv_roller.h", + "include/display/lv_objx/lv_bar.h", + "include/display/lv_themes/lv_theme_default.h", + "firmware/v5.ld", + "include/display/lv_misc/lv_ufs.h", + "include/display/lv_hal/lv_hal_tick.h", + "include/display/licence.txt", + "include/display/lv_objx/lv_ta.h", + "include/display/lv_objx/lv_imgbtn.h", + "include/pros/adi.h" + ], "target": "v5", - "templates": { - "kernel": { - "location": "/home/ethan/.config/pros/templates/kernel@3.8.0", - "metadata": { - "cold_addr": "58720256", - "cold_output": "bin/cold.package.bin", - "hot_addr": "125829120", - "hot_output": "bin/hot.package.bin", - "origin": "pros-mainline", - "output": "bin/monolith.bin" - }, - "name": "kernel", - "py/object": "pros.conductor.templates.local_template.LocalTemplate", - "supported_kernels": null, - "system_files": [ - "include/display/lv_core/lv_vdb.h", - "include/display/lv_core/lv_core.mk", - "include/display/lv_misc/lv_math.h", - "include/display/lv_objx/lv_tabview.h", - "include/display/lv_misc/lv_color.h", - "include/display/lv_hal/lv_hal_indev.h", - "include/display/lv_fonts/lv_fonts.mk", - "include/display/lv_misc/lv_symbol_def.h", - "include/display/lv_hal/lv_hal.mk", - "include/display/lv_themes/lv_theme_night.h", - "include/display/lv_draw/lv_draw_triangle.h", - "include/pros/optical.hpp", - "include/display/lv_draw/lv_draw_vbasic.h", - "include/display/lv_objx/lv_objx_templ.h", - "include/display/lv_core/lv_refr.h", - "include/pros/link.hpp", - "include/display/lv_objx/lv_btnm.h", - "include/display/lv_objx/lv_cb.h", - "firmware/v5-common.ld", - "include/pros/ext_adi.h", - "include/pros/rotation.h", - "include/display/lv_objx/lv_spinbox.h", - "include/display/lv_misc/lv_circ.h", - "include/display/lv_misc/lv_mem.h", - "include/display/lv_objx/lv_page.h", - "include/display/lv_objx/lv_ddlist.h", - "include/display/lv_core/lv_group.h", - "include/display/lvgl.h", - "include/display/lv_objx/lv_chart.h", - "include/pros/distance.h", - "include/display/lv_objx/lv_list.h", - "include/pros/vision.h", - "include/pros/misc.hpp", - "include/display/lv_draw/lv_draw.h", - "include/display/lv_objx/lv_label.h", - "include/display/lv_misc/lv_font.h", - "include/display/lv_draw/lv_draw_img.h", - "include/display/lv_misc/lv_log.h", - "include/display/lv_misc/lv_templ.h", - "include/pros/llemu.h", - "include/display/lv_objx/lv_btn.h", - "include/display/lv_fonts/lv_font_builtin.h", - "include/display/lv_objx/lv_calendar.h", - "firmware/libm.a", - "include/display/lv_conf.h", - "include/display/lv_objx/lv_sw.h", - "include/display/lv_draw/lv_draw_rect.h", - "include/display/lv_objx/lv_cont.h", - "include/pros/vision.hpp", - "include/display/lv_objx/lv_mbox.h", - "include/pros/adi.hpp", - "include/pros/imu.hpp", - "include/display/lv_objx/lv_table.h", - "include/pros/screen.hpp", - "include/display/lv_draw/lv_draw_label.h", - "include/display/lv_misc/lv_txt.h", - "include/pros/api_legacy.h", - "include/display/lv_objx/lv_lmeter.h", - "include/display/lv_themes/lv_theme_templ.h", - "include/pros/apix.h", - "include/display/lv_draw/lv_draw.mk", - "include/display/lv_themes/lv_theme_alien.h", - "include/pros/colors.hpp", - "include/display/lv_objx/lv_img.h", - "firmware/libc.a", - "include/display/lv_themes/lv_theme_zen.h", - "include/display/lv_themes/lv_theme_material.h", - "include/display/lv_draw/lv_draw_arc.h", - "include/display/lv_themes/lv_theme_mono.h", - "include/display/lv_themes/lv_themes.mk", - "include/display/lv_objx/lv_slider.h", - "include/pros/serial.h", - "include/display/lv_themes/lv_theme.h", - "include/display/README.md", - "include/display/lv_objx/lv_canvas.h", - "include/pros/misc.h", - "include/display/lv_misc/lv_fs.h", - "include/pros/rtos.h", - "include/display/lv_core/lv_indev.h", - "include/pros/motors.hpp", - "include/display/lv_core/lv_style.h", - "include/display/lv_version.h", - "include/display/lv_core/lv_lang.h", - "include/api.h", - "include/display/lv_objx/lv_gauge.h", - "include/pros/rtos.hpp", - "include/display/lv_hal/lv_hal_disp.h", - "include/pros/motors.h", - "include/display/lv_objx/lv_led.h", - "include/display/lv_draw/lv_draw_rbasic.h", - "include/display/lv_objx/lv_kb.h", - "include/display/lv_conf_checker.h", - "include/display/lv_hal/lv_hal.h", - "include/display/lv_draw/lv_draw_line.h", - "include/pros/gps.hpp", - "include/display/lv_objx/lv_objx.mk", - "include/display/lv_objx/lv_win.h", - "include/display/lv_core/lv_obj.h", - "include/display/lv_objx/lv_arc.h", - "include/pros/link.h", - "include/display/lv_objx/lv_preload.h", - "include/display/lv_misc/lv_area.h", - "include/display/lv_misc/lv_ll.h", - "include/pros/optical.h", - "include/pros/serial.hpp", - "include/pros/screen.h", - "include/display/lv_themes/lv_theme_nemo.h", - "include/pros/llemu.hpp", - "firmware/libpros.a", - "include/display/lv_misc/lv_gc.h", - "include/display/lv_misc/lv_anim.h", - "include/display/lv_objx/lv_line.h", - "include/pros/distance.hpp", - "include/pros/rotation.hpp", - "include/pros/error.h", - "include/display/lv_objx/lv_tileview.h", - "include/pros/gps.h", - "include/display/lv_misc/lv_task.h", - "include/pros/imu.h", - "firmware/v5-hot.ld", - "include/display/lv_misc/lv_misc.mk", - "include/pros/colors.h", - "common.mk", - "include/display/lv_objx/lv_roller.h", - "include/display/lv_objx/lv_bar.h", - "include/display/lv_themes/lv_theme_default.h", - "firmware/v5.ld", - "include/display/lv_misc/lv_ufs.h", - "include/display/lv_hal/lv_hal_tick.h", - "include/display/licence.txt", - "include/display/lv_objx/lv_ta.h", - "include/display/lv_objx/lv_imgbtn.h", - "include/pros/adi.h" - ], - "target": "v5", - "user_files": [ - "src/main.cc", - ".gitignore", - "Makefile", - "src/main.cpp", - "src/main.c", - "include/main.hpp", - "include/main.hh", - "include/main.h" - ], - "version": "3.8.0" - }, - "okapilib": { - "location": "/home/ethan/.config/pros/templates/okapilib@4.8.0", - "metadata": { - "origin": "pros-mainline" - }, - "name": "okapilib", - "py/object": "pros.conductor.templates.local_template.LocalTemplate", - "supported_kernels": "^3.3.1", - "system_files": [ - "include/okapi/api/odometry/odomMath.hpp", - "include/okapi/squiggles/physicalmodel/physicalmodel.hpp", - "include/okapi/api/chassis/model/threeEncoderXDriveModel.hpp", - "include/okapi/api/chassis/model/hDriveModel.hpp", - "include/okapi/api/chassis/model/readOnlyChassisModel.hpp", - "include/okapi/api/device/button/buttonBase.hpp", - "include/okapi/api/units/QAngularJerk.hpp", - "include/okapi/api/control/iterative/iterativeController.hpp", - "include/okapi/api/control/controllerInput.hpp", - "include/okapi/squiggles/math/quinticpolynomial.hpp", - "include/okapi/api/device/rotarysensor/continuousRotarySensor.hpp", - "include/okapi/api/control/async/asyncPositionController.hpp", - "include/okapi/api/filter/passthroughFilter.hpp", - "include/okapi/api/device/rotarysensor/rotarySensor.hpp", - "include/okapi/api/odometry/stateMode.hpp", - "include/okapi/impl/device/controller.hpp", - "include/okapi/api/units/QTime.hpp", - "include/okapi/api/control/util/controllerRunner.hpp", - "include/okapi/api/units/QTorque.hpp", - "include/okapi/impl/device/rotarysensor/IMU.hpp", - "include/okapi/api/control/util/pidTuner.hpp", - "include/okapi/impl/util/timeUtilFactory.hpp", - "include/okapi/api/device/motor/abstractMotor.hpp", - "include/okapi/impl/device/button/controllerButton.hpp", - "include/okapi/impl/device/rotarysensor/integratedEncoder.hpp", - "include/okapi/impl/device/rotarysensor/adiEncoder.hpp", - "include/okapi/api/filter/composableFilter.hpp", - "include/okapi/api/chassis/model/xDriveModel.hpp", - "include/okapi/squiggles/geometry/pose.hpp", - "include/okapi/api/odometry/twoEncoderOdometry.hpp", - "include/okapi/api/util/timeUtil.hpp", - "firmware/squiggles.mk", - "include/okapi/api/control/async/asyncPosPidController.hpp", - "include/okapi/impl/control/util/controllerRunnerFactory.hpp", - "include/okapi/impl/chassis/controller/chassisControllerBuilder.hpp", - "include/okapi/api/control/closedLoopController.hpp", - "include/okapi/squiggles/math/utils.hpp", - "include/okapi/api/control/async/asyncVelIntegratedController.hpp", - "include/okapi/api/units/QLength.hpp", - "include/okapi/impl/device/motor/motor.hpp", - "include/okapi/api/odometry/odometry.hpp", - "include/okapi/api/units/RQuantity.hpp", - "include/okapi/api/units/QArea.hpp", - "include/okapi/api/filter/filter.hpp", - "include/okapi/api/device/button/abstractButton.hpp", - "include/okapi/api/units/QPressure.hpp", - "include/okapi/api/control/async/asyncLinearMotionProfileController.hpp", - "include/okapi/api/control/util/flywheelSimulator.hpp", - "include/okapi/api/util/logging.hpp", - "include/okapi/api/chassis/model/threeEncoderSkidSteerModel.hpp", - "include/okapi/api/filter/emaFilter.hpp", - "include/okapi/impl/device/controllerUtil.hpp", - "include/okapi/api/util/mathUtil.hpp", - "include/okapi/api/filter/averageFilter.hpp", - "include/okapi/api/control/iterative/iterativeVelocityController.hpp", - "include/okapi/api/odometry/point.hpp", - "include/okapi/api/control/async/asyncController.hpp", - "include/okapi/impl/control/async/asyncMotionProfileControllerBuilder.hpp", - "include/okapi/api/coreProsAPI.hpp", - "include/okapi/api/units/QSpeed.hpp", - "include/okapi/impl/control/util/pidTunerFactory.hpp", - "include/okapi/api/units/QAngle.hpp", - "include/okapi/api/control/async/asyncVelocityController.hpp", - "include/okapi/api/odometry/threeEncoderOdometry.hpp", - "include/okapi/api/filter/velMath.hpp", - "include/okapi/api/control/async/asyncPosIntegratedController.hpp", - "include/okapi/squiggles/constraints.hpp", - "include/okapi/api/filter/filteredControllerInput.hpp", - "include/okapi/api/chassis/controller/chassisControllerPid.hpp", - "include/okapi/api/control/iterative/iterativePositionController.hpp", - "include/okapi/api/control/offsettableControllerInput.hpp", - "include/okapi/squiggles/squiggles.hpp", - "include/okapi/api/units/QAngularSpeed.hpp", - "include/okapi/api/control/iterative/iterativePosPidController.hpp", - "include/okapi/squiggles/physicalmodel/tankmodel.hpp", - "include/okapi/impl/util/rate.hpp", - "include/okapi/api/filter/medianFilter.hpp", - "include/okapi/impl/device/rotarysensor/potentiometer.hpp", - "include/okapi/api/control/iterative/iterativeVelPidController.hpp", - "include/okapi/squiggles/geometry/profilepoint.hpp", - "include/okapi/impl/device/button/adiButton.hpp", - "include/okapi/impl/control/async/asyncVelControllerBuilder.hpp", - "include/okapi/api/filter/demaFilter.hpp", - "include/okapi/api/units/RQuantityName.hpp", - "include/okapi/api/control/util/pathfinderUtil.hpp", - "include/okapi/api/util/abstractTimer.hpp", - "include/okapi/api.hpp", - "include/okapi/impl/control/async/asyncPosControllerBuilder.hpp", - "include/okapi/api/chassis/controller/chassisScales.hpp", - "include/okapi/api/units/QMass.hpp", - "include/okapi/impl/device/rotarysensor/adiGyro.hpp", - "include/okapi/impl/util/configurableTimeUtilFactory.hpp", - "include/okapi/api/control/util/settledUtil.hpp", - "include/okapi/impl/control/iterative/iterativeControllerFactory.hpp", - "include/okapi/impl/util/timer.hpp", - "include/okapi/api/chassis/controller/defaultOdomChassisController.hpp", - "include/okapi/impl/device/motor/motorGroup.hpp", - "include/okapi/api/control/async/asyncWrapper.hpp", - "include/okapi/squiggles/io.hpp", - "include/okapi/api/filter/ekfFilter.hpp", - "include/okapi/api/control/async/asyncMotionProfileController.hpp", - "include/okapi/squiggles/geometry/controlvector.hpp", - "include/okapi/impl/device/distanceSensor.hpp", - "include/okapi/api/control/controllerOutput.hpp", - "include/okapi/api/units/QVolume.hpp", - "include/okapi/squiggles/spline.hpp", - "include/okapi/api/units/QJerk.hpp", - "include/okapi/api/chassis/controller/chassisController.hpp", - "include/okapi/squiggles/physicalmodel/passthroughmodel.hpp", - "include/okapi/api/units/QAngularAcceleration.hpp", - "include/okapi/api/chassis/controller/chassisControllerIntegrated.hpp", - "include/okapi/impl/device/adiUltrasonic.hpp", - "include/okapi/impl/device/opticalSensor.hpp", - "include/okapi/api/units/QForce.hpp", - "include/okapi/api/util/supplier.hpp", - "include/okapi/api/chassis/controller/odomChassisController.hpp", - "include/okapi/impl/filter/velMathFactory.hpp", - "include/okapi/api/control/iterative/iterativeMotorVelocityController.hpp", - "include/okapi/api/control/async/asyncVelPidController.hpp", - "include/okapi/api/odometry/odomState.hpp", - "include/okapi/api/chassis/model/chassisModel.hpp", - "include/okapi/api/units/QFrequency.hpp", - "include/okapi/impl/device/rotarysensor/rotationSensor.hpp", - "include/okapi/api/util/abstractRate.hpp", - "include/okapi/impl/device/motor/adiMotor.hpp", - "include/okapi/api/units/QAcceleration.hpp", - "include/okapi/api/chassis/model/skidSteerModel.hpp", - "firmware/okapilib.a" - ], - "target": "v5", - "user_files": [], - "version": "4.8.0" - } + "user_files": [ + "src/main.cc", + ".gitignore", + "Makefile", + "src/main.cpp", + "src/main.c", + "include/main.hpp", + "include/main.hh", + "include/main.h" + ], + "version": "3.8.0" + }, + "okapilib": { + "location": "/home/ethan/.config/pros/templates/okapilib@4.8.0", + "metadata": { + "origin": "pros-mainline" }, - "upload_options": { - "description": "15\" Bot Main", - "icon": "pros" - } + "name": "okapilib", + "py/object": "pros.conductor.templates.local_template.LocalTemplate", + "supported_kernels": "^3.3.1", + "system_files": [ + "include/okapi/api/odometry/odomMath.hpp", + "include/okapi/squiggles/physicalmodel/physicalmodel.hpp", + "include/okapi/api/chassis/model/threeEncoderXDriveModel.hpp", + "include/okapi/api/chassis/model/hDriveModel.hpp", + "include/okapi/api/chassis/model/readOnlyChassisModel.hpp", + "include/okapi/api/device/button/buttonBase.hpp", + "include/okapi/api/units/QAngularJerk.hpp", + "include/okapi/api/control/iterative/iterativeController.hpp", + "include/okapi/api/control/controllerInput.hpp", + "include/okapi/squiggles/math/quinticpolynomial.hpp", + "include/okapi/api/device/rotarysensor/continuousRotarySensor.hpp", + "include/okapi/api/control/async/asyncPositionController.hpp", + "include/okapi/api/filter/passthroughFilter.hpp", + "include/okapi/api/device/rotarysensor/rotarySensor.hpp", + "include/okapi/api/odometry/stateMode.hpp", + "include/okapi/impl/device/controller.hpp", + "include/okapi/api/units/QTime.hpp", + "include/okapi/api/control/util/controllerRunner.hpp", + "include/okapi/api/units/QTorque.hpp", + "include/okapi/impl/device/rotarysensor/IMU.hpp", + "include/okapi/api/control/util/pidTuner.hpp", + "include/okapi/impl/util/timeUtilFactory.hpp", + "include/okapi/api/device/motor/abstractMotor.hpp", + "include/okapi/impl/device/button/controllerButton.hpp", + "include/okapi/impl/device/rotarysensor/integratedEncoder.hpp", + "include/okapi/impl/device/rotarysensor/adiEncoder.hpp", + "include/okapi/api/filter/composableFilter.hpp", + "include/okapi/api/chassis/model/xDriveModel.hpp", + "include/okapi/squiggles/geometry/pose.hpp", + "include/okapi/api/odometry/twoEncoderOdometry.hpp", + "include/okapi/api/util/timeUtil.hpp", + "firmware/squiggles.mk", + "include/okapi/api/control/async/asyncPosPidController.hpp", + "include/okapi/impl/control/util/controllerRunnerFactory.hpp", + "include/okapi/impl/chassis/controller/chassisControllerBuilder.hpp", + "include/okapi/api/control/closedLoopController.hpp", + "include/okapi/squiggles/math/utils.hpp", + "include/okapi/api/control/async/asyncVelIntegratedController.hpp", + "include/okapi/api/units/QLength.hpp", + "include/okapi/impl/device/motor/motor.hpp", + "include/okapi/api/odometry/odometry.hpp", + "include/okapi/api/units/RQuantity.hpp", + "include/okapi/api/units/QArea.hpp", + "include/okapi/api/filter/filter.hpp", + "include/okapi/api/device/button/abstractButton.hpp", + "include/okapi/api/units/QPressure.hpp", + "include/okapi/api/control/async/asyncLinearMotionProfileController.hpp", + "include/okapi/api/control/util/flywheelSimulator.hpp", + "include/okapi/api/util/logging.hpp", + "include/okapi/api/chassis/model/threeEncoderSkidSteerModel.hpp", + "include/okapi/api/filter/emaFilter.hpp", + "include/okapi/impl/device/controllerUtil.hpp", + "include/okapi/api/util/mathUtil.hpp", + "include/okapi/api/filter/averageFilter.hpp", + "include/okapi/api/control/iterative/iterativeVelocityController.hpp", + "include/okapi/api/odometry/point.hpp", + "include/okapi/api/control/async/asyncController.hpp", + "include/okapi/impl/control/async/asyncMotionProfileControllerBuilder.hpp", + "include/okapi/api/coreProsAPI.hpp", + "include/okapi/api/units/QSpeed.hpp", + "include/okapi/impl/control/util/pidTunerFactory.hpp", + "include/okapi/api/units/QAngle.hpp", + "include/okapi/api/control/async/asyncVelocityController.hpp", + "include/okapi/api/odometry/threeEncoderOdometry.hpp", + "include/okapi/api/filter/velMath.hpp", + "include/okapi/api/control/async/asyncPosIntegratedController.hpp", + "include/okapi/squiggles/constraints.hpp", + "include/okapi/api/filter/filteredControllerInput.hpp", + "include/okapi/api/chassis/controller/chassisControllerPid.hpp", + "include/okapi/api/control/iterative/iterativePositionController.hpp", + "include/okapi/api/control/offsettableControllerInput.hpp", + "include/okapi/squiggles/squiggles.hpp", + "include/okapi/api/units/QAngularSpeed.hpp", + "include/okapi/api/control/iterative/iterativePosPidController.hpp", + "include/okapi/squiggles/physicalmodel/tankmodel.hpp", + "include/okapi/impl/util/rate.hpp", + "include/okapi/api/filter/medianFilter.hpp", + "include/okapi/impl/device/rotarysensor/potentiometer.hpp", + "include/okapi/api/control/iterative/iterativeVelPidController.hpp", + "include/okapi/squiggles/geometry/profilepoint.hpp", + "include/okapi/impl/device/button/adiButton.hpp", + "include/okapi/impl/control/async/asyncVelControllerBuilder.hpp", + "include/okapi/api/filter/demaFilter.hpp", + "include/okapi/api/units/RQuantityName.hpp", + "include/okapi/api/control/util/pathfinderUtil.hpp", + "include/okapi/api/util/abstractTimer.hpp", + "include/okapi/api.hpp", + "include/okapi/impl/control/async/asyncPosControllerBuilder.hpp", + "include/okapi/api/chassis/controller/chassisScales.hpp", + "include/okapi/api/units/QMass.hpp", + "include/okapi/impl/device/rotarysensor/adiGyro.hpp", + "include/okapi/impl/util/configurableTimeUtilFactory.hpp", + "include/okapi/api/control/util/settledUtil.hpp", + "include/okapi/impl/control/iterative/iterativeControllerFactory.hpp", + "include/okapi/impl/util/timer.hpp", + "include/okapi/api/chassis/controller/defaultOdomChassisController.hpp", + "include/okapi/impl/device/motor/motorGroup.hpp", + "include/okapi/api/control/async/asyncWrapper.hpp", + "include/okapi/squiggles/io.hpp", + "include/okapi/api/filter/ekfFilter.hpp", + "include/okapi/api/control/async/asyncMotionProfileController.hpp", + "include/okapi/squiggles/geometry/controlvector.hpp", + "include/okapi/impl/device/distanceSensor.hpp", + "include/okapi/api/control/controllerOutput.hpp", + "include/okapi/api/units/QVolume.hpp", + "include/okapi/squiggles/spline.hpp", + "include/okapi/api/units/QJerk.hpp", + "include/okapi/api/chassis/controller/chassisController.hpp", + "include/okapi/squiggles/physicalmodel/passthroughmodel.hpp", + "include/okapi/api/units/QAngularAcceleration.hpp", + "include/okapi/api/chassis/controller/chassisControllerIntegrated.hpp", + "include/okapi/impl/device/adiUltrasonic.hpp", + "include/okapi/impl/device/opticalSensor.hpp", + "include/okapi/api/units/QForce.hpp", + "include/okapi/api/util/supplier.hpp", + "include/okapi/api/chassis/controller/odomChassisController.hpp", + "include/okapi/impl/filter/velMathFactory.hpp", + "include/okapi/api/control/iterative/iterativeMotorVelocityController.hpp", + "include/okapi/api/control/async/asyncVelPidController.hpp", + "include/okapi/api/odometry/odomState.hpp", + "include/okapi/api/chassis/model/chassisModel.hpp", + "include/okapi/api/units/QFrequency.hpp", + "include/okapi/impl/device/rotarysensor/rotationSensor.hpp", + "include/okapi/api/util/abstractRate.hpp", + "include/okapi/impl/device/motor/adiMotor.hpp", + "include/okapi/api/units/QAcceleration.hpp", + "include/okapi/api/chassis/model/skidSteerModel.hpp", + "firmware/okapilib.a" + ], + "target": "v5", + "user_files": [], + "version": "4.8.0" + } + }, + "upload_options": { + "description": "15\" Bot Main", + "icon": "pros", + "slot": 1 } + } } \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index fe7fd4b..a54dc12 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,12 +1,13 @@ #include "main.h" -#include -#include - #include "ports.h" +#include "pros/misc.h" +#include "pros/motors.h" +#include "pros/rtos.hpp" #include "timer.h" #define MAX_VOLTAGE 127 +#define MAX_RPM 200 pros::Controller ctrl(pros::E_CONTROLLER_MASTER); @@ -18,6 +19,19 @@ pros::Motor_Group catapult_group(CATAPULT_DRIVE_PORTS); pros::Motor catapult_block(CATAPULT_STOPPER_PORT); +enum class CatapultDeployStatus { + NotDeploying, + RemoveBlock, + Home, + PullBackFirst, + PlaceBlock, + PullBackSecond, +}; + +CatapultDeployStatus catapult_deploy_status = + CatapultDeployStatus::NotDeploying; +Timer deploy_timer = Timer(); + /** * A callback function for LLEMU's center button. */ @@ -27,23 +41,65 @@ void on_center_button() right_drive_group = 0; } -void deploy_catapult() +void handle_catapult_deploy() { - catapult_group.brake(); - catapult_block.move(MAX_VOLTAGE); - pros::delay(100); - catapult_block.move(0); - catapult_group.move(-20); - while (catapult_group.get_actual_velocities()[0] > 5.0) { + switch (catapult_deploy_status) { + case CatapultDeployStatus::NotDeploying: + break; + case CatapultDeployStatus::RemoveBlock: + catapult_group.brake(); + catapult_block.move(MAX_VOLTAGE); + if (deploy_timer.GetElapsedTime().AsMilliseconds() >= 500.0) { + catapult_deploy_status = CatapultDeployStatus::Home; + deploy_timer.Restart(); + } + break; + case CatapultDeployStatus::Home: + catapult_block.move(0); + catapult_group.move(-75); + if (deploy_timer.GetElapsedTime().AsMilliseconds() < 100) + break; + if (catapult_group.get_actual_velocities()[0] > -10.0) { + catapult_deploy_status = + CatapultDeployStatus::PullBackFirst; + catapult_group.brake(); + catapult_group.tare_position(); + } + break; + case CatapultDeployStatus::PullBackFirst: + catapult_group.move_absolute(2600, MAX_RPM); + if (catapult_group.get_positions()[0] >= 2600) { + catapult_deploy_status = + CatapultDeployStatus::PlaceBlock; + deploy_timer.Restart(); + } + break; + case CatapultDeployStatus::PlaceBlock: + catapult_block.move(-MAX_VOLTAGE); + if (deploy_timer.GetElapsedTime().AsMilliseconds() >= 500.0 || + deploy_timer.GetElapsedTime().AsSeconds() > 5.0) { + catapult_deploy_status = + CatapultDeployStatus::PullBackSecond; + deploy_timer.Restart(); + } + break; + case CatapultDeployStatus::PullBackSecond: + catapult_block.brake(); + catapult_group.move_absolute(3000, MAX_RPM); + if (catapult_group.get_positions()[0] >= 3000 || + deploy_timer.GetElapsedTime().AsSeconds() > 2.0) { + catapult_deploy_status = + CatapultDeployStatus::NotDeploying; + catapult_group.brake(); + } + break; } +} - catapult_group.brake(); - catapult_group.set_zero_position(0); - catapult_group.move_absolute(35, MAX_VOLTAGE); - pros::delay(1000); - catapult_block.move(-MAX_VOLTAGE); - pros::delay(500); - catapult_block.brake(); +void deploy_catapult() +{ + catapult_deploy_status = CatapultDeployStatus::RemoveBlock; + deploy_timer.Restart(); } void initCommon() @@ -72,14 +128,16 @@ void initCommon() catapult_group.set_gearing(pros::motor_gearset_e_t::E_MOTOR_GEAR_GREEN); catapult_group.set_encoder_units( pros::motor_encoder_units_e::E_MOTOR_ENCODER_DEGREES); + catapult_group.set_brake_modes(pros::E_MOTOR_BRAKE_HOLD); catapult_block.set_gearing(pros::motor_gearset_e_t::E_MOTOR_GEAR_GREEN); catapult_block.set_encoder_units( pros::motor_encoder_units_e::E_MOTOR_ENCODER_DEGREES); + catapult_block.set_brake_mode(pros::E_MOTOR_BRAKE_HOLD); intake_extension_group.move(-50); pros::delay(1000); - intake_extension_group.set_zero_position(0.0); + intake_extension_group.tare_position(); pros::delay(10); intake_extension_group.move(0); } @@ -93,6 +151,7 @@ void initCommon() void initialize() { initCommon(); + deploy_catapult(); } /** @@ -158,7 +217,8 @@ void autonomous() right_drive_group = 0; } -std::unique_ptr a_button_timer = std::make_unique(); +std::unique_ptr intake_extension_toggle_timer = + std::make_unique(); bool is_intake_extended = false; bool catapult_button_timer_running = false; @@ -180,51 +240,67 @@ std::unique_ptr catapult_button_timer = std::make_unique(); void opcontrol() { while (true) { + handle_catapult_deploy(); + int l_stick_y = ctrl.get_analog(ANALOG_LEFT_Y); int r_stick_y = ctrl.get_analog(ANALOG_RIGHT_Y); left_drive_group = l_stick_y; right_drive_group = r_stick_y; - int a_button = ctrl.get_digital(DIGITAL_R1); - if (a_button_timer->GetElapsedTime().AsMilliseconds() > 100) { + int right_trigger_upper = ctrl.get_digital(DIGITAL_R1); + if (right_trigger_upper && + intake_extension_toggle_timer->GetElapsedTime() + .AsMilliseconds() > 200) { is_intake_extended = !is_intake_extended; - a_button_timer->Restart(); + intake_extension_toggle_timer->Restart(); } if (is_intake_extended) { - intake_extension_group.move_absolute(170, MAX_VOLTAGE); + intake_extension_group.move_absolute(170, MAX_RPM); } else { - intake_extension_group.move_absolute(0, MAX_VOLTAGE); + intake_extension_group.move_absolute(60, MAX_RPM); } // bool right_trigger_upper = ctrl.get_digital(DIGITAL_R1); bool left_trigger_lower = ctrl.get_digital(DIGITAL_L2); bool left_trigger_upper = ctrl.get_digital(DIGITAL_L1); if (left_trigger_lower) { - intake_spin_group = MAX_VOLTAGE; + intake_spin_group.move(MAX_VOLTAGE); } else if (left_trigger_upper) { - intake_spin_group = -MAX_VOLTAGE; + intake_spin_group.move(-MAX_VOLTAGE); } else { intake_spin_group = 0; } // Catapult controls: - bool right_trigger_lower = ctrl.get_digital(DIGITAL_R2); - if (right_trigger_lower) { - catapult_group.move(-MAX_VOLTAGE); - } + if (catapult_deploy_status == + CatapultDeployStatus::NotDeploying) { + bool right_trigger_lower = ctrl.get_digital(DIGITAL_R2); + bool up_arrow_button = ctrl.get_digital(DIGITAL_UP); + if (up_arrow_button) { + catapult_block.move(MAX_VOLTAGE); + catapult_group.move(MAX_VOLTAGE); + } else if (right_trigger_lower) { + catapult_group.move(MAX_VOLTAGE); + catapult_block.brake(); + } else { + catapult_group.brake(); + catapult_block.brake(); + } - bool back_arrow_button = ctrl.get_digital(DIGITAL_DOWN); - if (back_arrow_button) { - if (catapult_button_timer_running == false) { - catapult_button_timer->Restart(); - catapult_button_timer_running = true; - } else if (catapult_button_timer->GetElapsedTime() - .AsMilliseconds() > 100.0) { - deploy_catapult(); + bool down_arrow_button = ctrl.get_digital(DIGITAL_DOWN); + if (down_arrow_button) { + if (catapult_button_timer_running == false) { + catapult_button_timer->Restart(); + catapult_button_timer_running = true; + } else if (catapult_button_timer + ->GetElapsedTime() + .AsMilliseconds() > 250.0) { + deploy_catapult(); + } + } else { + catapult_button_timer_running = false; } - } else { - catapult_button_timer_running = false; } pros::delay(5); diff --git a/src/ports.h b/src/ports.h index 9e83dd1..9df0463 100644 --- a/src/ports.h +++ b/src/ports.h @@ -3,8 +3,8 @@ #define LEFT_DRIVE_PORTS { 7, 8, -9, 10 } #define RIGHT_DRIVE_PORTS { -1, 2, -3, -4 } -#define INTAKE_EXTENSION_PORTS { -12, 19 } +#define INTAKE_EXTENSION_PORTS { 12, -19 } #define INTAKE_SPIN_PORTS { -11, 20 } #define CATAPULT_DRIVE_PORTS { -5 , 6 } -#define CATAPULT_STOPPER_PORT 13 +#define CATAPULT_STOPPER_PORT -13