roboforum.ru

#include <PS2X_lib.h>  //for v1.6 библиотека джойстика
#include <Wire.h>


#include <Servo.h>

Servo RXservo; //подключаем сервы
Servo RYservo;

#define ledPin 13
#define startbyte 0x0F
#define I2Caddress 0x07
int sv[6]={1000,1400,1500,1500,0,0};      //заданны позиции серво приводов, на тирексе их 6 штук, из них первые две отключены, им соответствуют значения = 0, на плате это 5 и 6 пины. далее 1000 = 7 пин, 1400 = 8 пин...           // servo positions: 0 = Not Used
int sd[6]={5,10,-5,-15,20,-20};                      // servo sweep speed/direction
int lmspeed,rmspeed;                                 // left and right motor speed from -255 to +255 (negative value = reverse)
int ldir=5;                                          // how much to change left  motor speed each loop (use for motor testing)
int rdir=5;                                          // how much to change right motor speed each loop (use for motor testing)
byte lmbrake,rmbrake;                                // left and right motor brake (non zero value = brake)
byte devibrate=50;                                   // time delay after impact to prevent false re-triggering due to chassis vibration
int sensitivity=50;                                  // threshold of acceleration / deceleration required to register as an impact
int lowbat=550;                                      // adjust to suit your battery: 550 = 5.50V
byte i2caddr=7;                                      // default I2C address of T'REX is 7. If this is changed, the T'REX will automatically store new address in EEPROM
byte i2cfreq=0;                                      // I2C clock frequency. Default is 0=100kHz. Set to 1 for 400kHz




/******************************************************************
* set pins connected to PS2 controller:
*   - 1e column: original
*   - 2e colmun: Stef?
* replace pin numbers by the ones you use
//подключение пинов блютус модуля геймпада:
******************************************************************/
#define PS2_DAT        12  //14   
#define PS2_CMD        11  //15
#define PS2_SEL        10  //16
#define PS2_CLK        9  //17

/******************************************************************
* select modes of PS2 controller:
*   - pressures = analog reading of push-butttons
*   - rumble    = motor rumbling
* uncomment 1 of the lines for each mode selection
******************************************************************/
//#define pressures   true
#define pressures   false
//#define rumble      true
#define rumble      false

PS2X ps2x; // create PS2 Controller Class
int PS2 = 0;
//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.

int error = 0;
byte type = 0;
byte vibrate = 0;

void setup(){

  //пины серв
  RXservo.attach(3);
  RYservo.attach(4);
 
  Serial.begin(57600);
  pinMode(ledPin, OUTPUT);
  Wire.begin();                // no address - join the bus as master
   
  delay(300);  //added delay to give wireless ps2 module some time to startup, before configuring it
   
  //CHANGES for v1.6 HERE!!! **************PAY ATTENTION*************
 
  //setup pins and settings: GamePad(clock, command, attention, data, Pressures?, Rumble?) check for error
  error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, pressures, rumble);
 
  if(error == 0){
    Serial.print("Found Controller, configured successful ");
    Serial.print("pressures = ");
   if (pressures)
     Serial.println("true ");
   else
     Serial.println("false");
   Serial.print("rumble = ");
   if (rumble)
     Serial.println("true)");
   else
     Serial.println("false");
  } 
  else
    Serial.println("Большой еггог :)");
   
 
//  Serial.print(ps2x.Analog(1), HEX);
 
  type = ps2x.readType();
  switch(type) {
    case 0:
      Serial.print("Unknown Controller type found ");
      break;
    case 1:
      Serial.print("DualShock Controller found ");
      break;
    case 2:
      Serial.print("GuitarHero Controller found ");
      break;
   case 3:
      Serial.print("Wireless Sony DualShock Controller found ");
      break;
   }
}

void loop() {
  /* You must Read Gamepad to get new values and set vibration values
     ps2x.read_gamepad(small motor on/off, larger motor strenght from 0-255)
     if you don't enable the rumble, use ps2x.read_gamepad(); with no values
     You should call this at least once a second
   */ 
  if(error == 1) //skip loop if no controller found
    return;
 
  if(type == 2){ //Guitar Hero Controller
    ps2x.read_gamepad();          //read controller
   
    if(ps2x.ButtonPressed(GREEN_FRET))
      Serial.println("Green Fret Pressed");
    if(ps2x.ButtonPressed(RED_FRET))
      Serial.println("Red Fret Pressed");
    if(ps2x.ButtonPressed(YELLOW_FRET))
      Serial.println("Yellow Fret Pressed");
    if(ps2x.ButtonPressed(BLUE_FRET))
      Serial.println("Blue Fret Pressed");
    if(ps2x.ButtonPressed(ORANGE_FRET))
      Serial.println("Orange Fret Pressed");

    if(ps2x.ButtonPressed(STAR_POWER))
      Serial.println("Star Power Command");
   
    if(ps2x.Button(UP_STRUM))          //will be TRUE as long as button is pressed
      Serial.println("Up Strum");
    if(ps2x.Button(DOWN_STRUM))
      Serial.println("DOWN Strum");

    if(ps2x.Button(PSB_START))         //will be TRUE as long as button is pressed
      Serial.println("Start is being held");
    if(ps2x.Button(PSB_SELECT))
      Serial.println("Select is being held");
   
    if(ps2x.Button(ORANGE_FRET)) {     // print stick value IF TRUE
      Serial.print("Wammy Bar Position:");
      Serial.println(ps2x.Analog(WHAMMY_BAR), DEC);
    }
  }
  else { //DualShock Controller
    ps2x.read_gamepad(false, vibrate); //read controller and set large motor to spin at 'vibrate' speed
   
    if(ps2x.Button(PSB_START))         //will be TRUE as long as button is pressed
      Serial.println("Start is being held");
    if(ps2x.Button(PSB_SELECT))
      Serial.println("Select is being held");     

    if(ps2x.Button(PSB_PAD_UP)) {      //will be TRUE as long as button is pressed
      Serial.print("Up held this hard: ");
      Serial.println(ps2x.Analog(PSAB_PAD_UP), DEC);
    }
    if(ps2x.Button(PSB_PAD_RIGHT)){
      Serial.print("Right held this hard: ");
      Serial.println(ps2x.Analog(PSAB_PAD_RIGHT), DEC);
    }
    if(ps2x.Button(PSB_PAD_LEFT)){
      Serial.print("LEFT held this hard: ");
      Serial.println(ps2x.Analog(PSAB_PAD_LEFT), DEC);
    }
    if(ps2x.Button(PSB_PAD_DOWN)){
      Serial.print("DOWN held this hard: ");
      Serial.println(ps2x.Analog(PSAB_PAD_DOWN), DEC);
    }   

    vibrate = ps2x.Analog(PSAB_CROSS);  //this will set the large motor vibrate speed based on how hard you press the blue (X) button
    if (ps2x.NewButtonState()) {        //will be TRUE if any button changes state (on to off, or off to on)
      if(ps2x.Button(PSB_L3))
        Serial.println("L3 pressed");
      if(ps2x.Button(PSB_R3))
        Serial.println("R3 pressed");
      if(ps2x.Button(PSB_L2))
        Serial.println("L2 pressed");
      if(ps2x.Button(PSB_R2))
        Serial.println("R2 pressed");
      if(ps2x.Button(PSB_TRIANGLE))
        Serial.println("Triangle pressed");       
    }

    if(ps2x.ButtonPressed(PSB_CIRCLE))               //will be TRUE if button was JUST pressed
      Serial.println("Circle just pressed");
    if(ps2x.NewButtonState(PSB_CROSS))               //will be TRUE if button was JUST pressed OR released
      Serial.println("X just changed");
    if(ps2x.ButtonReleased(PSB_SQUARE))              //will be TRUE if button was JUST released
      Serial.println("Square just released");     

    Serial.print("Stick:");

//управление моторам через левый стик геймпада
   lmspeed = -(ps2x.Analog(PSS_LY)-128); //скопировать данные джойстика в регистры правого и левого двигателя
    int tmp = -(ps2x.Analog(PSS_LX)-128); //
    rmspeed = lmspeed;
    lmspeed -= tmp; lmspeed *=2;
    rmspeed += tmp; rmspeed *=2;
    if (lmspeed>255) lmspeed =255;        //ограничить макс. значения моторов
    if (rmspeed>255) rmspeed =255;        //
    if (lmspeed<-255) lmspeed =-255;      //
    if (rmspeed<-255) rmspeed =-255;      //

    lmbrake =0;             //выкл. стоп
    rmbrake =0;             //
   
//управление сервами через правый стик геймпада, реализовано 2мя разными методами, результат примерно одинаковый:   
   
sv[0] = (map(ps2x.Analog(PSS_RY), 0, 255, 1000, 2000)); ;
sv[1] = 1500 -(ps2x.Analog(PSS_RX)-128)*2;
//
   
    Serial.print(lmspeed, DEC); //Left stick, Y axis. Other options: LX, RY, RX 
    Serial.print(",");
    Serial.print(rmspeed, DEC);
    Serial.print(",");
   
    RXservo.write(map(ps2x.Analog(PSS_RX), 255, 0, 0, 180)); ;
delay(50);
  ps2x.read_gamepad(false, 0);
 
  RYservo.write(map(ps2x.Analog(PSS_RY), 0, 255, 0, 90)); ;
delay(50);
  ps2x.read_gamepad(false, 0);

//    Serial.print(ps2x.Analog(PSS_RY), DEC);
  //   Serial.print(",");
   //  Serial.print(ps2x.Analog(PSS_RX), DEC);
   //  Serial.println("\t");
  }
  digitalWrite(ledPin, HIGH);

MasterSend(startbyte,2,lmspeed,lmbrake,rmspeed,rmbrake,sv[0],sv[1],sv[2],sv[3],sv[4],sv[5],devibrate,sensitivity,lowbat,i2caddr,i2cfreq);
  delay(20);
  MasterReceive();    // receive data packet from T'REX controller
}


void MasterSend(byte sbyte, byte pfreq, int lspeed, byte lbrake, int rspeed, byte rbrake, int sv0, int sv1, int sv2, int sv3, int sv4, int sv5, byte dev,int sens,int lowbat, byte i2caddr,byte i2cfreq)
{
  Wire.beginTransmission(I2Caddress); // transmit data to 7
  Wire.write(startbyte);              // start byte
  Wire.write(pfreq);                  // pwm frequency
 
  Wire.write(highByte(lspeed));       // MSB left  motor speed
  Wire.write( lowByte(lspeed));       // LSB left  motor speed
  Wire.write(lbrake);                 // left  motor brake
 
  Wire.write(highByte(rspeed));       // MSB right motor speed
  Wire.write( lowByte(rspeed));       // LSB right motor speed
  Wire.write(rbrake);                 // right motor brake
 
  Wire.write(highByte(sv0));          // MSB servo 0
  Wire.write( lowByte(sv0));          // LSB servo 0
 
  Wire.write(highByte(sv1));          // MSB servo 1
  Wire.write( lowByte(sv1));          // LSB servo 1
 
  Wire.write(highByte(sv2));          // MSB servo 2
  Wire.write( lowByte(sv2));          // LSB servo 2
 
  Wire.write(highByte(sv3));          // MSB servo 3
  Wire.write( lowByte(sv3));          // LSB servo 3
 
  Wire.write(highByte(sv4));          // MSB servo 4
  Wire.write( lowByte(sv4));          // LSB servo 4
 
  Wire.write(highByte(sv5));          // MSB servo 5
  Wire.write( lowByte(sv5));          // LSB servo 5
 
  Wire.write(dev);                    // devibrate
  Wire.write(highByte(sens));         // MSB impact sensitivity
  Wire.write( lowByte(sens));         // LSB impact sensitivity
 
  Wire.write(highByte(lowbat));       // MSB low battery voltage  550 to 30000 = 5.5V to 30V
  Wire.write( lowByte(lowbat));       // LSB low battery voltage
 
  Wire.write(i2caddr);                // I2C slave address for T'REX controller
  Wire.write(i2cfreq);                // I2C clock frequency:   0=100kHz   1=400kHz
  Wire.endTransmission();             // stop transmitting
 
  //Serial.println("Master Command Data Packet Sent");
 
 
  //-------------------------------- Make sure Master and Slave I2C clock the same ------------------------------------------------
 
  if(i2cfreq==0)                                                               // thanks to Nick Gammon: http://gammon.com.au/i2c
  {
    TWBR=72;                                                                   // default I²C clock is 100kHz
  }
  else
  {
    TWBR=12;                                                                   // changes the I²C clock to 400kHz
  }
}


void MasterReceive()
{//================================================================= Error Checking ==========================================================
  byte d;
  int i=0;
  Wire.requestFrom(I2Caddress,24);                                // request 24 bytes from device 007
 
  while(Wire.available()<24)                                      // wait for entire data packet to be received
  {
    if(i==0) Serial.print("Waiting for slave to send data.");     // Only print message once (i==0)
    if(i>0) Serial.print(".");                                    // print a dot for every loop where buffer<24 bytes
    i++;                                                          // increment i so that message only prints once.
    if(i>79)
    {
      Serial.println("");
      i=1;
    }
  }
  d=Wire.read();                                                  // read start byte from buffer
  if(d!=startbyte)                                                // if start byte not equal to 0x0F                                                   
  {
    Serial.print(d,DEC);
    while(Wire.available()>0)                                     // empty buffer of bad data
    {
      d=Wire.read();
    }
    Serial.println("  Wrong Start Byte");                         // error message
    return;                                                       // quit
  }
 
  //================================================================ Read Data ==============================================================

  //Serial.print(". Trex Err:");                           // slave error report
  Serial.print(Wire.read(),DEC);
 
  i=Wire.read()*256+Wire.read();                                  // T'REX battery voltage
  Serial.print(" Akk:");
  Serial.print(int(i/10));Serial.print(".");                     
  Serial.print(i-(int(i/10)*10));Serial.print("V");
 
  i=Wire.read()*256+Wire.read();
  Serial.print("\tL Curr:");
  Serial.print(i);Serial.print("mA");                           // T'REX left  motor current in mA
 
  i=Wire.read()*256+Wire.read();
  Serial.print(" Enc:");
  Serial.print(i);                                              // T'REX left  motor encoder count
 
  i=Wire.read()*256+Wire.read();
  Serial.print("\tR Curr:");
  Serial.print(i);Serial.print("mA");                           // T'REX right motor current in mA
 
  i=Wire.read()*256+Wire.read();
  Serial.print(" Enc:");
  Serial.print(i);                                              // T'REX right motor encoder count
 
  i=Wire.read()*256+Wire.read();
  Serial.print(" X-axis:");
  Serial.print(i);                                              // T'REX X-axis
 
  i=Wire.read()*256+Wire.read();
  Serial.print(" Y:");
  Serial.print(i);                                              // T'REX Y-axis
 
  i=Wire.read()*256+Wire.read();
  Serial.print(" Z:");
  Serial.print(i);                                              // T'REX Z-axis
 
  i=Wire.read()*256+Wire.read();
  Serial.print(" X-delta:");
  Serial.print(i);                                              // T'REX X-delta
 
  i=Wire.read()*256+Wire.read();
  Serial.print(" Y:");
  Serial.print(i);                                              // T'REX Y-delta
 
  i=Wire.read()*256+Wire.read();
  Serial.print(" Z:");
  Serial.println(i);                                              // T'REX Z-delta
}