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
}
боюсь погибнут они от пиковых токов. купите липольку для моделей на большой ток разряда. от 25С и выше.slom писал(а):Поскольку у меня сейчас питание от ли-ион батарей из ноутбука.
robovan писал(а):можете сделать дрон для страйкбола. Думаю людям понравится:)
Petr Kiselev писал(а):slom, если еще не решил проблему с нехваткой пинов, то может пригодится из моего опыта. Я просто использую две Ардуины, программируя их как мне надо по отдельности, а в работу их запускаю вместе, подавая на каждую питание одновременно.
Можно, конечно, применить множитель пинов. Есть такой шилд.
Сейчас этот форум просматривают: нет зарегистрированных пользователей и гости: 18