API tools faq
paste
TungstenVn

CamTayTest 9.jul

TungstenVn
Jul 9th, 2019
239
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C# 8.27 KB | None | 0 0
raw download clone embed print report
  1. #include <PS2X_lib.h>
  2.  
  3. #define PS2_DAT 5 // data
  4. #define PS2_CMD 2 //command
  5. #define PS2_SEL 3 // attention
  6. #define PS2_CLK 4 //clock
  7. PS2X ps2x; // tạo PS2 điều khiển lớp
  8. int error = 0;
  9. byte type = 0;
  10. byte vibrate = 0;
  11.  
  12. //Chẵn là dương thì thuận chiều, lẽ là dương thì ngược chiều
  13.  
  14. const byte MotorInput[4] = {3, 9, 10, 11};
  15. const byte MotorOutput[4] = {5, 8, 12, 13};
  16. //Pittong - Dùng High,Low
  17. #define LeftHand 1
  18. #define RightHand 2
  19. #define MiddleHand 4
  20. bool isCloseL = false; //Ktra tay trái đóng hay mở
  21. bool isCloseR = false;
  22. bool isCloseM = false;
  23. bool isCloseBoth = false; //Ktra cả 2 tay đang đóng hay mở
  24. bool isPress1 = false, isPress2 = false;
  25. //Ổ bi,dùng digital
  26. #define obi_1 6
  27. #define obi_2 7
  28. //0:tay trai,1:tay phai,2:tay giua,3:dong mo ca 2 tay,4:d/c keo di len, 5:d/c keo di xuong
  29. boolean buttons[6] = {false};
  30. //---------------------------------------------------------------------------------------------------------------------------------
  31. void setup()
  32. {
  33.   Serial.begin(9600);
  34.   error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, false, false);
  35.   if (error == 0) {
  36.     Serial.print("Đã tìm thấy bộ điều khiển ");
  37.   }
  38.   else if (error == 1)
  39.     Serial.println("Không kết nối đc, thử lại...");
  40.  
  41.   else if (error == 2)
  42.     Serial.println("Bộ điều khiển tìm thấy nhưng không chấp nhận lệnh");
  43.  
  44.   else if (error == 3)
  45.     Serial.println("Bộ điều khiển từ chối để vào chế độ Pressures ,hoặc ko hỗ trợ");
  46.  
  47.   type = ps2x.readType();
  48.   switch (type) {
  49.     case 0:
  50.       Serial.println("Tay điều khiển không phù hợp ");
  51.       break;
  52.     case 1:
  53.       Serial.println("Đã tìm thấy DualShock ");
  54.       break;
  55.   }
  56.   a();
  57. }
  58. void a()
  59. {
  60.   error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, false, false);
  61.   if (error == 0) {
  62.     Serial.print("Đã tìm thấy bộ điều khiển ");
  63.   }
  64.   else if (error == 1)
  65.     Serial.println("Không kết nối đc, thử lại...");
  66.  
  67.   else if (error == 2)
  68.     Serial.println("Bộ điều khiển tìm thấy nhưng không chấp nhận lệnh");
  69.  
  70.   else if (error == 3)
  71.     Serial.println("Bộ điều khiển từ chối để vào chế độ Pressures ,hoặc ko hỗ trợ");
  72.  
  73.   type = ps2x.readType();
  74.   switch (type) {
  75.     case 0:
  76.       Serial.println("Tay điều khiển không phù hợp ");
  77.       break;
  78.     case 1:
  79.       Serial.println("Đã tìm thấy DualShock ");
  80.       break;
  81.   }
  82.  
  83. }
  84.  
  85. int Channels[3] = {0};
  86. long long int sys_start = 0;
  87. int MAX_SPEED = 255;
  88. int MIN_SPEED = -MAX_SPEED;
  89. //=======================================================================================================================================
  90. void loop() {
  91.   buttonState();
  92.   ProcessMotor2();
  93.   //taygap();
  94. }
  95.  
  96. void buttonState() {
  97.  
  98.  
  99.   ps2x.read_gamepad(false, vibrate);
  100.  
  101.   Channels[0] = map(ps2x.Analog(PSS_LY), 256, 0, -255, 255);
  102.   Channels[1] = map(ps2x.Analog(PSS_LX), 0, 256, -255, 255);
  103.   Channels[2] = map(ps2x.Analog(PSS_RX), 0, 256, -255, 255);
  104.  
  105.   for (int i = 0; i < 3; i++) {
  106.     if (abs(Channels[i]) <= 100) {
  107.       Channels[i] = 0;
  108.     }
  109.   }
  110.  
  111.   int sum = 0;
  112.   for (int i = 0; i < 3; i++) {
  113.     sum += abs(Channels[i]);
  114.   }
  115.   sum /= 3;
  116.   if (sum < 20) {
  117.     sys_start = millis();
  118.   }
  119.  
  120.   Channels[2] /= 3.5;
  121.   if (millis() - sys_start < 400) {
  122.     Channels[2] = (float)(Channels[2] * ((millis() - sys_start + 310)) / 710);
  123.   }
  124.  
  125.   taygap();
  126.  
  127. }
  128.  
  129.  
  130.  
  131.  
  132.  
  133. bool isMove_obi = false;
  134. void taygap() {
  135.   //ps2x.read_gamepad(false, vibrate);
  136.   /*
  137.      Serial.print(ps2x.Analog(PSS_LY));
  138.     Serial.print("    ");
  139.     Serial.print(ps2x.Analog(PSS_LX));
  140.     Serial.print("    ");
  141.     Serial.print(ps2x.Analog(PSS_RX));
  142.     Serial.println();
  143.   */
  144.  
  145.   //----------------Ổ bi-------------------------------------------------------------------------
  146.   bool temp = ps2x.Button(PSB_PAD_UP);
  147.   if (temp != buttons[4]) {
  148.     buttons[4] = temp;
  149.     if (temp) {
  150.       digitalWrite(obi_1, HIGH);
  151.       Serial.println("Keo Len");
  152.     } else {
  153.       digitalWrite(obi_1, LOW);
  154.       Serial.println("STOP keo");
  155.     }
  156.   }
  157.  
  158.  
  159.   temp = ps2x.Button(PSB_PAD_DOWN);
  160.   if (temp != buttons[5]) {
  161.     buttons[5] = temp;
  162.     if (temp) {
  163.       digitalWrite(obi_2, HIGH);
  164.       Serial.println("Keo Xuong");
  165.     } else {
  166.       digitalWrite(obi_2, LOW);
  167.       Serial.println("Stop keo");
  168.     }
  169.   }
  170.   //------------------Tay Gắp--------------------------------------
  171.  
  172.   temp = ps2x.Button(PSB_PINK);
  173.   if (temp != buttons[0]) { //Tay Trái
  174.     buttons[0] = temp;
  175.     if (temp) {
  176.       if (!isCloseL) {
  177.         Serial.println("Dong tay trai");
  178.         digitalWrite(13, HIGH);
  179.         digitalWrite(LeftHand, HIGH);
  180.         isCloseL = true;
  181.       } else {
  182.         Serial.print("Mo tay trai");
  183.         digitalWrite(13, LOW);
  184.         digitalWrite(LeftHand, LOW);
  185.         isCloseL = false;
  186.       }
  187.     }
  188.   }
  189.   //-----------------------------------------------
  190.  
  191.  
  192.  
  193.   temp = ps2x.Button(PSB_RED);
  194.   if (temp != buttons[1]) { //Tay Phải
  195.     buttons[1] = temp;
  196.     if (temp) {
  197.       if (!isCloseR) {
  198.         Serial.println("Dong tay phai");
  199.         digitalWrite(RightHand, HIGH);
  200.         isCloseR = true;
  201.       } else {
  202.         Serial.println("Mo tay phai");
  203.         digitalWrite(RightHand, LOW);
  204.         isCloseR = false;
  205.       }
  206.     }
  207.   }
  208.   //-----------------------------------------------
  209.  
  210.  
  211.  
  212.   temp = ps2x.Button(PSB_TRIANGLE);
  213.   if (temp != buttons[2]) { //Tay Giữa
  214.     buttons[2] = temp;
  215.     if (temp) {
  216.       if (!isCloseM) {
  217.         Serial.println("Dong tay giua");
  218.         digitalWrite(MiddleHand, HIGH);
  219.         isCloseM = true;
  220.       } else {
  221.         Serial.println("Mo tay giua");
  222.         digitalWrite(MiddleHand, LOW);
  223.         isCloseM = false;
  224.       }
  225.     }
  226.   }
  227.  
  228.   //-----------------------------------------------
  229.   temp = ps2x.Button(PSB_BLUE);
  230.   if (temp != buttons[3]) { //Đóng mở 2 tay cùng lúc
  231.     buttons[3] = temp;
  232.     if (temp) {
  233.       if (!isCloseBoth) {
  234.         Serial.println("Dong ca hai tay");
  235.         digitalWrite(LeftHand, HIGH);
  236.         digitalWrite(RightHand, HIGH);
  237.         isCloseL = true;
  238.         isCloseR = true;
  239.         isCloseBoth = true;
  240.       } else {
  241.         Serial.println("Mo ca hai tay");
  242.         digitalWrite(LeftHand, LOW);
  243.         digitalWrite(RightHand, LOW);
  244.         isCloseL = false;
  245.         isCloseR = false;
  246.         isCloseBoth = false;
  247.       }
  248.     }
  249.   }
  250. }
  251.  
  252.  
  253. void ProcessMotor2() {
  254.  
  255.   int Motors[4] = {0};
  256.  
  257.   Motors[0] = Channels[0];
  258.   Motors[1] = Channels[0];
  259.   Motors[2] = Channels[0];
  260.   Motors[3] = Channels[0];
  261.  
  262.   Motors[0] += Channels[1];
  263.   Motors[1] -= Channels[1];
  264.   Motors[2] -= Channels[1];
  265.   Motors[3] += Channels[1];
  266.  
  267.   Motors[0] += Channels[2];
  268.   Motors[1] -= Channels[2];
  269.   Motors[2] += Channels[2];
  270.   Motors[3] -= Channels[2];
  271.  
  272.   //SetSpeed2(Motors);
  273.  
  274. }
  275. int SetSpeed2(int* motors) {
  276.   Constrains(motors);
  277. //E xoa vong for thi moi chay dc nhu clip, con neu co thi k chay dc
  278. /*
  279. for (int i = 0; i < 4; i++) {
  280.     if (motors[i] >= 0) {
  281.       analogWrite(MotorInput[i], motors[i]);
  282.       analogWrite(MotorOutput[i], 0);
  283.       //Serial.print("Thuan");
  284.       //Serial.print("    ");
  285.     } else {
  286.       analogWrite(MotorInput[i], 255 + motors[i]);
  287.       analogWrite(MotorOutput[i], 255);
  288.       //Serial.print("Nghich");
  289.       //Serial.print("    ");
  290.     }
  291.   }
  292. */
  293. }
  294.  
  295. void Constrains2(int *motors) {
  296.   int TEMP_MAX_SPEED = 0;
  297.   if (millis() - sys_start < 400) {
  298.     TEMP_MAX_SPEED = (float)(MAX_SPEED * ((millis() - sys_start + 800)) / 1200);
  299.   } else {
  300.     TEMP_MAX_SPEED = MAX_SPEED;
  301.   }
  302.  
  303.   int TEMP_MIN_SPEED = -TEMP_MAX_SPEED;
  304.  
  305.   /*Gotta make sure every value is vaild*/
  306.   int temp = TEMP_MIN_SPEED;
  307.   for (int i = 0; i < 4; i++) {
  308.     if (motors[i] > temp) {
  309.       temp = motors[i];
  310.     }
  311.   }
  312.  
  313.   if (temp > TEMP_MAX_SPEED) {
  314.     float ratio = (float)TEMP_MAX_SPEED / temp;
  315.     for (int i = 0; i < 4; i++) {
  316.       motors[i] *= ratio;
  317.     }
  318.   }
  319.  
  320.   temp = TEMP_MAX_SPEED;
  321.   for (int i = 0; i < 4; i++) {
  322.     if (motors[i] < temp) {
  323.       temp = motors[i];
  324.     }
  325.   }
  326.  
  327.   if (temp < TEMP_MIN_SPEED) {
  328.     float ratio = (float)TEMP_MIN_SPEED / temp;
  329.     for (int i = 0; i < 4; i++) {
  330.       motors[i] *= ratio;
  331.     }
  332.   }
  333. }
Advertisement
Add Comment
Please, Sign In to add comment
Public Pastes
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!