Botboarduino-PS2-Rover/Control.ino at master · KeplerElectronics/Botboarduino-PS2-Rover · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
//*********************************************************************************
// www.Lynxmotion.com
// Basic code for 2WD rover using continuous rotation servos, controlled via PS2
// Right now, the library does NOT support hot pluggable controllers, meaning
// you must always either restart your Arduino after you connect the controller,
// or call config_gamepad(pins) again after connecting the controller.
//*********************************************************************************

#include <PS2X_lib.h>  //for v1.6
#include <Servo.h>
#include <Wire.h>
#include <Adafruit_MotorShield.h>
#include "utility/Adafruit_MS_PWMServoDriver.h"

//create Adafruit Motorshield Object
Adafruit_MotorShield AFMS = Adafruit_MotorShield();

//Create DC Motor object #1 is which port is used.
Adafruit_DCMotor *LeftMotor = AFMS.getMotor(1);
Adafruit_DCMotor *RightMotor = AFMS.getMotor(2);
// create PS2 Controller Class
PS2X ps2x;
int error = 0;
byte type = 0;
byte vibrate = 0;

#define Deadzone 10 //PS2 analog joystick Deadzone

int speed1 =128, speed2 = 128;

byte M1L = 5;// PWM
byte M1R = 3;// PWM
byte M2L = 6;// PWM
byte M2R = 9;// PWM

void setup(){
  delay(50);
  Serial.begin(57600);
  AFMS.begin();
  LeftMotor->setSpeed(200);
  RightMotor->setSpeed(200);


 pinMode(7, INPUT);

 error = ps2x.config_gamepad(9,7,6,8, true, true);

  //**************PAY ATTENTION*************
  pinMode(M1L, OUTPUT);                                // Establishes LEDPin as an output so the LED can be seen
  pinMode(M1R, OUTPUT);
  pinMode(M2L, OUTPUT);                                // Establishes LEDPin as an output so the LED can be seen
  pinMode(M2R, OUTPUT);
  error = ps2x.config_gamepad(9,7,6,8, false, true);   //setup pins and settings:  GamePad(clock, command, attention, data, Pressures?, Rumble?) check for error

  if(error == 0)
    Serial.println("Controller found! You may now send commands");

  else if(error == 1)
    Serial.println("No controller found, check wiring, see readme.txt to enable debug. visit www.billporter.info for troubleshooting tips");

  else if(error == 2)
    Serial.println("Controller found but not accepting commands. see readme.txt to enable debug. Visit www.billporter.info for troubleshooting tips");

  else if(error == 3)
    Serial.println("Controller refusing to enter Pressures mode, may not support it. ");

type = ps2x.readType();
  switch(type)
  {
  case 0:
    Serial.println("Unknown Controller type");
    break;
  case 1:
    Serial.println("DualShock Controller Found");
    break;
  }
}

void loop()
{
  if(error == 1) //skip loop if no controller found
    return;

  else { //DualShock Controller

    ps2x.read_gamepad(false, vibrate);          //read controller and set large motor to spin at 'vibrate' speed
//RIGHT
    if(ps2x.Analog(PSS_LY) >= 170 && ps2x.Analog(PSS_LY) <= 255)
    {
      speed2 = map(ps2x.Analog(PSS_RY),140 , 255, 0 , 255);
      RightMotor->run(FORWARD);
    }
    else if(ps2x.Analog(PSS_LY) >= 0 && ps2x.Analog(PSS_LY) <= 85)
    {
      speed2 = map(ps2x.Analog(PSS_RY),0 ,120 , 255 , 0);
      RightMotor->run(BACKWARD);
    }
    else
    {
      RightMotor->run(RELEASE);
    }

 //LEFT
    if(ps2x.Analog(PSS_RY) >= 170 && ps2x.Analog(PSS_RY) <= 255)
    {
      speed2 = map(ps2x.Analog(PSS_RY),140 , 255, 0 , 255);
      LeftMotor->run(BACKWARD);
    }

    else if(ps2x.Analog(PSS_RY) >= 0 && ps2x.Analog(PSS_RY) <= 85)
    {
      speed2 = map(ps2x.Analog(PSS_RY),0 ,120 , 255 , 0);
      LeftMotor->run(FORWARD);
    }
    else
   {
      LeftMotor->run(RELEASE);
    }
  }
}