API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Oct 17th, 2019
129
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 9.80 KB | None | 0 0
raw download clone embed print report
  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. }
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!