API tools faq
paste
Advertisement
Trainlover08

src/main.cpp

Trainlover08
Oct 29th, 2024
42
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 28.31 KB | None | 0 0
raw download clone embed print report
  1. // (src/main.cpp)
  2.  
  3.  
  4.  
  5. // includes the other code that get called in "main.cpp"
  6. #include "common.cpp"
  7.  
  8. // makes a char enum value for all of our pneumatic ports
  9. // naming convention 'solinoid' used for the enums, for a more long term project I would make it a class, but this is subject to change at the drop of a hat
  10. #define intake_solenoid 'E'
  11. #define wrist_solenoid 'B'
  12. #define arm_solenoid 'C'
  13. #define mogo_solenoid 'D'
  14. #define chopper_solenoid 'F'
  15.  
  16. // just as before, makes an enum value for the intake ports; the sign of the enum signifies direction the motor spins
  17. #define intake_stage_1 -10
  18. #define intake_stage_2 -9
  19.  
  20. const std::string PROGRAM = "Blue";
  21. const bool E_VARIENTCE = false;
  22. const double E_ENTROPY = 0.5;
  23. const bool USE_AI = true;
  24.  
  25. // creates a controller object for the two controllers we use during matches
  26. pros::Controller controller_1(pros::E_CONTROLLER_MASTER);
  27. pros::Controller controller_2(pros::E_CONTROLLER_PARTNER);
  28.  
  29. // a constructor for the pros::Motor objects used
  30. pros::Motor lm1(-15);
  31. pros::Motor lm2(14);
  32. pros::Motor lm3(-13);
  33. pros::Motor rm1(16);
  34. pros::Motor rm2(-12);
  35. pros::Motor rm3(11);
  36. pros::Motor intake_lower(intake_stage_1);
  37. pros::Motor intake_upper(intake_stage_2);
  38.  
  39. // constructs an easily manipulatable std::vector<pros::Motor> for use in the program
  40. pros::MotorGroup left_motors({lm1.get_port(), lm2.get_port(), lm3.get_port()}, pros::MotorGears::blue);
  41.  
  42. pros::MotorGroup right_motors({rm1.get_port(), rm2.get_port(), rm3.get_port()}, pros::MotorGears::blue);
  43.  
  44. pros::MotorGroup intake({intake_lower.get_port(), intake_upper.get_port()}, pros::MotorGears::green);
  45.  
  46. // constructs the drivetrain from the builder classes used in lemlib
  47. lemlib::Drivetrain drivetrain(&left_motors, &right_motors, 11.625, 3.25, 480, 8); //left motor group, right motor group, track width, wheel diameter, drive rpm, horizontal drift
  48.  
  49. // constructs the imu used in heading tracking for odometery
  50. pros::IMU imu(5);
  51.  
  52. // more lemlib builder class objects for odom
  53. lemlib::TrackingWheel leftVerticalWheel(&left_motors, lemlib::Omniwheel::NEW_325, -5.825, 480);
  54. lemlib::TrackingWheel rightVerticalWheel(&right_motors, lemlib::Omniwheel::NEW_325, 5.825, 480);
  55.  
  56. lemlib::OdomSensors sensors(nullptr, nullptr, nullptr, nullptr, &imu); //vertical tracking wheels 1 & 2, horizontal tracking wheels 3 & 4, imu
  57.  
  58. // pneumatics objects for the solinoids using the enums defined earlier, the second variable (bool) is used to denote startng position
  59. pros::adi::Pneumatics mogo_mech(mogo_solenoid, true);
  60. pros::adi::Pneumatics arm_mech(arm_solenoid, false);
  61. pros::adi::Pneumatics intake_mech(intake_solenoid, false);
  62. pros::adi::Pneumatics wrist_mech(wrist_solenoid, false);
  63. pros::adi::Pneumatics chopper_mech(chopper_solenoid, false);
  64.  
  65. // lateral PID controller
  66. lemlib::ControllerSettings lateral_controller(8, // proportional gain (kP)
  67. 0, // integral gain (kI)
  68. 18, // derivative gain (kD)
  69. 0, // anti windup
  70. 0, // small error range, in inches
  71. 0, // small error range timeout, in milliseconds
  72. 0, // large error range, in inches
  73. 0, // large error range timeout, in milliseconds
  74. 0 // maximum acceleration (slew)
  75. );
  76.  
  77. // angular PID controller
  78. lemlib::ControllerSettings angular_controller(2, // proportional gain (kP)
  79. 0, // integral gain (kI)
  80. 7, // derivative gain (kD)
  81. 0, // anti windup
  82. 0, // small error range, in inches
  83. 0, // small error range timeout, in milliseconds
  84. 0, // large error range, in inches
  85. 0, // large error range timeout, in milliseconds
  86. 0 // maximum acceleration (slew)
  87. );
  88.  
  89. // input curve for throttle input during driver control
  90. lemlib::ExpoDriveCurve throttle_curve(2, // joystick deadband out of 127
  91. 0, // minimum output where drivetrain will move out of 127
  92. 1.023 // expo curve gain
  93. );
  94.  
  95. // same thing for steering curves
  96. lemlib::ExpoDriveCurve steer_curve(2,
  97. 0,
  98. 1.020
  99. );
  100.  
  101. // create the chassis object
  102. lemlib::Chassis chassis(drivetrain, // drivetrain settings
  103. lateral_controller, // lateral PID settings
  104. angular_controller, // angular PID settings
  105. sensors, // odometry sensors
  106. &throttle_curve,
  107. &steer_curve
  108. );
  109.  
  110. // a function called later in the program that determines which controller is the one that allows input used in controlling the robot
  111. std::vector<cmn::my_controller> get_active_controller(cmn::my_controller controller_1_input, cmn::my_controller controller_2_input) {
  112. if(controller_1_input.is_master_controller) {
  113. return {controller_1_input, controller_2_input};
  114. } else {
  115. return {controller_2_input, controller_1_input};
  116. }
  117. }
  118.  
  119. // the specific instance of a class that stores the varibles for the drive controls used by our driver Chase
  120. cmn::my_controller chase_config(cmn::my_controller chase) {
  121. chase.arm_pneumatic_button = pros::E_CONTROLLER_DIGITAL_UP;
  122. chase.wrist_pneumatic_button = pros::E_CONTROLLER_DIGITAL_LEFT;
  123. chase.intake_pneumatic_button = pros::E_CONTROLLER_DIGITAL_RIGHT;
  124. chase.mogo_pneumatic_button = pros::E_CONTROLLER_DIGITAL_DOWN;
  125. chase.switch_controller_button = pros::E_CONTROLLER_DIGITAL_B;
  126. chase.intake_motor_button = pros::E_CONTROLLER_DIGITAL_R1;
  127. chase.extake_motor_button = pros::E_CONTROLLER_DIGITAL_L1;
  128. chase.load_arm_button = pros::E_CONTROLLER_DIGITAL_A;
  129. chase.chopper_mech = pros::E_CONTROLLER_DIGITAL_L2;
  130. chase.reverse = pros::E_CONTROLLER_DIGITAL_R2;
  131.  
  132. return chase;
  133. }
  134.  
  135. // the same thing but this is for our driver miles
  136. cmn::my_controller miles_config(cmn::my_controller miles) {
  137. miles.arm_pneumatic_button = pros::E_CONTROLLER_DIGITAL_L1;
  138. miles.wrist_pneumatic_button = pros::E_CONTROLLER_DIGITAL_L2;
  139. miles.mogo_pneumatic_button = pros::E_CONTROLLER_DIGITAL_DOWN;
  140. miles.switch_controller_button = pros::E_CONTROLLER_DIGITAL_RIGHT;
  141. miles.intake_motor_button = pros::E_CONTROLLER_DIGITAL_LEFT;
  142. miles.extake_motor_button = pros::E_CONTROLLER_DIGITAL_UP;
  143. miles.intake_pneumatic_button = pros::E_CONTROLLER_DIGITAL_A;
  144. miles.extake_motor_button = pros::E_CONTROLLER_DIGITAL_UP;
  145. miles.shift_button = pros::E_CONTROLLER_DIGITAL_X;
  146.  
  147. return miles;
  148. }
  149.  
  150. // construct an object for the brain printing class
  151. cmn::GUI gui;
  152.  
  153. // include the assets from the static folder used in lemlib pure pursuit, not used (yet)
  154. ASSET(path_jerryio_left_red_v1_1_txt);
  155. ASSET(path_jerryio_left_red_v1_2_txt);
  156. ASSET(path_jerryio_left_red_v1_3_txt);
  157. ASSET(path_jerryio_left_red_v1_4_txt);
  158. ASSET(path_jerryio_left_red_v1_5_txt);
  159. ASSET(example_txt);
  160. ASSET(ppTest_txt);
  161.  
  162.  
  163. // declare objects for the air tanks, tubes, pistons, and pneumatics components
  164. // declarations can get tedious because for every piece of real pneumatic components that have volume, an object must be constructed in the code
  165. cmn::Air_Tank tank1;
  166. cmn::Air_Tank tank2;
  167.  
  168. cmn::Tube tank1_to_T;
  169. cmn::Tube t1_main;
  170. cmn::Tube doinker;
  171. cmn::Tube arm;
  172. cmn::Tube connector;
  173. cmn::Tube intake_tb;
  174. cmn::Tube clamp;
  175. cmn::Tube aux;
  176. cmn::Tube intake2;
  177. cmn::Tube main2;
  178.  
  179. cmn::Piston ArmP;
  180. cmn::Piston DoinkerP;
  181. cmn::Piston intakeP;
  182. cmn::Piston auxP;
  183. cmn::Piston clamp1P;
  184. cmn::Piston clamp2P;
  185. cmn::Piston intake2P;
  186.  
  187. cmn::Pneumatics_Component ArmC;
  188. cmn::Pneumatics_Component DoinkerC;
  189. cmn::Pneumatics_Component AuxC;
  190. cmn::Pneumatics_Component IntakeC;
  191. cmn::Pneumatics_Component ClampC;
  192. cmn::Pneumatics_Component TanksC;
  193.  
  194. cmn::Pneumatics_System p_system;
  195.  
  196. cmn::Controller_Print controller_print;
  197.  
  198. // constructs and initalizes the pose of the robot within lemlib
  199. lemlib::Pose pose = chassis.getPose();
  200.  
  201. int seed;
  202.  
  203. void initialize() {
  204. // start be converting then calculating the volume of all the components used
  205. // #must be floats because it will not work properly when trying to preform floating point operations on an int
  206. tank1_to_T.length = (3.0f / 2.54);
  207. t1_main.length = (64.0f / 2.54);
  208. doinker.length = (39.0f / 2.54);
  209. arm.length = (42.0f / 2.54);
  210. connector.length = ((18.0f + 19.0f + 3.0f) / 2.54);
  211. intake_tb.length = (24.0f / 2.54);
  212. clamp.length = (41.0f / 2.54);
  213. aux.length = (3.0f / 2.54);
  214. intake2.length = ((6.0f + 3.0f) / 2.54);
  215. main2.length = (20.0f / 2.54);
  216.  
  217. ArmP.stroke = 75.0f;
  218. DoinkerP.stroke = 75.0f;
  219. intakeP.stroke = 75.0f;
  220. auxP.stroke = 75.0f;
  221. clamp1P.stroke = 50.0f;
  222. clamp2P.stroke = 50.0f;
  223. intake2P.stroke = 50.0f;
  224.  
  225. // calculate the actual volumes of each object
  226. ArmP.init();
  227. arm.init();
  228. DoinkerP.init();
  229. t1_main.init();
  230. doinker.init();
  231. aux.init();
  232. auxP.init();
  233. intakeP.init();
  234. intake2P.init();
  235. intake_tb.init();
  236. intake2.init();
  237. clamp1P.init();
  238. clamp2P.init();
  239. clamp.init();
  240. tank1_to_T.init();
  241. main2.init();
  242.  
  243. ArmC.volumes.push_back(ArmP.volume);
  244. ArmC.volumes.push_back(arm.volume);
  245.  
  246. DoinkerC.volumes.push_back(DoinkerP.volume);
  247. //DoinkerC.volumes.push_back(t1_main.volume);
  248. DoinkerC.volumes.push_back(doinker.volume);
  249.  
  250. AuxC.volumes.push_back(aux.volume);
  251. AuxC.volumes.push_back(auxP.volume);
  252.  
  253. IntakeC.volumes.push_back(intakeP.volume);
  254. IntakeC.volumes.push_back(intake_tb.volume);
  255. IntakeC.volumes.push_back(intake2P.volume);
  256. IntakeC.volumes.push_back(intake2.volume);
  257.  
  258. ClampC.volumes.push_back(clamp1P.volume);
  259. ClampC.volumes.push_back(clamp2P.volume);
  260. ClampC.volumes.push_back(clamp.volume);
  261.  
  262. TanksC.volumes.push_back(tank1_to_T.volume);
  263. TanksC.volumes.push_back(t1_main.volume);
  264. TanksC.volumes.push_back(main2.volume);
  265. TanksC.volumes.push_back(tank2.capacity);
  266. TanksC.volumes.push_back(tank1.capacity);
  267.  
  268. ArmC.compute_system_volume();
  269. DoinkerC.compute_system_volume();
  270. AuxC.compute_system_volume();
  271. IntakeC.compute_system_volume();
  272. ClampC.compute_system_volume();
  273. TanksC.compute_system_volume();
  274.  
  275. p_system.system_init({ArmC, DoinkerC, AuxC, IntakeC, ClampC, TanksC});
  276.  
  277. // prepare the screen for printing
  278. pros::lcd::initialize();
  279.  
  280. // the prefered drivetrain setup by both drivers
  281. chassis.setBrakeMode(pros::E_MOTOR_BRAKE_COAST);
  282.  
  283. // this is where we can name individual autons for display on the brain during selection
  284. gui.auton_name_list.push_back("RL AWP");
  285. gui.auton_name_list.push_back("RR E");
  286. gui.auton_name_list.push_back(" ");
  287. gui.auton_name_list.push_back(" ");
  288. gui.auton_name_list.push_back(" ");
  289. gui.auton_name_list.push_back(" ");
  290. gui.auton_name_list.push_back(" ");
  291. gui.auton_name_list.push_back(" ");
  292.  
  293. intake.set_encoder_units(pros::E_MOTOR_ENCODER_DEGREES);
  294.  
  295. seed = pros::micros() + std::time(0) + static_cast<int>(lm1.get_position());
  296.  
  297. // a critical function in order to get odemetry to work
  298. chassis.calibrate();
  299. }
  300.  
  301. void red_right_elims(lemlib::Chassis& chassis) {
  302. chassis.setPose(0, 0, 180);
  303. mogo_mech.retract();
  304.  
  305. // all the code for the autons
  306. // basically just scripting, not to exiting
  307. chassis.moveToPose(1, 13, 150, 2100, {.forwards=false, .maxSpeed= 71, .minSpeed=1, .earlyExitRange=0.25});
  308. chassis.moveToPose(-5, 29, 150, 1500, {.forwards=false, .maxSpeed=71});
  309. arm_mech.extend();
  310. pros::delay(1400);
  311. mogo_mech.extend();
  312. pros::delay(500);
  313. chassis.moveToPose(5, 18, 30, 2500, {.maxSpeed=71, .minSpeed=10, .earlyExitRange=1});
  314. intake_upper.move_velocity(200);
  315. intake_lower.move_velocity(200);
  316. chassis.moveToPose(17, 34, 30, 2500, {.maxSpeed=71, .minSpeed=10, .earlyExitRange=1});
  317. intake_lower.move_velocity(200);
  318. intake_upper.move_velocity(200);
  319. chassis.moveToPoint(-12, 24, 2500, {.maxSpeed=71, .minSpeed=10, .earlyExitRange=3});
  320. chassis.moveToPoint(-33, 8, 2500, {.maxSpeed=71, .minSpeed=10, .earlyExitRange=1});
  321. intake_mech.extend();
  322. intake.move_velocity(200);
  323. pros::delay(750);
  324. intake.move_velocity(200);
  325. chassis.turnToPoint(24, 2.5, 500, {.maxSpeed=71, .minSpeed=1, .earlyExitRange=.25});
  326. intake_mech.retract();
  327. arm_mech.retract();
  328. intake_upper.move_velocity(200);
  329. intake.move_velocity(200);
  330. pros::delay(500);
  331. intake_lower.move_velocity(0);
  332. chassis.moveToPose(20, 6, 105, 3000, {.maxSpeed=71, .minSpeed = 31, .earlyExitRange = 1});
  333. intake.move_velocity(200);
  334. pros::delay(500);
  335. intake_lower.move_velocity(0);
  336. intake_upper.move_velocity(200);
  337. chassis.moveToPose(16, 12, 315, 2000, {.maxSpeed=71, .minSpeed=10, .earlyExitRange=1});
  338. pros::delay(100);
  339. mogo_mech.retract();
  340. intake_upper.move_velocity(0);
  341. intake_upper.move_relative(-360, 200);
  342. chassis.moveToPose(22, 44, 180, 2750, {.forwards=false});
  343.  
  344.  
  345. /*chassis.setPose(-50, -30, 270);
  346. arm_mech.extend();
  347. chassis.moveToPose(-31.75, -27.75, 240, 2000, {.forwards=false, .minSpeed=31, .earlyExitRange = .5});
  348. chassis.moveToPose(-19, -22.5, 240, 2000, {.forwards=false, .minSpeed = 1, .earlyExitRange=.25});
  349. chassis.moveToPose(-33, -33.5, 180, 1200, {.minSpeed = 31, .earlyExitRange = .5});
  350. pros::delay(10);
  351. mogo_mech.extend();
  352. intake.move_velocity(200);
  353. chassis.moveToPoint(-20, -52, 2000, {.minSpeed = 28, .earlyExitRange =.5});
  354. intake.move_velocity(200);
  355. chassis.moveToPose(-33, -8, 315, 2000, {.minSpeed = 31, .earlyExitRange =1});
  356. intake_upper.move_velocity(200);
  357. chassis.moveToPose(-51, 4, 270, 2000, {.minSpeed = 1, .earlyExitRange = .1});
  358. intake.move_velocity(200);
  359. intake_mech.extend();
  360. chassis.moveToPose(-52, -52, 135, 3000, {.minSpeed = 31, .earlyExitRange = .1});
  361. intake_mech.retract();
  362. intake.move_velocity(200);
  363. mogo_mech.retract();
  364. chassis.moveToPose(-30, -30, 200, 5000, {.minSpeed = 1, .earlyExitRange = .5});
  365. chassis.moveToPose(-12, -45.5, 270, 5000, {.forwards=false, .minSpeed=1, .earlyExitRange=0.1});
  366. mogo_mech.retract();*/
  367. }
  368.  
  369. void blue_left_elims(lemlib::Chassis& chassis) {
  370. chassis.setPose(0, 0, 180);
  371. mogo_mech.retract();
  372.  
  373. // all the code for the autons
  374. // basically just scripting, not to exiting
  375. chassis.moveToPose(-1, 13, -150, 2100, {.forwards=false, .maxSpeed= 71, .minSpeed=1, .earlyExitRange=0.25});
  376. arm_mech.extend();
  377. chassis.moveToPose(7, 30, -150, 1500, {.forwards=false, .maxSpeed=71});
  378. pros::delay(1400);
  379. mogo_mech.extend();
  380. pros::delay(500);
  381. chassis.moveToPose(-5, 18, -30, 2500, {.maxSpeed=71, .minSpeed=10, .earlyExitRange=1});
  382. intake_upper.move_velocity(200);
  383. intake_lower.move_velocity(200);
  384. chassis.moveToPose(-17, 34, -30, 2500, {.maxSpeed=71, .minSpeed=10, .earlyExitRange=1});
  385. intake_lower.move_velocity(200);
  386. intake_upper.move_velocity(200);
  387. chassis.moveToPoint(12, 24, 2500, {.maxSpeed=71, .minSpeed=10, .earlyExitRange=3});
  388. chassis.moveToPoint(33, 8, 2500, {.maxSpeed=71, .minSpeed=10, .earlyExitRange=1});
  389. intake_mech.extend();
  390. intake.move_velocity(200);
  391. pros::delay(750);
  392. intake.move_velocity(200);
  393. chassis.turnToPoint(-24, 2.5, 500, {.maxSpeed=71, .minSpeed=1, .earlyExitRange=.25});
  394. intake_mech.retract();
  395. arm_mech.retract();
  396. intake_upper.move_velocity(200);
  397. intake.move_velocity(200);
  398. pros::delay(500);
  399. intake_lower.move_velocity(0);
  400. chassis.moveToPose(-20, 6, -105, 3000, {.maxSpeed=71, .minSpeed = 31, .earlyExitRange = 1});
  401. intake.move_velocity(200);
  402. pros::delay(500);
  403. intake_lower.move_velocity(0);
  404. intake_upper.move_velocity(200);
  405. chassis.moveToPose(-16, 12, -315, 2000, {.maxSpeed=71, .minSpeed=10, .earlyExitRange=1});
  406. pros::delay(100);
  407. mogo_mech.retract();
  408. intake_upper.move_velocity(0);
  409. intake_upper.move_relative(-360, 200);
  410. chassis.moveToPose(-22, 44, 180, 2750, {.forwards=false});
  411.  
  412.  
  413. /*chassis.setPose(-50, -30, 270);
  414. arm_mech.extend();
  415. chassis.moveToPose(-31.75, -27.75, 240, 2000, {.forwards=false, .minSpeed=31, .earlyExitRange = .5});
  416. chassis.moveToPose(-19, -22.5, 240, 2000, {.forwards=false, .minSpeed = 1, .earlyExitRange=.25});
  417. chassis.moveToPose(-33, -33.5, 180, 1200, {.minSpeed = 31, .earlyExitRange = .5});
  418. pros::delay(10);
  419. mogo_mech.extend();
  420. intake.move_velocity(200);
  421. chassis.moveToPoint(-20, -52, 2000, {.minSpeed = 28, .earlyExitRange =.5});
  422. intake.move_velocity(200);
  423. chassis.moveToPose(-33, -8, 315, 2000, {.minSpeed = 31, .earlyExitRange =1});
  424. intake_upper.move_velocity(200);
  425. chassis.moveToPose(-51, 4, 270, 2000, {.minSpeed = 1, .earlyExitRange = .1});
  426. intake.move_velocity(200);
  427. intake_mech.extend();
  428. chassis.moveToPose(-52, -52, 135, 3000, {.minSpeed = 31, .earlyExitRange = .1});
  429. intake_mech.retract();
  430. intake.move_velocity(200);
  431. mogo_mech.retract();
  432. chassis.moveToPose(-30, -30, 200, 5000, {.minSpeed = 1, .earlyExitRange = .5});
  433. chassis.moveToPose(-12, -45.5, 270, 5000, {.forwards=false, .minSpeed=1, .earlyExitRange=0.1});
  434. mogo_mech.retract();*/
  435. }
  436.  
  437. void skills(lemlib::Chassis& chassis) {
  438.  
  439. if (!USE_AI) {
  440. chassis.setPose(0, 0, 0);
  441. chassis.moveToPose(12, -24, 0, 5000, {.forwards=false, .minSpeed=1, .earlyExitRange=0.25});
  442. mogo_mech.retract();
  443. chassis.moveToPose(-5, -62, 45, 5000, {.forwards=false});
  444. pros::delay(500);
  445. mogo_mech.extend();
  446. intake_upper.move_velocity(200);
  447. chassis.moveToPoint(24, -62 + 24, 2000);
  448. mogo_mech.retract();
  449. pros::delay(500);
  450. intake_upper.move_velocity(0);
  451. } else {
  452. NeuralNetwork agent;
  453. AdamWOptimizer agentOptimizer(0.001, 0.9, 0.999, 0.01, 1e-4);
  454.  
  455. agent.add_layer(Layer(5, 256, "relu", agentOptimizer));
  456. agent.add_layer(Layer(256, 256, "relu", agentOptimizer));
  457. agent.add_layer(Layer(256, 256, "relu", agentOptimizer));
  458. agent.add_layer(Layer(256, 5, "linear", agentOptimizer));
  459.  
  460. agent.load("agent_network_params.txt");
  461.  
  462. std::vector<std::vector<double>> outputs = agent.forward({lm1.get_actual_velocity(), rm1.get_actual_velocity(), intake_lower.get_actual_velocity(), intake_upper.get_actual_velocity(), static_cast<double>(mogo_mech.is_extended())});
  463.  
  464. left_motors.move_velocity(outputs[0][0]);
  465. right_motors.move_velocity(outputs[0][1]);
  466. intake_lower.move_velocity(outputs[0][2]);
  467. intake_upper.move_velocity(outputs[0][3]);
  468. mogo_mech.set_value(static_cast<bool>(std::round(outputs[0][4])));
  469.  
  470. pros::delay(100);
  471. }
  472. }
  473.  
  474. double get_rand_double() {
  475. std::srand(seed);
  476. return static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
  477. }
  478.  
  479. double derating_level () {
  480. return E_ENTROPY < get_rand_double() ? .5 > get_rand_double() ? 12.5 : 25 : 100;
  481. }
  482.  
  483. void autonomous() {
  484. // Switch the brain to displaying current motor temperature status in preperation for the match
  485. gui.current_view = 1;
  486.  
  487. if (PROGRAM == "Blue") {
  488. blue_left_elims(chassis);
  489. } else if (PROGRAM == "Red") {
  490. red_right_elims(chassis);
  491. } else if (PROGRAM == "Skills") {
  492. skills(chassis);
  493. }
  494.  
  495. // determines which autons run based on the one selected on the previous screen
  496. /*switch (gui.selected_auton) {
  497. case 0:
  498. red_right_elims(chassis);
  499. break;
  500. case 1:
  501. blue_left_elims(chassis);
  502. break;
  503. default:
  504. break;
  505.  
  506. }*/
  507. }
  508.  
  509. void opcontrol() {
  510.  
  511. mogo_mech.extend();
  512.  
  513. gui.current_view = 1;
  514.  
  515. // gets a baseline for when opcontrol starts
  516. unsigned int current_time = pros::millis();
  517.  
  518. // constructs objects, initializes objects, etc...
  519. bool arm_toggle = 0;
  520.  
  521. cmn::my_controller chase;
  522. cmn::my_controller miles;
  523.  
  524. chase.controller = &controller_1;
  525. miles.controller = &controller_2;
  526.  
  527. chase = chase_config(chase);
  528. miles = miles_config(miles);
  529.  
  530. chase.is_master_controller = true;
  531. miles.is_master_controller = false;
  532.  
  533. cmn::my_controller master;
  534. cmn::my_controller slave;
  535.  
  536. gui.motor_list.push_back(lm1);
  537. gui.motor_list.push_back(lm2);
  538. gui.motor_list.push_back(lm3);
  539. gui.motor_list.push_back(rm1);
  540. gui.motor_list.push_back(rm2);
  541. gui.motor_list.push_back(rm3);
  542. gui.motor_list.push_back(intake_lower);
  543. gui.motor_list.push_back(intake_upper);
  544.  
  545. master = get_active_controller(chase, miles)[1];
  546. slave = get_active_controller(chase, miles)[0];
  547.  
  548. controller_print.cont_drivetrain.push_back(lm1);
  549. controller_print.cont_drivetrain.push_back(lm2);
  550. controller_print.cont_drivetrain.push_back(lm3);
  551. controller_print.cont_drivetrain.push_back(rm1);
  552. controller_print.cont_drivetrain.push_back(rm2);
  553. controller_print.cont_drivetrain.push_back(rm3);
  554.  
  555. controller_print.init(chase, miles);
  556.  
  557. arm_mech.extend();
  558.  
  559. std::srand(seed);
  560.  
  561. double throttle_rand = get_rand_double();
  562. double steer_rand = get_rand_double();
  563.  
  564. double delay_extremity = get_rand_double() * 1000;
  565.  
  566. double arm_delay = E_ENTROPY < get_rand_double() ? get_rand_double() * delay_extremity : 0;
  567. double wrist_delay = E_ENTROPY < get_rand_double() ? get_rand_double() * delay_extremity : 0;
  568. double intake_mech_delay = E_ENTROPY < get_rand_double() ? get_rand_double() * delay_extremity : 0;
  569. double mogo_delay = E_ENTROPY < get_rand_double() ? get_rand_double() * delay_extremity : 0;
  570. double intake_delay = E_ENTROPY < get_rand_double() ? get_rand_double() * delay_extremity : 0;
  571. double chopper_delay = E_ENTROPY < get_rand_double() ? get_rand_double() * delay_extremity : 0;
  572.  
  573. arm_delay = E_VARIENTCE == true ? arm_delay : 0;
  574. wrist_delay = E_VARIENTCE == true ? wrist_delay : 0;
  575. intake_mech_delay = E_VARIENTCE == true ? intake_mech_delay : 0;
  576. mogo_delay = E_VARIENTCE == true ? mogo_delay : 0;
  577. intake_delay = E_VARIENTCE == true ? intake_delay : 0;
  578. chopper_delay = E_VARIENTCE == true ? get_rand_double() > .5 ? chopper_delay : 0 : 0;
  579.  
  580. //left_motors.set_voltage_limit_all(derating_level() * 120);
  581. //right_motors.set_voltage_limit_all(derating_level() * 120);
  582. // intake.set_voltage_limit_all(derating_level() * 120);
  583.  
  584. // runs until the program is turned off
  585. while(1){
  586. // finds the current time relative to when the opcontrol function was called relitive to the initalialize funtion
  587. unsigned int opcontrol_time = pros::millis() - current_time;
  588. // set the current pneumatics for printing on the controller
  589. controller_print.p_sys = p_system;
  590. // creates an inverse of opcontrol_time for keeping track of remaining time during a match
  591. unsigned int time_left;
  592.  
  593. if(opcontrol_time > 105000) {
  594. time_left = 0;
  595. } else if (PROGRAM != "Skills") {
  596. time_left = 105000 - opcontrol_time;
  597. } else {
  598. time_left = 60000 - opcontrol_time;
  599. }
  600.  
  601. controller_print.time = time_left;
  602.  
  603. if ((time_left < 15000 && time_left > 12000 ) && PROGRAM != "Skills") {
  604. master.controller->rumble("-");
  605. slave.controller->rumble("-");
  606. } else if (PROGRAM == "Skills") {
  607. if ((time_left > 60000 - 12000 && time_left < 60000 - 15000) || (time_left > 60000 - (12000 + 15000) && time_left < 60000 - (15000 + 15000)) || (time_left > 60000 - (12000 + 30000) && time_left < 60000 - (15000 + 30000)) || (time_left > 60000 - (12000 + 45000) && time_left < 60000 - (15000 + 45000))) {
  608. master.controller->rumble("-");
  609. }
  610. }
  611.  
  612. // a check to see which controller is the one controlling the robot and to see which one is reciving the stats
  613. if(master.controller->get_digital(master.switch_controller_button)) {
  614. chase.is_master_controller = chase.is_master_controller;
  615. miles.is_master_controller = !miles.is_master_controller;
  616. master = get_active_controller(chase, miles)[0];
  617. slave = get_active_controller(chase, miles)[1];
  618. master.controller->rumble("-");
  619. chase.controller->clear();
  620. miles.controller->clear();
  621. }
  622.  
  623. // a hacky way to get the "master" and "slave"
  624. master = get_active_controller(chase, miles)[0];
  625. slave = get_active_controller(chase, miles)[1];
  626.  
  627. // all of the basic robot control scriping
  628.  
  629. double throttle, steer;
  630.  
  631. throttle = master.controller->get_analog(master.throttle_axis);
  632. steer = master.controller->get_analog(master.steer_axis);
  633.  
  634. if (E_VARIENTCE == true) {
  635. //throttle = (E_ENTROPY < get_rand_double() ? ((throttle_rand * 2 - 1) * .5) * throttle : 0) + throttle;
  636. //steer = (E_ENTROPY < get_rand_double() ? ((steer_rand * 2 - 1) * .5) * steer : 0) + steer;
  637. }
  638.  
  639. if (master.controller->get_digital(master.reverse)) {
  640. throttle = -throttle;
  641. }
  642.  
  643. // uses curvature drive when not at slow speeds, otherwise uses arcarde drive
  644. if((throttle > 2) && (steer > 2)){
  645. chassis.curvature(throttle, steer);
  646. } else {
  647. chassis.arcade(throttle, steer);
  648. }
  649.  
  650. if(master.controller->get_digital(master.arm_pneumatic_button)) {
  651. pros::delay(arm_delay);
  652. arm_mech.toggle();
  653. p_system.cycled(ArmC);
  654. while (master.controller->get_digital(master.arm_pneumatic_button)) {
  655. pros::delay(2);
  656. }
  657. if (!arm_mech.is_extended()) {
  658. wrist_mech.retract();
  659. }
  660. }
  661.  
  662. if(master.controller->get_digital(master.wrist_pneumatic_button)) {
  663. pros::delay(wrist_delay);
  664. wrist_mech.toggle();
  665. p_system.cycled(AuxC);
  666. while(master.controller->get_digital(master.wrist_pneumatic_button)) {
  667. pros::delay(2);
  668. }
  669. }
  670.  
  671. if(master.controller->get_digital(master.intake_pneumatic_button)) {
  672. pros::delay(intake_mech_delay);
  673. intake_mech.toggle();
  674. p_system.cycled(IntakeC);
  675. while(master.controller->get_digital(master.intake_pneumatic_button)) {
  676. pros::delay(2);
  677. }
  678. }
  679.  
  680. if(master.controller->get_digital(master.mogo_pneumatic_button)) {
  681. pros::delay(mogo_delay);
  682. mogo_mech.toggle();
  683. if(mogo_mech.is_extended() == false) {
  684. intake_upper.move_relative(-600.0f, 200);
  685. }
  686. p_system.cycled(ClampC);
  687. while(master.controller->get_digital(master.mogo_pneumatic_button)) {
  688. pros::delay(2);
  689. }
  690. }
  691.  
  692. if (master.controller->get_digital(master.intake_motor_button)) {
  693. pros::delay(intake_delay);
  694. intake.move_velocity(200);
  695. } else if (master.controller->get_digital(master.extake_motor_button)) {
  696. pros::delay(intake_delay);
  697. intake.move_velocity(-200);
  698. } else {
  699. intake.move_velocity(0);
  700. }
  701.  
  702. if (master.controller->get_digital(master.chopper_mech)) {
  703. pros::delay(chopper_delay);
  704. chopper_mech.extend();
  705. p_system.cycled(DoinkerC);
  706. } else {
  707. pros::delay(chopper_delay);
  708. chopper_mech.retract();
  709. }
  710.  
  711. // renders the brain
  712. gui.render();
  713.  
  714. // renders the controllers
  715. controller_print.print_master(master);
  716. controller_print.print_slave(slave);
  717.  
  718. // would be more important if we had thread scheduling, but for now its just there as good practice
  719. pros::delay(15);
  720. }
  721. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
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!