SHARE
TWEET

Untitled

a guest Oct 17th, 2019 106 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include "main.h"
  2. #include <stdio.h>
  3. #include <string>
  4. #include "okapi/api.hpp"
  5.  
  6. using namespace okapi;
  7.  
  8. #define LEFT_WHEELS_PORT 2
  9. #define LEFT_WHEELS_PORT_2 6
  10. #define RIGHT_WHEELS_PORT 5
  11. #define RIGHT_WHEELS_PORT_2 3
  12. #define INTAKE_PORT_L 8
  13. #define INTAKE_PORT_R 7
  14. #define OUTTAKE_PORT 20
  15. #define LIFT_PORT 9
  16. #define VISION_PORT 17
  17. //testing something
  18. bool profiling = false;
  19.  
  20. //ports/encoders
  21. pros::Motor left_wheels (LEFT_WHEELS_PORT);
  22. pros::Motor left_wheels_2 (LEFT_WHEELS_PORT_2,true);
  23. pros::Motor right_wheels (RIGHT_WHEELS_PORT, true);
  24. pros::Motor right_wheels_2 (RIGHT_WHEELS_PORT_2);
  25. pros::Motor intake_L (INTAKE_PORT_L);
  26. pros::Motor intake_R (INTAKE_PORT_R);
  27. pros::Motor lift (LIFT_PORT,MOTOR_GEARSET_36, false, pros::E_MOTOR_ENCODER_COUNTS);
  28. pros::Motor outtake (OUTTAKE_PORT,MOTOR_GEARSET_36, false, pros::E_MOTOR_ENCODER_COUNTS);
  29. IntegratedEncoder enc = IntegratedEncoder(outtake);
  30.  
  31. //budget motion profiling???
  32. std::vector<int> right_motor_movement_log={};
  33. std::vector<int> left_motor_movement_log={};
  34. std::vector<int> outtake_motor_movement_log={};
  35. std::vector<int> intake_motor_movement_log={};
  36.  
  37. //various numbers
  38. float SPEED_COEFFICIENT=126/127;
  39. float SPEED_FAST=126/127;
  40. float SPEED_SLOW=0.4;
  41. int INTAKE_SPEED=126;
  42. int OUTTAKE_ENCODER_TICKS=12000;//???
  43. int OUTTAKE_SPEED=126;//????
  44. int LIFT_SPEED=126;
  45.  
  46. //color codes?
  47.  
  48. pros::Vision vis (VISION_PORT);
  49.  
  50.  
  51. pros::vision_signature_s_t THANOSCUBE = pros::Vision::signature_from_utility(1, 1379, 2243, 1811, 8235, 9945, 9090, 3.000, 1);
  52. pros::vision_signature_s_t ORANGCUBE = pros::Vision::signature_from_utility(2, 6253, 6967, 6610, -2621, -2185, -2403, 3.000, 0);
  53. pros::vision_signature_s_t GREENCUBES = pros::Vision::signature_from_utility(3, -8371, -7575, -7973, -4897, -3719, -4308, 3.000, 0);
  54. pros::vision_signature_s_t PURPLE = pros::Vision::signature_from_utility( 4, 895, 1927, 1411, 8041, 10169, 9105, 3.000, 1);
  55. //motion profiling variables??
  56. auto myChassis = ChassisControllerFactory::create(
  57.   {1, -2}, // Left motors
  58.   {-10, 9},   // Right motors
  59.   AbstractMotor::gearset::red, // Torque gearset
  60.   {4_in, 12.5_in} // 4 inch wheels, 12.5 inch wheelbase width
  61. );
  62.  
  63. auto profileController = AsyncControllerFactory::motionProfile(
  64.   2.0,  // Maximum linear velocity of the Chassis in m/s
  65.   2.0,  // Maximum linear acceleration of the Chassis in m/s/s
  66.   10.0, // Maximum linear jerk of the Chassis in m/s/s/s
  67.   myChassis // Chassis Controller
  68. );
  69.  
  70. int autoAlignCube(){
  71.   pros::vision_object_s_t purp = vis.get_by_sig(0, 1);
  72.   pros::vision_object_s_t green = vis.get_by_sig(0, 2);
  73.   pros::vision_object_s_t orang = vis.get_by_sig(0, 3);
  74.   int size1= purp.width*purp.height;
  75.   int size2= green.width*green.height;
  76.   int size3= orang.width*orang.height;
  77.   float left=-0;
  78.   float right=0;
  79.   if(size1>=size2&&size1>=size3){
  80.     int x = purp.left_coord;
  81.     double y = -1.2307*x+128.462;
  82.     if(abs(purp.top_coord-y)<10){
  83.       printf("true");
  84.       return 0;
  85.     }
  86.     else if(purp.top_coord-y<=10){
  87.       return 30;
  88.     }
  89.     else{
  90.       return -30;
  91.     }
  92.   }
  93.   else if(size2>size3){
  94.  
  95.       int x = green.left_coord;
  96.       double y = -1.2307*x+128.462;
  97.       printf("%d %d %d\n",purp.x_middle_coord,purp.y_middle_coord,y);
  98.       if(abs(green.top_coord-y)<10){
  99.         printf("true");
  100.         return 0;
  101.       }
  102.       else if(green.top_coord-y<=10){
  103.         return 30;
  104.       }
  105.       else{
  106.         return -30;
  107.       }
  108.     }
  109.   else{
  110.  
  111.       int x = orang.left_coord;
  112.       double y = -1.2307*x+128.462;
  113.       printf("%d %d %d\n",purp.x_middle_coord,purp.y_middle_coord,y);
  114.       if(abs(orang.top_coord-y)<10){
  115.         printf("true");
  116.         return 0;
  117.       }
  118.       else if(orang.top_coord-y<=10){
  119.         return 30;
  120.       }
  121.       else{
  122.         return -30;
  123.       }
  124.   }
  125. }
  126.  
  127. void moveIntake(bool dir){//false for reverse
  128.   if(dir){
  129.     intake_L.move(127);
  130.     intake_R.move(-127);
  131.     intake_motor_movement_log.push_back(INTAKE_SPEED);
  132.   }
  133.   else{
  134.     intake_L.move(-127);
  135.     intake_R.move(127);
  136.     intake_motor_movement_log.push_back(-INTAKE_SPEED);
  137.   }
  138. }
  139. void stopIntake(){
  140.   intake_L.move(0);
  141.   intake_R.move(0);
  142.   intake_motor_movement_log.push_back(0);
  143. }
  144. void moveOuttake(bool dir){
  145.  if(dir){
  146.   outtake.move(OUTTAKE_SPEED);
  147.   outtake_motor_movement_log.push_back(OUTTAKE_SPEED);
  148.  }
  149.  else{
  150.   outtake.move(-OUTTAKE_SPEED);
  151.   outtake_motor_movement_log.push_back(-OUTTAKE_SPEED);
  152.  }
  153. }
  154. void stopOuttake(){
  155.   outtake.move(0);
  156.   outtake_motor_movement_log.push_back(0);
  157. }
  158. void outtake_macro(bool state,long encStart){//false for reverse
  159.   printf("%d\n",enc.get());
  160.   if(state==1){
  161.     if(enc.get()-encStart<OUTTAKE_ENCODER_TICKS){
  162.       outtake.move(OUTTAKE_SPEED);
  163.       outtake_motor_movement_log.push_back(OUTTAKE_SPEED);
  164.     }
  165.     else{
  166.       stopOuttake();
  167.     }
  168.   }
  169.   if(state==0){
  170.     if(enc.get()>0){
  171.       outtake.move(-OUTTAKE_SPEED);
  172.       outtake_motor_movement_log.push_back(-OUTTAKE_SPEED);
  173.     }
  174.     else{
  175.       stopOuttake();
  176.     }
  177.   }
  178. }
  179. void outtake_macro(bool dir){//false for reverse
  180.   IntegratedEncoder enc = IntegratedEncoder(outtake);
  181.   enc.reset();
  182.   if(dir){
  183.     while(enc.get()<=OUTTAKE_ENCODER_TICKS){
  184.       outtake.move(OUTTAKE_SPEED);
  185.       outtake_motor_movement_log.push_back(OUTTAKE_SPEED);
  186.     }
  187.   }
  188.   else{
  189.     while(enc.get()>=-OUTTAKE_ENCODER_TICKS){
  190.       outtake.move(-OUTTAKE_SPEED);
  191.       outtake_motor_movement_log.push_back(-OUTTAKE_SPEED);
  192.     }
  193.   }
  194.   outtake.move(0);
  195.   outtake_motor_movement_log.push_back(-OUTTAKE_SPEED);
  196. }
  197. void move_lift(bool dir){
  198.   if(dir){
  199.     lift.move(LIFT_SPEED);
  200.   }
  201.   else{
  202.     lift.move(-LIFT_SPEED);
  203.   }
  204. }
  205. void stop_lift(){
  206.   lift.move(0);
  207. }
  208. bool lift_macro_up(IntegratedEncoder encL){
  209.   if(encL.get()<500){
  210.     lift.move(LIFT_SPEED);
  211.     return true;
  212.   }
  213.   else {
  214.     return false;
  215.   }
  216. }
  217. bool lift_macro_down(IntegratedEncoder encL){
  218.   if(encL.get()>-500){
  219.     lift.move(-LIFT_SPEED);
  220.     return true;
  221.   }
  222.   else {
  223.     return false;
  224.   }
  225. }
  226. void outtake_macro2(bool dir){//false for reverse
  227.   IntegratedEncoder enc = IntegratedEncoder(outtake);
  228.   enc.reset();
  229.   if(dir){
  230.     while(enc.get()<=4000){
  231.       outtake.move(OUTTAKE_SPEED);
  232.       outtake_motor_movement_log.push_back(OUTTAKE_SPEED);
  233.     }
  234.   }
  235.   else{
  236.     while(enc.get()>=-4000){
  237.       outtake.move(-OUTTAKE_SPEED);
  238.       outtake_motor_movement_log.push_back(-OUTTAKE_SPEED);
  239.     }
  240.   }
  241.   outtake.move(0);
  242.   outtake_motor_movement_log.push_back(-OUTTAKE_SPEED);
  243. }
  244. void opcontrol() {
  245.   vis.set_signature(1, &THANOSCUBE);
  246.   vis.set_signature(2, &GREENCUBES);
  247.   vis.set_signature(3, &ORANGCUBE);
  248.   int tick = 0;
  249.   enc.reset();
  250.   pros::Controller master (CONTROLLER_MASTER);
  251.  
  252.   outtake.set_brake_mode(pros::E_MOTOR_BRAKE_HOLD);
  253.   intake_R.set_brake_mode(pros::E_MOTOR_BRAKE_HOLD);
  254.   intake_L.set_brake_mode(pros::E_MOTOR_BRAKE_HOLD);
  255.   left_wheels.set_brake_mode(pros::E_MOTOR_BRAKE_HOLD);
  256.   right_wheels.set_brake_mode(pros::E_MOTOR_BRAKE_HOLD);
  257.   left_wheels_2.set_brake_mode(pros::E_MOTOR_BRAKE_HOLD);
  258.   right_wheels_2.set_brake_mode(pros::E_MOTOR_BRAKE_HOLD);
  259.   IntegratedEncoder drive = IntegratedEncoder(left_wheels);
  260.   while (true) {
  261.      //DRIVE (TANK)
  262.      /*float left=(master.get_analog(ANALOG_LEFT_Y));
  263.      float right=(master.get_analog(ANALOG_RIGHT_Y));
  264.      if(master.get_digital(DIGITAL_L2)){
  265.        int adjust = autoAlignCube();
  266.        if(adjust==-30){
  267.          right-=30;
  268.        }
  269.        if(adjust==30){
  270.          left-=30;
  271.        }
  272.      }
  273.      left_wheels.move(-left);
  274.      right_wheels.move(right);
  275.      left_wheels_2.move(left);
  276.      right_wheels_2.move(right);
  277.      left_motor_movement_log.push_back(left);
  278.      right_motor_movement_log.push_back(right);
  279.      //printf("%d %d",master.get_analog(ANALOG_LEFT_Y),master.get_analog(ANALOG_RIGHT_Y));
  280.      *///DRIVE (ARCADE)
  281.      int power = master.get_analog(ANALOG_LEFT_Y);
  282.      int turn = master.get_analog(ANALOG_RIGHT_X);
  283.      int left = (power + turn);
  284.      int right = (power - turn);
  285.      printf("%f\n",drive.get());
  286.      left_wheels.move(-left);
  287.      right_wheels.move(right);
  288.      left_wheels_2.move(left);
  289.      right_wheels_2.move(right);
  290.      left_motor_movement_log.push_back(left);
  291.      right_motor_movement_log.push_back(right);
  292.      //SLOW MODE CONTROL`
  293.      /*bool moveLiftUp = false;
  294.      bool moveLiftDown = false;
  295.      IntegratedEncoder encL = IntegratedEncoder(lift);
  296.      if (master.get_digital(DIGITAL_L1)) {
  297.        encL.reset();
  298.        moveLiftUp=true;
  299.      }
  300.      else if (master.get_digital(DIGITAL_L2))  {
  301.        encL.reset();
  302.        moveLiftDown=true;
  303.      }
  304.      if(moveLiftUp){
  305.        moveLiftUp=lift_macro_up(encL);
  306.      }
  307.      else if(moveLiftDown){
  308.        moveLiftDown=lift_macro_down(encL);
  309.      }*/
  310.     if (master.get_digital(DIGITAL_L1)) {
  311.       move_lift(true);
  312.     }
  313.     else if(master.get_digital(DIGITAL_L2)) {
  314.       move_lift(false);
  315.     }
  316.     else{
  317.       stop_lift();
  318.     }
  319.     //INTAKE CONTROL
  320.     if (master.get_digital(DIGITAL_R1)) {
  321.       printf("%s","yay");
  322.       moveIntake(true);
  323.     }
  324.     else if (master.get_digital(DIGITAL_R2)) {
  325.       moveIntake(false);
  326.     }
  327.     else {
  328.       stopIntake();
  329.     }
  330.     //OUTTAKE SYSTEM
  331.     if(master.get_digital(DIGITAL_X)){
  332.       OUTTAKE_ENCODER_TICKS=12000;
  333.       outtake_macro(true);
  334.       //moveOuttake(true);//controlled outtake
  335.     }
  336.     else if(master.get_digital(DIGITAL_UP)){
  337.       OUTTAKE_ENCODER_TICKS=12000;
  338.       outtake_macro(false);
  339.       //moveOuttake(false);
  340.     }
  341.     else if(master.get_digital(DIGITAL_B)){
  342.     OUTTAKE_ENCODER_TICKS=3600;
  343.       outtake_macro(true);
  344.       //moveOuttake(true);//controlled outtake
  345.     }
  346.     else if(master.get_digital(DIGITAL_DOWN)){
  347.     OUTTAKE_ENCODER_TICKS=3600;
  348.       outtake_macro(false);
  349.       //moveOuttake(false);
  350.     }
  351.     else{
  352.      stopOuttake();
  353.     }
  354.     pros::delay(10);
  355.   }
  356. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!
 
Top