Hi All,
This is my first time writing a post so i’ll try to put all the information needed! I am using a tri-track from lynxmotion with a botboarduino using their controllers (v1.5) using the ps2x library to control the robot wirelessly. When i do a simple sequence program to send pulses to the robots tracks and servos everything works fine, what happens is what expect and program them to do. My tracks work fine, when i write 1500 us to both servos they stop as it’s their mid point and 2000 they go forward, 1000 they roll backwards. So i can work with that.
Problems arise is when i put those servo commands within the IF loops which are activated by the different buttons press of the controller. I am using the following code :
[code]
#include <PS2X_lib.h> //for v1.6
#include <Servo.h>
/******************************************************************
- set pins connected to PS2 controller:
-
- 1e column: original
-
- 2e colmun: Stef?
- replace pin numbers by the ones you use
******************************************************************/
#define PS2_DAT 6
#define PS2_CMD 7
#define PS2_SEL 8
#define PS2_CLK 9
/******************************************************************
- 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
//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 Pulsewidth_lefttrack=1300;
int Pulsewidth_righttrack=1700;
int Pulsewidth_Base=1500;
int Pulsewidth_Shoulder=1500;
int angleArm=90;
int angleElbow=90;
int angleWrist=90;
int angleGripper=90;
int error = 0;
byte type = 0;
byte vibrate = 0;
// Create the values for the right joystick
int x_Axis = 0;
int y_Axis = 0;
int ly_Axis =0;
int lx_Axis = 0;
Servo Base;
Servo Shoulder;
Servo Elbow;
Servo Arm;
Servo Wrist;
Servo Gripper;
Servo Trackleft;
Servo Trackright;
void setup(){
Serial.begin(57600);
delay(300); //added delay to give wireless ps2 module some time to startup, before configuring it
Trackright.attach(13);
Trackleft.attach(3);
Base.attach(4);
Shoulder.attach(5);
Elbow.attach(10);
Arm.attach(11);
Wrist.attach(12);
Gripper.attach(2);
//CHANGES for v1.6 HERE!!! *PAY ATTENTION
//setup pins and settings: GamePad(clock, command, attention, data, Pressures?, Rumble?) check for error
ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, pressures, rumble);
}
void loop() {
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");
Trackright.writeMicroseconds(1500);
Trackleft.writeMicroseconds(1500);
// delay(0.4);
}
if(ps2x.Button(PSB_SELECT)){
Serial.println(“Select is being held”);
Trackright.writeMicroseconds(1000);
Trackleft.writeMicroseconds(1000);
//delay(0.5);
}
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), BIN);
Trackright.writeMicroseconds(2000);
Trackleft.write(2000);
}
if(ps2x.Button(PSB_PAD_RIGHT)){
Serial.print("Right held this hard: ");
Serial.println(ps2x.Analog(PSAB_PAD_RIGHT), DEC);
Trackright.writeMicroseconds(1400);
Trackleft.writeMicroseconds(2000);
}
if(ps2x.Button(PSB_PAD_LEFT)){
Serial.print("LEFT held this hard: ");
Serial.println(ps2x.Analog(PSAB_PAD_LEFT), DEC);
Trackright.writeMicroseconds(2000);
Trackleft.writeMicroseconds(1400);
delay(0.7);
}
if(ps2x.Button(PSB_PAD_DOWN)){
Serial.print("DOWN held this hard: ");
Serial.println(ps2x.Analog(PSAB_PAD_DOWN), BIN);
Trackright.writeMicroseconds(1000);
Trackleft.writeMicroseconds(1000);
}
x_Axis = ps2x.Analog(PSS_RX);
y_Axis = ps2x.Analog(PSS_RY);
lx_Axis = ps2x.Analog(PSS_LX);
ly_Axis = ps2x.Analog(PSS_LY);
// joystick down
if (y_Axis >= 130 && y_Axis<=255){
Pulsewidth_Shoulder=Pulsewidth_Shoulder -50;
//Shoulder.writeMicroseconds(Pulsewidth_Shoulder);
Trackleft.writeMicroseconds(1000);
Trackright.writeMicroseconds(1000);
delayMicroseconds(200);
}
//, joystick up
if (y_Axis>=0 && y_Axis<=125){
Pulsewidth_Shoulder= Pulsewidth_Shoulder +50;
//Shoulder.writeMicroseconds(Pulsewidth_Shoulder);
Trackleft.writeMicroseconds(2000);
Trackright.writeMicroseconds(2000);
}
//right turn slowly, joystick right
if (x_Axis >=130 && x_Axis <=255){
Pulsewidth_Base= Pulsewidth_Base +50;
//Base.writeMicroseconds(Pulsewidth_Base);
Trackleft.writeMicroseconds(2000);
Trackright.writeMicroseconds(1400);
}
//stop , joystick left
if (x_Axis >=0 && x_Axis <=125){
Pulsewidth_Base= Pulsewidth_Base -50;
// Base.writeMicroseconds(Pulsewidth_Base);
Trackleft.writeMicroseconds(1400);
Trackright.writeMicroseconds(2000);
delayMicroseconds(1000);
}
if(ps2x.Button(PSB_TRIANGLE)){
Serial.print("DOWN held this hard: ");
Serial.println(ps2x.Analog(PSAB_TRIANGLE), BIN);
Serial.print(angleElbow);
angleElbow=angleElbow -5;
Elbow.write(angleElbow);
}
if(ps2x.Button(PSB_L2)){
Serial.println("L2 pressed");
angleArm = angleArm - 5;
Arm.write(angleArm);
if(ps2x.Button(PSB_R2))
Serial.println("R2 pressed");
angleArm = angleArm + 5;
Arm.write(angleArm);
}
if(ps2x.Button(PSB_L1)){
angleWrist=angleWrist - 5;
Wrist.write(angleWrist);
}
if(ps2x.Button(PSB_R1)){
angleWrist=angleWrist + 5;
Wrist.write(angleWrist);
}
if(ps2x.ButtonPressed(PSB_CIRCLE)){ //will be TRUE if button was JUST pressed
Serial.println("Circle just pressed");
angleGripper= angleGripper + 5;
Gripper.write(angleGripper);
}
if(ps2x.Button(PSB_CROSS)) { //will be TRUE if button was JUST pressed OR released
Serial.println("X just changed");
angleElbow = angleElbow +5;
Elbow.write(angleElbow);
}
if(ps2x.ButtonPressed(PSB_SQUARE)) { //will be TRUE if button was JUST released
Serial.println("Square just released");
angleGripper = angleGripper -5;
Gripper.write(angleGripper);
}
if(ps2x.Button(PSB_L1) || ps2x.Button(PSB_R1)) { //print stick values if either is TRUE
Serial.print("Stick Values:");
Serial.print(ps2x.Analog(PSS_LY), DEC); //Left stick, Y axis. Other options: LX, RY, RX
Serial.print(",");
Serial.print(ps2x.Analog(PSS_LX), DEC);
Serial.print(",");
Serial.print(ps2x.Analog(PSS_RY), DEC);
Serial.print(",");
Serial.println(ps2x.Analog(PSS_RX), DEC);
}
delay(500);
}[/code]
Using this code, the commands to the tracks stop behaving the way i want them to and start behaving intermitently ! sometimes it does one thing, sometimes it does another. I feel that for some reason maybe the pulses are getting superimposed or maybe there needs to be some kind of delays that i need to insert to make it work (even though i fell the servo library does that for you), but it hasn’t helped. Maybe there is some kind of interference between the ps2 controller commands and the pins connected to the servos or the sabertooth, i don’t really know anymore.
I’m hoping someone has a clue why does a code behave a different way inside IF loops than outside them, when i’m using a simple sequence of pulse commands to the servo. Thanks in advance for any help as it would be much appreciated !
Gui
