SHARE
TWEET

Untitled

a guest Jul 29th, 2016 20 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include <PS2X_lib.h>  
  2. #include <Wire.h>
  3. #include <Adafruit_MotorShield.h>
  4. #include "utility/Adafruit_MS_PWMServoDriver.h"
  5. #include <Servo.h>
  6.  
  7. #define PS2_DAT        3    
  8. #define PS2_CMD        5  
  9. #define PS2_SEL        4  
  10. #define PS2_CLK        6  
  11. #define pressures   false
  12. #define rumble      false
  13.  
  14. Adafruit_MotorShield AFMS = Adafruit_MotorShield();
  15. Adafruit_DCMotor *flywheelOne = AFMS.getMotor(1);
  16. Adafruit_DCMotor *flywheelTwo = AFMS.getMotor(2);
  17. Adafruit_DCMotor *portSideMotor = AFMS.getMotor(3);
  18. Adafruit_DCMotor *starboardSideMotor = AFMS.getMotor(4);
  19. Servo autoloader;
  20.  
  21. PS2X ps2x;
  22. int error = 0;
  23. byte type = 0;
  24. byte vibrate = 0;
  25.  
  26. void setup(){
  27.  
  28.   Serial.begin(57600);
  29.  
  30.   delay(300);  //added delay to give wireless ps2 module some time to startup, before configuring it
  31.   pinMode(3, INPUT_PULLUP);
  32.   autoloader.attach(9);
  33.   Serial.println("tank motors");
  34.   AFMS.begin();
  35.   error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, pressures, rumble);
  36.  
  37.   if(error == 0){
  38.     Serial.print("Found Controller, configured successful ");
  39.     Serial.print("pressures = ");
  40.     if (pressures)
  41.       Serial.println("true ");
  42.     else
  43.       Serial.println("false");
  44.     Serial.print("rumble = ");
  45.     if (rumble)
  46.       Serial.println("true)");
  47.     else
  48.       Serial.println("false");
  49.     Serial.println("Try out all the buttons, X will vibrate the controller, faster as you press harder;");
  50.     Serial.println("holding L1 or R1 will print out the analog stick values.");
  51.     Serial.println("Note: Go to www.billporter.info for updates and to report bugs.");
  52.   }  
  53.   else if(error == 1)
  54.     Serial.println("No controller found, check wiring, see readme.txt to enable debug. visit www.billporter.info for troubleshooting tips");
  55.    
  56.   else if(error == 2)
  57.     Serial.println("Controller found but not accepting commands. see readme.txt to enable debug. Visit www.billporter.info for troubleshooting tips");
  58.  
  59.   else if(error == 3)
  60.     Serial.println("Controller refusing to enter Pressures mode, may not support it. ");
  61.  
  62. //  Serial.print(ps2x.Analog(1), HEX);
  63.  
  64.   type = ps2x.readType();
  65.   switch(type) {
  66.     case 0:
  67.       Serial.print("Unknown Controller type found ");
  68.       break;
  69.     case 1:
  70.       Serial.print("DualShock Controller found ");
  71.       break;
  72.     case 2:
  73.       Serial.print("GuitarHero Controller found ");
  74.       break;
  75.     case 3:
  76.       Serial.print("Wireless Sony DualShock Controller found ");
  77.       break;
  78.    }
  79. }
  80.  
  81. void loop() {
  82.  
  83.   if(error == 1) //skip loop if no controller found
  84.     return;
  85.  
  86.   if(type == 2) //Guitar Hero Controller
  87.     return;
  88.    
  89.   else { //DualShock Controller
  90.     ps2x.read_gamepad(false, vibrate); //read controller and set large motor to spin at 'vibrate' speed
  91.    
  92.     if(ps2x.Button(PSB_START))         //will be TRUE as long as button is pressed
  93.       Serial.println("Start is being held");
  94.     if(ps2x.Button(PSB_SELECT))
  95.       Serial.println("Select is being held");      
  96.  
  97.     if(ps2x.ButtonPressed(PSB_PAD_UP)) {      
  98.       Serial.println("TRIGGERED");
  99.       Serial.println(ps2x.Analog(PSAB_PAD_UP), DEC);
  100.       portSideMotor->setSpeed(220);
  101.       starboardSideMotor->setSpeed(200);
  102.       portSideMotor->run(FORWARD);
  103.       starboardSideMotor->run(FORWARD);
  104.     }
  105.     if(ps2x.ButtonReleased(PSB_PAD_UP)){
  106.       portSideMotor->setSpeed(0);
  107.       starboardSideMotor->setSpeed(0);
  108.     }
  109.     if(ps2x.ButtonPressed(PSB_PAD_RIGHT)) {      
  110.       Serial.println("TRIGGERED");
  111.       Serial.println(ps2x.Analog(PSAB_PAD_UP), DEC);
  112.       portSideMotor->setSpeed(105);
  113.       starboardSideMotor->setSpeed(100);
  114.       portSideMotor->run(FORWARD);
  115.       starboardSideMotor->run(BACKWARD);
  116.     }
  117.     if(ps2x.ButtonReleased(PSB_PAD_RIGHT)){
  118.       portSideMotor->setSpeed(0);
  119.       starboardSideMotor->setSpeed(0);
  120.     }
  121.     if(ps2x.ButtonPressed(PSB_PAD_LEFT)) {      
  122.       Serial.println("TRIGGERED");
  123.       Serial.println(ps2x.Analog(PSAB_PAD_UP), DEC);
  124.       portSideMotor->setSpeed(105);
  125.       starboardSideMotor->setSpeed
  126.       (100);
  127.       portSideMotor->run(BACKWARD);
  128.       starboardSideMotor->run(FORWARD);
  129.     }
  130.     if(ps2x.ButtonReleased(PSB_PAD_LEFT)){
  131.       portSideMotor->setSpeed(0);
  132.       starboardSideMotor->setSpeed(0);
  133.     }
  134.     if(ps2x.ButtonPressed(PSB_PAD_DOWN)){
  135.       Serial.println("I DON'T FEEL SAFE");
  136.       Serial.println(ps2x.Analog(PSAB_PAD_DOWN), DEC);
  137.       portSideMotor->setSpeed(220);
  138.       starboardSideMotor->setSpeed(200);
  139.       portSideMotor->run(BACKWARD);
  140.       starboardSideMotor->run(BACKWARD);
  141.     }  
  142.     if(ps2x.ButtonReleased(PSB_PAD_DOWN)){
  143.       portSideMotor->setSpeed(0);
  144.       starboardSideMotor->setSpeed(0);
  145.     }
  146.     vibrate = ps2x.Analog(PSAB_CROSS);  //this will set the large motor vibrate speed based on how hard you press the blue (X) button
  147.     if (ps2x.NewButtonState()) {        //will be TRUE if any button changes state (on to off, or off to on)
  148.       if(ps2x.Button(PSB_L3))
  149.         Serial.println("L3 pressed");
  150.       if(ps2x.Button(PSB_R3))
  151.         Serial.println("R3 pressed");
  152.       if(ps2x.Button(PSB_L2))
  153.         Serial.println("L2 pressed");
  154.       if(ps2x.Button(PSB_R2))
  155.         Serial.println("R2 pressed");
  156.       if(ps2x.Button(PSB_TRIANGLE))
  157.         Serial.println("Triangle pressed");        
  158.     }
  159.     if(ps2x.ButtonPressed(PSB_L1)){
  160.       Serial.println("half speed rev");
  161.       flywheelOne->setSpeed(120);
  162.       flywheelTwo->setSpeed(120);
  163.       flywheelOne->run(BACKWARD);
  164.       flywheelTwo->run(BACKWARD);
  165.     }
  166.     if(ps2x.ButtonReleased(PSB_L1)){
  167.       flywheelOne->setSpeed(0);
  168.       flywheelTwo->setSpeed(0);
  169.     }
  170.  
  171.     if(ps2x.ButtonPressed(PSB_TRIANGLE)){
  172.       Serial.println("test lel");
  173.       flywheelOne->setSpeed(20);
  174.       flywheelTwo->setSpeed(20);
  175.       flywheelOne->run(BACKWARD);
  176.       flywheelTwo->run(BACKWARD);
  177.     }
  178.     if(ps2x.ButtonReleased(PSB_TRIANGLE)){
  179.       flywheelOne->setSpeed(0);
  180.       flywheelTwo->setSpeed(0);
  181.     }
  182.    
  183.     if(ps2x.ButtonPressed(PSB_CROSS)){
  184.       Serial.println("FULL SPEED REV");
  185.       flywheelOne->setSpeed(255);
  186.       flywheelTwo->setSpeed(255);
  187.       flywheelOne->run(BACKWARD);
  188.       flywheelTwo->run(BACKWARD);
  189.     }
  190.     if(ps2x.ButtonReleased(PSB_CROSS)){
  191.       flywheelOne->setSpeed(0);
  192.       flywheelTwo->setSpeed(0);
  193.     }
  194.     if(ps2x.ButtonPressed(PSB_L2)){
  195.       Serial.println("FULL SPEED REV with cool spindown");
  196.       flywheelOne->setSpeed(255);
  197.       flywheelTwo->setSpeed(255);
  198.       flywheelOne->run(BACKWARD);
  199.       flywheelTwo->run(BACKWARD);
  200.     }
  201.     if(ps2x.ButtonReleased(PSB_L2)){
  202.       flywheelOne->run(RELEASE);
  203.       flywheelTwo->run(RELEASE);
  204.     }
  205.     if(ps2x.ButtonPressed(PSB_CIRCLE)){
  206.       Serial.println("toggle maxspeed");
  207.       flywheelOne->setSpeed(255);
  208.       flywheelTwo->setSpeed(255);
  209.       flywheelOne->run(BACKWARD);
  210.       flywheelTwo->run(BACKWARD);
  211.     }
  212.     if(ps2x.ButtonPressed(PSB_R1)){
  213.       autoloader.write(10);
  214.       delay(250);
  215.       autoloader.write(140);
  216.       Serial.println("actuate servo");
  217.     }
  218.  
  219.     if(ps2x.Button(PSB_L1) || ps2x.Button(PSB_R1)) { //print stick values if either is TRUE
  220.       Serial.print("Stick Values:");
  221.       Serial.print(ps2x.Analog(PSS_LY), DEC); //Left stick, Y axis. Other options: LX, RY, RX  
  222.       Serial.print(",");
  223.       Serial.print(ps2x.Analog(PSS_LX), DEC);
  224.       Serial.print(",");
  225.       Serial.print(ps2x.Analog(PSS_RY), DEC);
  226.       Serial.print(",");
  227.       Serial.println(ps2x.Analog(PSS_RX), DEC);
  228.     }    
  229.   }
  230.   delay(50);  
  231. }
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
 
Top