Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include <iostream>
- #include "vex.h"
- using namespace vex;
- int Laxle_speed = 0;
- int Raxle_speed = 0;
- int Left_Bank_rpm = 0;
- int Right_Bank_rpm = 0;
- int Left_Bank_power = 0;
- int Right_Bank_power = 0;
- int gear = 1;
- bool PTOtoggle = true;
- vex::solenoid Solenoid[] = { vex::solenoid(A), vex::solenoid(B), vex::solenoid(C), vex::solenoid(D), vex::solenoid(E), vex::solenoid(F) };
- bool solenoidStates[] = { false, false, false, false, false, false };
- void Gear1() {
- solenoidStates[0] = true;
- solenoidStates[1] = true;
- gear = 1;
- }
- void Gear2() {
- solenoidStates[1] = true;
- solenoidStates[2] = true;
- gear = 2;
- }
- void Gear3() {
- solenoidStates[2] = true;
- solenoidStates[3] = true;
- gear = 3;
- }
- void Gear4() {
- solenoidStates[3] = true;
- solenoidStates[4] = true;
- gear = 4;
- }
- void Aux1() {
- solenoidStates[4] = true;
- solenoidStates[5] = true;
- gear = -1;
- }
- void Aux2() {
- solenoidStates[5] = true;
- gear = -2;
- }
- void Aux3() {
- for (int i = 0; i < 6; i++) {
- solenoidStates[i] = false;
- }
- gear = -3;
- }
- void UpShift() {
- if (PTOtoggle == false) {
- if (gear <= 1) {
- Gear2();
- } else if (gear == 2) {
- Gear3();
- } else if (gear == 3) {
- Gear4();
- }
- } else {
- if (gear == -3) {
- Aux2();
- } else if (gear == -2) {
- Aux1();
- }
- }
- this_thread::sleep_for(500);
- }
- void DownShift() {
- if (PTOtoggle == false) {
- if (gear == 2) {
- Gear1();
- } else if (gear == 3) {
- Gear2();
- } else if (gear == 4) {
- Gear3();
- }
- } else {
- if (gear == -2) {
- Aux3();
- } else if (gear => -1) {
- Aux2();
- }
- }
- this_thread::sleep_for(500);
- }
- int main() {
- // Initializing the VEX Brain
- vex::brain Brain;
- // Set the motor brake mode to hold
- vex::motor LeftMotor = vex::motor(vex::PORT1, vex::gearSetting::ratio18_1, true);
- vex::motor RightMotor = vex::motor(vex::PORT2, vex::gearSetting::ratio18_1);
- // Create a drive base with two motors
- motor_group driveBase = motor_group(LeftMotor, RightMotor);
- // Configure the controller
- controller Controller = controller(primary);
- vex::Gif gif("world.gif", 120, 0);
- UpShift(2);
- PTOtoggle = false;
- UpShift();
- while (true) {
- if (Brain.Controller1.ButtonR1.pressing()) {
- UpShift();
- }
- if (Brain.Controller1.ButtonR2.pressing()) {
- DownShift();
- }
- if (Brain.Controller1.ButtonB.pressing()) {
- PTOtoggle = !PTOtoggle;
- }
- // Get joystick positions
- int RightStickY = Controller.Axis2.position(percentUnits::pct);
- int RightStickX = Controller.Axis1.position(percentUnits::pct);
- // Calculate motor velocities based on joystick inputs
- double y = 0;
- double x = RightStickX;
- if (x < 0) {
- x = -x;
- }
- if (0 <= x && x < 60) {
- y = pow(1.04029, x * 1.284467);
- } else if (60 <= x && x < 80) {
- y = 0.75 * x - 25;
- } else if (80 <= x && x <= 108) {
- y = 1.0357 * x - 47.85714857;
- } else if (x > 108) {
- y = 63 + pow(1.232099, x - 108);
- } else if (x >= 128) {
- y = 128;
- }
- if (RightStickX < 0) {
- y = -y;
- }
- double steerX = RightStickX;
- double rightWheels = y - steerX;
- double leftWheels = y + steerX;
- // Set the motor velocities
- driveBase.setVelocity(rightWheels, percentUnits::pct);
- driveBase.setVelocity(leftWheels, percentUnits::pct);
- // Run the motors
- driveBase.spin(forward);
- // Print GIF frame index on the Brain screen
- Brain.Screen.printAt(5, 230, "frame %3d", gif.getFrameIndex());
- // Delay for smooth GIF playback
- this_thread::sleep_for(10);
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement