Advertisement
Trainlover08

V4

Nov 9th, 2023 (edited)
143
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 8.76 KB | None | 0 0
  1. #pragma region VEXcode Generated Robot Configuration
  2. // Make sure all required headers are included.
  3. #include <stdio.h>
  4. #include <stdlib.h>
  5. #include <stdbool.h>
  6. #include <math.h>
  7. #include <string.h>
  8.  
  9.  
  10. #include "vex.h"
  11.  
  12. using namespace vex;
  13.  
  14. competition Competition;
  15.  
  16. //define global variables
  17. int axis3Drive = 0;
  18. int axis1Drive = 0;
  19. float coef = 1.28;
  20.  
  21. // Brain should be defined by default
  22. brain Brain;
  23.  
  24.  
  25. // START V5 MACROS
  26. #define waitUntil(condition)                                                   \
  27.   do {                                                                         \
  28.     wait(5, msec);                                                             \
  29.   } while (!(condition))
  30.  
  31. #define repeat(iterations)                                                     \
  32.   for (int iterator = 0; iterator < iterations; iterator++)
  33. // END V5 MACROS
  34.  
  35.  
  36. // Robot configuration code.
  37. controller Controller1 = controller(primary);
  38. distance Distance1 = distance(PORT1);
  39. motor leftMotorA = motor(PORT2, ratio6_1, false);
  40. motor leftMotorB = motor(PORT7, ratio6_1, false);
  41. motor_group LeftDriveSmart = motor_group(leftMotorA, leftMotorB);
  42. motor rightMotorA = motor(PORT4, ratio6_1, true);
  43. motor rightMotorB = motor(PORT8, ratio6_1, true);
  44. motor_group RightDriveSmart = motor_group(rightMotorA, rightMotorB);
  45. drivetrain Drivetrain = drivetrain(LeftDriveSmart, RightDriveSmart, 219.44, 295, 40, mm, 1);
  46.  
  47. motor cataMotorA = motor(PORT13, ratio36_1, true);
  48. motor cataMotorB = motor(PORT10, ratio36_1, false);
  49. motor_group cata = motor_group(cataMotorA, cataMotorB);
  50.  
  51. motor intake = motor(PORT12, ratio36_1, true);
  52.  
  53. digital_out DigitalOutB = digital_out(Brain.ThreeWirePort.B);
  54. digital_out DigitalOutH = digital_out(Brain.ThreeWirePort.H);
  55.  
  56. digital_in DigitalInD = digital_in(Brain.ThreeWirePort.D);
  57.  
  58. // define variable for remote controller enable/disable
  59. bool RemoteControlCodeEnabled = true;
  60. // define variables used for controlling motors based on controller inputs
  61. bool DrivetrainLNeedsToBeStopped_Controller1 = true;
  62. bool DrivetrainRNeedsToBeStopped_Controller1 = true;
  63.  
  64. int axis3calc() {
  65.     while(true){
  66.     //get controller info and prep for processing
  67.    
  68.     //initalize variables first
  69.     double y = 0;
  70.    
  71.     int x = fabs(Controller1.Axis3.position() * coef);
  72.    
  73.     switch(x){
  74.         case 0 ... 60:
  75.             y = pow(1.04029, x * 1.284467);
  76.             break;
  77.         case 61 ... 80:
  78.             y = 0.75 * x -25;
  79.             break;
  80.         case 81 ... 108:
  81.             y = 1.0357 * x - 47.85714857;
  82.             break;
  83.         case 109 ... 127:
  84.             y = 63 + pow(1.232099, x - 108);
  85.             break;
  86.         default:
  87.             y = 128;
  88.             break;
  89.     }
  90.     y = y / coef;
  91.    
  92.     if(Controller1.Axis3.position() < 0){
  93.         y = -y;
  94.     }
  95.    
  96.     axis3Drive = -y;
  97.     wait(2.5, msec);
  98.     }
  99.     return 0;
  100. }
  101. int axis1calc(){
  102.     while(true){
  103.     //get controller info and prep for processing
  104.    
  105.     //initalize variables first
  106.     double y = 0;
  107.    
  108.     int x = fabs(Controller1.Axis1.position() * coef);
  109.    
  110.     switch(x){
  111.         case 0 ... 60:
  112.             y = pow(1.04029, x * 1.284467);
  113.             break;
  114.         case 61 ... 80:
  115.             y = 0.75 * x -25;
  116.             break;
  117.         case 81 ... 108:
  118.             y = 1.0357 * x - 47.85714857;
  119.             break;
  120.         case 109 ... 127:
  121.             y = 63 + pow(1.232099, x - 108);
  122.             break;
  123.         default:
  124.             y = 128;
  125.             break;
  126.     }
  127.     y = y / coef;
  128.    
  129.     if(Controller1.Axis1.position() < 0){
  130.         y = -y;
  131.     }
  132.     axis1Drive = y;
  133.     wait(2.5, msec);
  134.     }
  135.     return 0;
  136. }
  137.  
  138. // define a task that will handle monitoring inputs from Controller1
  139. int rc_auto_loop_function_Controller1() {
  140.   // process the controller input every 20 milliseconds
  141.   // update the motors based on the input values
  142.   while(true) {
  143.  
  144.     vex::task ws4(axis1calc);
  145.  
  146.     vex::task ws3(axis3calc);
  147.  
  148.     if(RemoteControlCodeEnabled) {
  149.       int drivetrainLeftSideSpeed = axis3Drive - axis1Drive;
  150.       int drivetrainRightSideSpeed = axis3Drive + axis1Drive;
  151.  
  152.       // check if the value is inside of the deadband range
  153.       if (drivetrainLeftSideSpeed < 5 && drivetrainLeftSideSpeed > -5) {
  154.         // check if the left motor has already been stopped
  155.         if (DrivetrainLNeedsToBeStopped_Controller1) {
  156.           // stop the left drive motor
  157.           LeftDriveSmart.stop();
  158.           // tell the code that the left motor has been stopped
  159.           DrivetrainLNeedsToBeStopped_Controller1 = false;
  160.         }
  161.       } else {
  162.         // reset the toggle so that the deadband code knows to stop the left motor nexttime the input is in the deadband range
  163.         DrivetrainLNeedsToBeStopped_Controller1 = true;
  164.       }
  165.       // check if the value is inside of the deadband range
  166.       if (drivetrainRightSideSpeed < 5 && drivetrainRightSideSpeed > -5) {
  167.         // check if the right motor has already been stopped
  168.         if (DrivetrainRNeedsToBeStopped_Controller1) {
  169.           // stop the right drive motor
  170.           RightDriveSmart.stop();
  171.           // tell the code that the right motor has been stopped
  172.           DrivetrainRNeedsToBeStopped_Controller1 = false;
  173.         }
  174.       } else {
  175.         // reset the toggle so that the deadband code knows to stop the right motor next time the input is in the deadband range
  176.         DrivetrainRNeedsToBeStopped_Controller1 = true;
  177.       }
  178.  
  179.       // only tell the left drive motor to spin if the values are not in the deadband range
  180.       if (DrivetrainLNeedsToBeStopped_Controller1) {
  181.         LeftDriveSmart.setVelocity(drivetrainLeftSideSpeed, percent);
  182.         LeftDriveSmart.spin(forward);
  183.       }
  184.       // only tell the right drive motor to spin if the values are not in the deadband range
  185.       if (DrivetrainRNeedsToBeStopped_Controller1) {
  186.         RightDriveSmart.setVelocity(drivetrainRightSideSpeed, percent);
  187.         RightDriveSmart.spin(forward);
  188.       }
  189.     }
  190.     // wait before repeating the process
  191.     wait(5, msec);
  192.   }
  193.   return 0;
  194. }
  195.  
  196. task rc_auto_loop_task_Controller1(rc_auto_loop_function_Controller1);
  197.  
  198. #pragma endregion VEXcode Generated Robot Configuration
  199. bool intakeInterlock = false;
  200. bool cataInterlock = false;
  201. int switchpoint = 0;
  202.  
  203.  
  204. int Interlock(){
  205.     while(true){
  206.         if(DigitalInD.value() == 0){
  207.             intakeInterlock = true;
  208.         } else {
  209.             intakeInterlock = false;
  210.         }
  211.         if(DigitalOutB.value() == 1){
  212.             cataInterlock = false;
  213.         } else {
  214.             cataInterlock = true;
  215.         }
  216.     wait(5, msec);
  217.     }
  218.     return 0;
  219. }
  220.  
  221. int Intake(){
  222.     bool intakeToggle = false;
  223.       while(true){
  224.         intake.setMaxTorque(100, percent);
  225.         intake.spin(forward);
  226.         if(Controller1.ButtonR1.pressing()){
  227.             intakeToggle = !intakeToggle;
  228.             wait(250, msec);
  229.         }
  230.         if(Distance1.objectDistance(inches) < 4.0){
  231.         intake.setStopping(hold);
  232.           intake.stop();
  233.           intake.setVelocity(0, percent);
  234.         } else {
  235.             intake.setVelocity(60, percent);  
  236.         }
  237.     wait(5, msec);
  238.     }
  239.     return 0;
  240. }
  241.  
  242. int cataFire(){
  243.   cata.setMaxTorque(100, percent);
  244.   cata.setVelocity(100, percent);
  245.   cata.setStopping(hold);
  246.   float i = 0;
  247.   while(true){
  248.     i = cata.position(degrees) - (floor(cata.position(turns)) * 360);
  249.     if(cataInterlock == false && !Controller1.ButtonR1.pressing()){
  250.       if(Distance1.objectDistance(inches) < 4.0 || Controller1.ButtonA.pressing()){
  251.       intake.setVelocity(0, percent);
  252.       cata.spin(forward);
  253.       wait(100, msec);
  254.       while(DigitalInD.value() == 1){
  255.         cata.spin(forward);
  256.       }
  257.       cata.stop();
  258.       } else {
  259.       cata.stop();
  260.       intake.setVelocity(60, percent);
  261.     }
  262.    }
  263.     wait(1, msec);
  264.   }
  265.   return 0;
  266. }
  267.  
  268. int wings(){
  269.   while(true){
  270.     if(Controller1.ButtonL1.pressing()){
  271.       DigitalOutH.set(true);
  272.     }else{
  273.       DigitalOutH.set(false);
  274.     }
  275.     wait(5, msec);
  276.     }
  277.   return 0;
  278. }
  279.  
  280. void whenStarted(){
  281.     DigitalOutB.set(true);
  282.   cata.setVelocity(40, percent);
  283.     while(DigitalInD.value() == 1){
  284.       cata.spin(forward);
  285.     }
  286.     cata.setStopping(hold);
  287.     cata.stop();
  288.     //cata.setVelocity(30, percent);
  289.     //cata.setMaxTorque(100, percent);
  290.     //while(DigitalInD.value() == 1){
  291.         //cata.spin(forward);
  292.     //}
  293.     //cata.setVelocity(0, percent);
  294.     //cata.setStopping(hold);
  295.     //cata.stop();
  296. }
  297.  
  298. int main() {
  299.   // post event registration
  300.  
  301.   // set default print color to black
  302.   printf("\033[30m");
  303.  
  304.   // wait for rotation sensor to fully initialize
  305.   wait(30, msec);
  306.  
  307.   //run the important things
  308.   whenStarted();
  309.   vex::task ws5(Interlock);
  310.   vex::task ws6(wings);
  311.   vex::task ws1(Intake);
  312.   vex::task ws2(cataFire);
  313. }
  314.  
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement