Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include <stdio.h>
- #include <stdlib.h>
- #include <string.h>
- #include <math.h>
- #include "C:/Program Files (x86)/VEX Robotics/VEXcode/sdk/vexv5/include/v5_apitypes.h"
- #include "C:/Program Files (x86)/VEX Robotics/VEXcode/sdk/vexv5/include/v5_apiuser.h"
- #include "C:/Program Files (x86)/VEX Robotics/VEXcode/sdk/vexv5/include/vex_units.h"
- #include "v5.h"
- #include "v5_vcs.h"
- #include <cmath>
- #include "vex.h"
- using namespace vex;
- vex::brain Brain;
- vex::motor FL_mtr(vex::PORT10, vex::gearSetting::ratio18_1, true);
- vex::motor BL_mtr(vex::PORT3, vex::gearSetting::ratio18_1, true);
- vex::motor FR_mtr(vex::PORT2, vex::gearSetting::ratio18_1, false);
- vex::motor BR_mtr(vex::PORT4, vex::gearSetting::ratio18_1, false);
- vex::motor Tilt(vex::PORT5, vex::gearSetting::ratio18_1, false);
- vex::motor Lift(vex::PORT6, vex::gearSetting::ratio18_1, false);
- vex::motor LeftIntake(vex::PORT8, vex::gearSetting::ratio18_1, false);
- vex::motor RightIntake(vex::PORT7, vex::gearSetting::ratio18_1, false);
- vex::line Line(Brain.ThreeWirePort.C);
- vex::controller (Controller1);
- vex::competition Competition;
- void wait_until_settled(){
- int average_velocity = 1;
- while (average_velocity >= 1){
- average_velocity = (FL_mtr.velocity(vex::velocityUnits::rpm) + FR_mtr.velocity(vex::velocityUnits::rpm) + BL_mtr.velocity(vex::velocityUnits::rpm) + BR_mtr.velocity(vex::velocityUnits::rpm)) / 4;
- vex::task::sleep(10);
- }
- }
- void drive_inches(int distance, int max_power = 90, int min_power = 35, bool wait_for_rest = true){
- float error;
- float previous_error;
- float drive_power;
- float previous_drive_power;
- float distance_covered;
- float max_voltage = 120 * max_power;
- float min_voltage = 120 * min_power;
- const int Kp = 200;
- const int Kd = -100;
- BL_mtr.resetRotation();
- BR_mtr.resetRotation();
- if (distance <= 2){
- distance = 3;
- }
- while(error > 2){
- error = distance - (fabs((BL_mtr.rotation(vex::rotationUnits::deg)) - (BR_mtr.rotation(vex::rotationUnits::deg)) / 2) / 28.86);
- drive_power = (error * Kp) + ((previous_error - error) * Kd);
- if (distance_covered < 0.25 * distance){
- drive_power = (drive_power - previous_drive_power) * 0.75;
- }
- if (drive_power < min_voltage && error > 2){
- drive_power = min_voltage;
- } else if (drive_power > min_voltage){
- drive_power = max_voltage;
- } else if (drive_power < min_voltage && error <= 2){
- drive_power = min_voltage * 0.75;
- }
- FL_mtr.spin(vex::directionType::undefined, drive_power, vex::voltageUnits::mV);
- BR_mtr.spin(vex::directionType::undefined, drive_power, vex::voltageUnits::mV);
- BL_mtr.spin(vex::directionType::undefined, drive_power, vex::voltageUnits::mV);
- FR_mtr.spin(vex::directionType::undefined, drive_power, vex::voltageUnits::mV);
- previous_error = error;
- previous_drive_power = drive_power;
- }
- if (wait_for_rest == true){
- wait_until_settled();
- } else {
- FL_mtr.setStopping(vex::brakeType::brake);
- FR_mtr.setStopping(vex::brakeType::brake);
- BL_mtr.setStopping(vex::brakeType::brake);
- BR_mtr.setStopping(vex::brakeType::brake);
- vex::task::sleep(100);
- FL_mtr.setStopping(vex::brakeType::coast);
- FR_mtr.setStopping(vex::brakeType::coast);
- BL_mtr.setStopping(vex::brakeType::coast);
- BR_mtr.setStopping(vex::brakeType::coast);
- }
- }
- void drive_inches_reverse (int distance, int max_power = 90, int min_power = 35, bool wait_for_rest = true){
- float error;
- float previous_error;
- float drive_power;
- float previous_drive_power;
- float distance_covered;
- float max_voltage = 120 * max_power;
- float min_voltage = 120 * min_power;
- const int Kp = 200;
- const int Kd = -100;
- BL_mtr.resetRotation();
- BR_mtr.resetRotation();
- if (distance <= 2){
- distance = 3;
- }
- while(error > 1){
- error = distance - (fabs((BL_mtr.rotation(vex::rotationUnits::deg)) - (BR_mtr.rotation(vex::rotationUnits::deg)) / 2) / 28.86);
- drive_power = (error * Kp) + ((previous_error - error) * Kd);
- if (distance_covered < 0.25 * distance){
- drive_power = (drive_power - previous_drive_power) * 0.75;
- }
- if (drive_power < min_voltage && error > 2){
- drive_power = min_voltage;
- } else if (drive_power > min_voltage){
- drive_power = max_voltage;
- } else if (drive_power < min_voltage && error <= 2){
- drive_power = min_voltage * 0.75;
- }
- FL_mtr.spin(vex::directionType::undefined, -(drive_power), vex::voltageUnits::mV);
- BR_mtr.spin(vex::directionType::undefined, -(drive_power), vex::voltageUnits::mV);
- BL_mtr.spin(vex::directionType::undefined, -(drive_power), vex::voltageUnits::mV);
- FR_mtr.spin(vex::directionType::undefined, -(drive_power), vex::voltageUnits::mV);
- previous_error = error;
- previous_drive_power = drive_power;
- }
- if (wait_for_rest == true){
- wait_until_settled();
- } else {
- FL_mtr.setStopping(vex::brakeType::brake);
- FR_mtr.setStopping(vex::brakeType::brake);
- BL_mtr.setStopping(vex::brakeType::brake);
- BR_mtr.setStopping(vex::brakeType::brake);
- vex::task::sleep(100);
- FL_mtr.setStopping(vex::brakeType::coast);
- FR_mtr.setStopping(vex::brakeType::coast);
- BL_mtr.setStopping(vex::brakeType::coast);
- BR_mtr.setStopping(vex::brakeType::coast);
- }
- }
- void pre_auton( void ) {
- Tilt.resetRotation();
- Lift.resetRotation();
- FL_mtr.resetRotation();
- FR_mtr.resetRotation();
- BL_mtr.resetRotation();
- BR_mtr.resetRotation();
- }
- void autonomous( void ) {
- drive_inches(24);
- drive_inches_reverse(24);
- }
- void resetTilt(int tiltSpeed){
- Tilt.rotateTo(0, vex::rotationUnits::deg, tiltSpeed, vex::velocityUnits::pct);
- Tilt.stop(vex::brakeType::hold);
- }
- void lowTower(){
- RightIntake.startRotateFor(30, vex::rotationUnits::deg, 70, vex::velocityUnits::pct);
- LeftIntake.rotateFor(-30, vex::rotationUnits::deg, 70, vex::velocityUnits::pct);
- vex::task::sleep(10);
- Lift.rotateTo(-115, vex::rotationUnits::deg, 90, vex::velocityUnits::pct);// Moves Lift up to Score in Tower
- RightIntake.startRotateFor(150, vex::rotationUnits::deg, 25, vex::velocityUnits::pct);
- LeftIntake.startRotateFor(-150, vex::rotationUnits::deg, 25, vex::velocityUnits::pct);
- Lift.rotateTo(-625, vex::rotationUnits::deg, 90, vex::velocityUnits::pct);// Moves Lift up to Score in Tower
- }
- void highTower(){
- RightIntake.startRotateFor(30, vex::rotationUnits::deg, 70, vex::velocityUnits::pct);
- LeftIntake.rotateFor(-30, vex::rotationUnits::deg, 70, vex::velocityUnits::pct);
- vex::task::sleep(10);
- Lift.rotateTo(-115, vex::rotationUnits::deg, 90, vex::velocityUnits::pct);// Moves Lift up to Score in Tower
- RightIntake.startRotateFor(150, vex::rotationUnits::deg, 25, vex::velocityUnits::pct);
- LeftIntake.startRotateFor(-150, vex::rotationUnits::deg, 25, vex::velocityUnits::pct);
- Lift.rotateTo(-915, vex::rotationUnits::deg, 90, vex::velocityUnits::pct);// Moves Lift up to Score in Tower
- }
- int tilterTasks (){
- Tilt.resetRotation();
- while(1){
- int TilterSpeed(-1175-(Tilt.rotation(vex::rotationUnits::deg)));
- if(Controller1.ButtonL1.pressing()){
- Lift.spin(vex::directionType::rev, 90, vex::percentUnits::pct);
- }
- else if (Controller1.ButtonL2.pressing()) {
- Lift.spin(vex::directionType::fwd, 90, vex::percentUnits::pct);
- }
- else if(Controller1.ButtonA.pressing()){
- Lift.stop(vex::brakeType::coast);
- Tilt.spin(vex::directionType::rev, ((TilterSpeed)/-8), vex::velocityUnits::pct);
- }
- else if(Controller1.ButtonB.pressing()){
- resetTilt(90);
- }
- else if(Controller1.ButtonX.pressing()){
- lowTower();
- }
- else if(Controller1.ButtonY.pressing()){
- highTower();
- }
- else{
- Tilt.stop(vex::brakeType::hold);
- Lift.stop(vex::brakeType::hold);
- }
- if(Controller1.ButtonR2.pressing()){
- RightIntake.spin(vex::directionType::fwd, 55, vex::velocityUnits::pct);
- LeftIntake.spin(vex::directionType::rev, 55, vex::velocityUnits::pct);
- }
- else if(Controller1.ButtonR1.pressing()){
- RightIntake.spin(vex::directionType::rev, 100, vex::velocityUnits::pct);
- LeftIntake.spin(vex::directionType::fwd, 100, vex::velocityUnits::pct);
- }
- else{
- RightIntake.stop(vex::brakeType::hold);
- LeftIntake.stop(vex::brakeType::hold);
- }
- }
- return 1;
- }
- void usercontrol( void ) {
- vex::task tilter = vex::task(tilterTasks);
- while (1) {
- FR_mtr.spin(directionType::fwd, (Controller1.Axis3.value() + (Controller1.Axis1.value())*.75)/1.25, velocityUnits::pct); //(Axis3+Axis4)/2;
- FR_mtr.spin(directionType::fwd, (Controller1.Axis3.value() - (Controller1.Axis1.value())*.75)/1.25, velocityUnits::pct);//(Axis3-Axis4)/2;
- BL_mtr.spin(directionType::fwd, (Controller1.Axis3.value() + (Controller1.Axis1.value())*.75)/1.25, velocityUnits::pct); //(Axis3+Axis4)/2;
- BR_mtr.spin(directionType::fwd, (Controller1.Axis3.value() - (Controller1.Axis1.value())*.75)/1.25, velocityUnits::pct);//(Axis3-Axis4)/2;
- }
- vex::task::sleep(10);
- }
- int main() {
- //Set up callbacks for autonomous and driver control periods.
- Competition.autonomous( autonomous );
- Competition.drivercontrol( usercontrol );
- //Run the pre-autonomous function.
- pre_auton();
- //Prevent main from exiting with an infinite loop.
- while(1) {
- vex::task::sleep(100);//Sleep the task for a short amount of time to prevent wasted resources.
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement