API tools faq
paste
vencinachev

Mecanum-Robot

vencinachev
Mar 30th, 2021 (edited)
368
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Arduino 13.05 KB | None | 0 0
raw download clone embed print report
  1. #include <PS2X_lib.h>
  2. #include <PID_v1.h>
  3. #define SPEED_CONST 230
  4.  
  5. // ENCODERS PINS
  6. #define EFLA 45
  7. #define EFLB 19
  8. #define EFRA 43
  9. #define EFRB 18
  10. #define EBLA 39
  11. #define EBLB 21
  12. #define EBRA 41
  13. #define EBRB 20
  14.  
  15. // MOTOR PINS
  16. #define MFL1 3
  17. #define MFL2 2
  18. #define MFR1 5
  19. #define MFR2 4
  20. #define MBL1 8
  21. #define MBL2 9
  22. #define MBR1 6
  23. #define MBR2 7
  24.  
  25. enum State {
  26.   STOP, FWL, FWD, FWR, LEFT, CW, ACW, RIGHT, BWL, BWD, BWR
  27. };
  28.  
  29. int timeD = 10; //ms
  30.  
  31. double pwmFL = 0;
  32. double pwmFR = 0;
  33. double pwmBL = 0;
  34. double pwmBR = 0;
  35.  
  36. double speedFL = 0;
  37. double speedFR = 0;
  38. double speedBL = 0;
  39. double speedBR = 0;
  40.  
  41. volatile long cntFL = 0;
  42. volatile long cntFR = 0;
  43. volatile long cntBL = 0;
  44. volatile long cntBR = 0;
  45.  
  46. double SetFL, SetFR, SetBL, SetBR;
  47. double Kp = 2, Ki = 1.5, Kd = 0.5;
  48.  
  49. PID flPID(&speedFL, &pwmFL, &SetFL, Kp, Ki, Kd, DIRECT);
  50. PID frPID(&speedFR, &pwmFR, &SetFR, Kp, Ki, Kd, DIRECT);
  51. PID blPID(&speedBL, &pwmBL, &SetBL, Kp, Ki, Kd, DIRECT);
  52. PID brPID(&speedBR, &pwmBR, &SetBR, Kp, Ki, Kd, DIRECT);
  53.  
  54. PS2X ps2x;
  55. int speedPID= 120;
  56. int speedOpen = 100;
  57. State state = STOP;
  58. bool isOpen = true;
  59.  
  60. void setup() {
  61.   Serial2.begin(9600);   // bluetooth
  62.   pinMode(MFL1, OUTPUT);
  63.   pinMode(MFL2, OUTPUT);
  64.   pinMode(MFR1, OUTPUT);
  65.   pinMode(MFR2, OUTPUT);
  66.   pinMode(MBL1, OUTPUT);
  67.   pinMode(MBL2, OUTPUT);
  68.   pinMode(MBR1, OUTPUT);
  69.   pinMode(MBR2, OUTPUT);
  70.   pinMode(EFLA, INPUT);
  71.   pinMode(EFRA, INPUT);
  72.   pinMode(EBLA, INPUT);
  73.   pinMode(EBRA, INPUT);
  74.   pinMode(EFLB, INPUT);
  75.   attachInterrupt(digitalPinToInterrupt(EFLB), encoderFL, FALLING);
  76.   attachInterrupt(digitalPinToInterrupt(EFRB), encoderFR, FALLING);
  77.   attachInterrupt(digitalPinToInterrupt(EBLB), encoderBL, FALLING);
  78.   attachInterrupt(digitalPinToInterrupt(EBRB), encoderBR, FALLING);
  79.   //setup pins and settings:  GamePad(clock, command, attention, data, Pressures?, Rumble?)
  80.   ps2x.config_gamepad(31,33,35,37, true, true);
  81.   SetFL = SetFR = SetBL = SetBR = 60;
  82.   flPID.SetMode(AUTOMATIC);
  83.   frPID.SetMode(AUTOMATIC);
  84.   blPID.SetMode(AUTOMATIC);
  85.   brPID.SetMode(AUTOMATIC);
  86.   flPID.SetSampleTime(100);
  87.   frPID.SetSampleTime(100);
  88.   blPID.SetSampleTime(100);
  89.   brPID.SetSampleTime(100);
  90. }
  91.  
  92. void loop() {
  93.   // joystick control
  94.   ps2x.read_gamepad(false, 0);
  95.   if(ps2x.ButtonPressed(PSB_L3)) {  
  96.     speedOpen = 255 - ps2x.Analog(PSS_RY);
  97.   } else if(ps2x.ButtonPressed(PSB_R3)) {  
  98.     speedOpen = 255 - ps2x.Analog(PSS_LY);
  99.   } else if(ps2x.ButtonPressed(PSB_PAD_RIGHT)) {  
  100.          state = RIGHT;
  101.   } else if(ps2x.ButtonPressed(PSB_BLUE)) {          
  102.          speedOpen = 100;
  103.   } else if(ps2x.ButtonPressed(PSB_GREEN)) {          
  104.          speedOpen = 255;
  105.   } else if(ps2x.ButtonPressed(PSB_PAD_LEFT)) {          
  106.          state = LEFT;
  107.   } else if(ps2x.ButtonPressed(PSB_PAD_DOWN)) {        
  108.          state = BWD;
  109.   } else if(ps2x.ButtonPressed(PSB_PAD_UP)) {            
  110.          state = FWD;
  111.   } else if(ps2x.ButtonPressed(PSB_RED)) {            
  112.          state = CW;
  113.   } else if(ps2x.ButtonPressed(PSB_PINK)) {          
  114.          state = ACW;
  115.   } else if(ps2x.ButtonPressed(PSB_L1)) {            
  116.          state = FWL;
  117.   } else if(ps2x.ButtonPressed(PSB_L2)) {            
  118.          state = BWL;
  119.   } else if(ps2x.ButtonPressed(PSB_R1)) {            
  120.          state = FWR;
  121.   } else if(ps2x.ButtonPressed(PSB_R2)) {            
  122.          state = BWR;
  123.   } else if(ps2x.ButtonReleased(PSB_L1) ||
  124.             ps2x.ButtonReleased(PSB_L2) ||
  125.             ps2x.ButtonReleased(PSB_R1) ||
  126.             ps2x.ButtonReleased(PSB_R2) ||
  127.             ps2x.ButtonReleased(PSB_PINK) ||
  128.             ps2x.ButtonReleased(PSB_RED) ||
  129.             ps2x.ButtonReleased(PSB_PAD_UP) ||
  130.             ps2x.ButtonReleased(PSB_PAD_DOWN) ||
  131.             ps2x.ButtonReleased(PSB_PAD_LEFT) ||
  132.             ps2x.ButtonReleased(PSB_PAD_RIGHT))  {
  133.          state = STOP;
  134.          stop();
  135.   }
  136.  
  137.   // bluetooth control
  138.   if (Serial2.available() > 0){
  139.     String command = Serial2.readStringUntil('\n');
  140.     if (command == "stop") {
  141.       state = STOP;
  142.       stop();
  143.     } else if (command == "open") {
  144.       isOpen = true;
  145.     } else if (command[0] == 'P'){
  146.       // PID set
  147.       isOpen = false;
  148.       double kp = command.substring(1, 6).toDouble();
  149.       double ki = command.substring(6, 11).toDouble();
  150.       double kd = command.substring(11, 16).toDouble();
  151.       flPID.SetTunings(kp, ki, kd);
  152.       frPID.SetTunings(kp, ki, kd);
  153.       blPID.SetTunings(kp, ki, kd);
  154.       brPID.SetTunings(kp, ki, kd);
  155.     } else if (command == "fwd"){
  156.       state = FWD;
  157.     } else if (command == "back"){
  158.       state = BWD;
  159.     } else if (command == "left"){
  160.       state = LEFT;
  161.     } else if (command == "right"){
  162.       state = RIGHT;
  163.     } else if (command == "fwl") {
  164.       state = FWL;
  165.     } else if (command == "fwr") {
  166.       state = FWR;
  167.     } else if (command == "bwl") {
  168.       state = BWL;
  169.     } else if (command == "bwr") {
  170.       state = BWR;
  171.     } else if (command == "clockwise") {
  172.       state = CW;
  173.     } else if (command == "anticlockwise") {
  174.       state = ACW;
  175.     } else if (isDigit(command[0])) {
  176.       speedPID= map(command.toInt(), 0, 100, 0, SPEED_CONST);
  177.       speedOpen = map(command.toInt(), 0, 100, 0, 255);
  178.     }
  179.   }
  180.  
  181.    // calculate speed
  182.    long prevFL = cntFL;
  183.    long prevFR = cntFR;
  184.    long prevBL = cntBL;
  185.    long prevBR = cntBR;
  186.    delay(timeD);
  187.    speedFL = abs(cntFL - prevFL) * 10;
  188.    speedFR = abs(cntFR - prevFR) * 10;
  189.    speedBL = abs(cntBL - prevBL) * 10;
  190.    speedBR = abs(cntBR - prevBR) * 10;
  191.    
  192.    
  193.    if (!isOpen){
  194.     // close-loop - with PID
  195.      switch(state){
  196.       case STOP: stop(); break;
  197.       case FWL: diagonalFL(speedPID); break;
  198.       case FWD: forward(speedPID); break;
  199.       case FWR: diagonalFR(speedPID); break;
  200.       case LEFT: left(speedPID); break;
  201.       case CW: clockWise(speedPID); break;
  202.       case ACW: antiClockWise(speedPID); break;
  203.       case RIGHT: right(speedPID); break;
  204.       case BWL: diagonalBL(speedPID); break;
  205.       case BWD: backward(speedPID); break;
  206.       case BWR: diagonalBR(speedPID); break;
  207.      }
  208.    } else {
  209.     // open-loop
  210.     switch(state){
  211.       case STOP: stop(); break;
  212.       case FWL: diagonalFLO(speedOpen); break;
  213.       case FWD: forwardO(speedOpen); break;
  214.       case FWR: diagonalFRO(speedOpen); break;
  215.       case LEFT: leftO(speedOpen); break;
  216.       case CW: clockWiseO(speedOpen); break;
  217.       case ACW: antiClockWiseO(speedOpen); break;
  218.       case RIGHT: rightO(speedOpen); break;
  219.       case BWL: diagonalBLO(speedOpen); break;
  220.       case BWD: backwardO(speedOpen); break;
  221.       case BWR: diagonalBRO(speedOpen); break;
  222.      }
  223.    }
  224. }
  225.  
  226. void encoderFL(){
  227.   if (digitalRead(EFLA)){
  228.     cntFL--;
  229.    } else {
  230.     cntFL++;
  231.    }
  232. }
  233.  
  234. void encoderFR(){
  235.   if (digitalRead(EFRA)){
  236.     cntFR++;
  237.    } else {
  238.     cntFR--;
  239.    }
  240. }
  241.  
  242. void encoderBL(){
  243.   if (digitalRead(EBLA)){
  244.     cntBL--;
  245.    } else {
  246.     cntBL++;
  247.    }
  248. }
  249.  
  250. void encoderBR(){
  251.   if (digitalRead(EBRA)){
  252.     cntBR++;
  253.    } else {
  254.     cntBR--;
  255.    }
  256. }
  257.  
  258. void stop(){
  259.   SetFL = SetFR = SetBL = SetBR = 0;
  260.   analogWrite(MFL1, 0);
  261.   analogWrite(MFL2, 0);
  262.   analogWrite(MFR1, 0);
  263.   analogWrite(MFR2, 0);
  264.   analogWrite(MBL1, 0);
  265.   analogWrite(MBL2, 0);
  266.   analogWrite(MBR1, 0);
  267.   analogWrite(MBR2, 0);
  268. }
  269.  
  270.  
  271. void forward(int sp){
  272.   SetFL = SetFR = SetBL = SetBR = sp;
  273.   flPID.Compute();
  274.   frPID.Compute();
  275.   blPID.Compute();
  276.   brPID.Compute();
  277.   analogWrite(MFL1, ceil(pwmFL));
  278.   analogWrite(MFL2, 0);
  279.   analogWrite(MFR1, ceil(pwmFR));
  280.   analogWrite(MFR2, 0);
  281.   analogWrite(MBL1, ceil(pwmBL));
  282.   analogWrite(MBL2, 0);
  283.   analogWrite(MBR1, ceil(pwmBR));
  284.   analogWrite(MBR2, 0);
  285. }
  286.  
  287. void backward(int sp){
  288.   SetFL = SetFR = SetBL = SetBR = sp;
  289.   flPID.Compute();
  290.   frPID.Compute();
  291.   blPID.Compute();
  292.   brPID.Compute();
  293.   analogWrite(MFL1, 0);
  294.   analogWrite(MFL2, ceil(pwmFL));
  295.   analogWrite(MFR1, 0);
  296.   analogWrite(MFR2, ceil(pwmFR));
  297.   analogWrite(MBL1, 0);
  298.   analogWrite(MBL2, ceil(pwmBL));
  299.   analogWrite(MBR1, 0);
  300.   analogWrite(MBR2, ceil(pwmBR));
  301. }
  302.  
  303.  
  304. void left(int sp){
  305.   SetFL = SetFR = SetBL = SetBR = sp;
  306.   flPID.Compute();
  307.   frPID.Compute();
  308.   blPID.Compute();
  309.   brPID.Compute();
  310.   analogWrite(MFL1, 0);
  311.   analogWrite(MFL2, ceil(pwmFL));
  312.   analogWrite(MFR1, ceil(pwmFR));
  313.   analogWrite(MFR2, 0);
  314.   analogWrite(MBL1, ceil(pwmBL));
  315.   analogWrite(MBL2, 0);
  316.   analogWrite(MBR1, 0);
  317.   analogWrite(MBR2, ceil(pwmBR));
  318. }
  319.  
  320.  
  321. void right(int sp){
  322.   SetFL = SetFR = SetBL = SetBR = sp;
  323.   flPID.Compute();
  324.   frPID.Compute();
  325.   blPID.Compute();
  326.   brPID.Compute();
  327.   analogWrite(MFL1, ceil(pwmFL));
  328.   analogWrite(MFL2, 0);
  329.   analogWrite(MFR1, 0);
  330.   analogWrite(MFR2, ceil(pwmFR));
  331.   analogWrite(MBL1, 0);
  332.   analogWrite(MBL2, ceil(pwmBL));
  333.   analogWrite(MBR1, ceil(pwmBR));
  334.   analogWrite(MBR2, 0);
  335. }
  336.  
  337. void diagonalFR(int sp){
  338.   SetFL = SetBR = sp;
  339.   flPID.Compute();
  340.   brPID.Compute();
  341.   analogWrite(MFL1, ceil(pwmFL));
  342.   analogWrite(MFL2, 0);
  343.   analogWrite(MFR1, 0);
  344.   analogWrite(MFR2, 0);
  345.   analogWrite(MBL1, 0);
  346.   analogWrite(MBL2, 0);
  347.   analogWrite(MBR1, ceil(pwmBR));
  348.   analogWrite(MBR2, 0);
  349. }
  350.  
  351.  
  352. void diagonalFL(int sp){
  353.   SetFR = SetBL = sp;
  354.   frPID.Compute();
  355.   blPID.Compute();
  356.   analogWrite(MFL1, 0);
  357.   analogWrite(MFL2, 0);
  358.   analogWrite(MFR1, ceil(pwmFR));
  359.   analogWrite(MFR2, 0);
  360.   analogWrite(MBL1, ceil(pwmBL));
  361.   analogWrite(MBL2, 0);
  362.   analogWrite(MBR1, 0);
  363.   analogWrite(MBR2, 0);
  364. }
  365.  
  366.  
  367. void diagonalBL(int sp){
  368.   SetFL = SetBR = sp;
  369.   flPID.Compute();
  370.   brPID.Compute();
  371.   analogWrite(MFL1, 0);
  372.   analogWrite(MFL2, ceil(pwmFL));
  373.   analogWrite(MFR1, 0);
  374.   analogWrite(MFR2, 0);
  375.   analogWrite(MBL1, 0);
  376.   analogWrite(MBL2, 0);
  377.   analogWrite(MBR1, 0);
  378.   analogWrite(MBR2, ceil(pwmBR));
  379. }
  380.  
  381. void diagonalBR(int sp){
  382.   SetFR = SetBL = sp;
  383.   frPID.Compute();
  384.   blPID.Compute();
  385.   analogWrite(MFL1, 0);
  386.   analogWrite(MFL2, 0);
  387.   analogWrite(MFR1, 0);
  388.   analogWrite(MFR2, ceil(pwmFR));
  389.   analogWrite(MBL1, 0);
  390.   analogWrite(MBL2, ceil(pwmBL));
  391.   analogWrite(MBR1, 0);
  392.   analogWrite(MBR2, 0);
  393. }
  394.  
  395.  
  396. void clockWise(int sp){
  397.   SetFL = SetFR = SetBL = SetBR= sp;
  398.   flPID.Compute();
  399.   frPID.Compute();
  400.   blPID.Compute();
  401.   brPID.Compute();
  402.   analogWrite(MFL1, ceil(pwmFL));
  403.   analogWrite(MFL2, 0);
  404.   analogWrite(MFR1, 0);
  405.   analogWrite(MFR2, ceil(pwmFR));
  406.   analogWrite(MBL1, ceil(pwmBL));
  407.   analogWrite(MBL2, 0);
  408.   analogWrite(MBR1, 0);
  409.   analogWrite(MBR2, ceil(pwmBR));
  410. }
  411.  
  412. void antiClockWise(int sp){
  413.   SetFL = SetFR = SetBL = SetBR = sp;
  414.   flPID.Compute();
  415.   frPID.Compute();
  416.   blPID.Compute();
  417.   brPID.Compute();
  418.   analogWrite(MFL1, 0);
  419.   analogWrite(MFL2, ceil(pwmFL));
  420.   analogWrite(MFR1, ceil(pwmFR));
  421.   analogWrite(MFR2, 0);
  422.   analogWrite(MBL1, 0);
  423.   analogWrite(MBL2, ceil(pwmBL));
  424.   analogWrite(MBR1, ceil(pwmBR));
  425.   analogWrite(MBR2, 0);
  426. }
  427.  
  428. void forwardO(int sp){
  429.   analogWrite(MFL1, sp);
  430.   analogWrite(MFL2, 0);
  431.   analogWrite(MFR1, sp);
  432.   analogWrite(MFR2, 0);
  433.   analogWrite(MBL1, sp);
  434.   analogWrite(MBL2, 0);
  435.   analogWrite(MBR1, sp);
  436.   analogWrite(MBR2, 0);
  437. }
  438.  
  439.  
  440. void backwardO(int sp){
  441.   analogWrite(MFL1, 0);
  442.   analogWrite(MFL2, sp);
  443.   analogWrite(MFR1, 0);
  444.   analogWrite(MFR2, sp);
  445.   analogWrite(MBL1, 0);
  446.   analogWrite(MBL2, sp);
  447.   analogWrite(MBR1, 0);
  448.   analogWrite(MBR2, sp);
  449. }
  450.  
  451.  
  452. void leftO(int sp){
  453.   analogWrite(MFL1, 0);
  454.   analogWrite(MFL2, sp);
  455.   analogWrite(MFR1, sp);
  456.   analogWrite(MFR2, 0);
  457.   analogWrite(MBL1, sp);
  458.   analogWrite(MBL2, 0);
  459.   analogWrite(MBR1, 0);
  460.   analogWrite(MBR2, sp);
  461. }
  462.  
  463.  
  464. void rightO(int sp){
  465.   analogWrite(MFL1, sp);
  466.   analogWrite(MFL2, 0);
  467.   analogWrite(MFR1, 0);
  468.   analogWrite(MFR2, sp);
  469.   analogWrite(MBL1, 0);
  470.   analogWrite(MBL2, sp);
  471.   analogWrite(MBR1, sp);
  472.   analogWrite(MBR2, 0);
  473. }
  474.  
  475. void diagonalFRO(int sp){
  476.   analogWrite(MFL1, sp);
  477.   analogWrite(MFL2, 0);
  478.   analogWrite(MFR1, 0);
  479.   analogWrite(MFR2, 0);
  480.   analogWrite(MBL1, 0);
  481.   analogWrite(MBL2, 0);
  482.   analogWrite(MBR1, sp);
  483.   analogWrite(MBR2, 0);
  484. }
  485.  
  486.  
  487. void diagonalFLO(int sp){
  488.   analogWrite(MFL1, 0);
  489.   analogWrite(MFL2, 0);
  490.   analogWrite(MFR1, sp);
  491.   analogWrite(MFR2, 0);
  492.   analogWrite(MBL1, sp);
  493.   analogWrite(MBL2, 0);
  494.   analogWrite(MBR1, 0);
  495.   analogWrite(MBR2, 0);
  496. }
  497.  
  498.  
  499. void diagonalBLO(int sp){
  500.   analogWrite(MFL1, 0);
  501.   analogWrite(MFL2, sp);
  502.   analogWrite(MFR1, 0);
  503.   analogWrite(MFR2, 0);
  504.   analogWrite(MBL1, 0);
  505.   analogWrite(MBL2, 0);
  506.   analogWrite(MBR1, 0);
  507.   analogWrite(MBR2, sp);
  508. }
  509.  
  510. void diagonalBRO(int sp){
  511.   analogWrite(MFL1, 0);
  512.   analogWrite(MFL2, 0);
  513.   analogWrite(MFR1, 0);
  514.   analogWrite(MFR2, sp);
  515.   analogWrite(MBL1, 0);
  516.   analogWrite(MBL2, sp);
  517.   analogWrite(MBR1, 0);
  518.   analogWrite(MBR2, 0);
  519. }
  520.  
  521.  
  522. void clockWiseO(int sp){
  523.   analogWrite(MFL1, sp);
  524.   analogWrite(MFL2, 0);
  525.   analogWrite(MFR1, 0);
  526.   analogWrite(MFR2, sp);
  527.   analogWrite(MBL1, sp);
  528.   analogWrite(MBL2, 0);
  529.   analogWrite(MBR1, 0);
  530.   analogWrite(MBR2, sp);
  531. }
  532.  
  533. void antiClockWiseO(int sp){
  534.   analogWrite(MFL1, 0);
  535.   analogWrite(MFL2, sp);
  536.   analogWrite(MFR1, sp);
  537.   analogWrite(MFR2, 0);
  538.   analogWrite(MBL1, 0);
  539.   analogWrite(MBL2, sp);
  540.   analogWrite(MBR1, sp);
  541.   analogWrite(MBR2, 0);
  542. }
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!