PDA

View Full Version : [Question(s)] Arduino to Arduino serial servo control



cpunk7
06-29-2015, 05:39 PM
I'm connecting a ps2 controller to arduino #1, connecting that to arduino #2 via serial, arduino #2 controls pan/tilt servos. Currently the code only moves the pan servo right. The TX works fine, writes my protocol of servonumber/velocity/ the velocity is just a value that correlates to a speed and direction. I want the servos to respond to the joystick like a fps game, move to the right only when the joystick is engaged, etc. Currently the code results in very jittery movement in the right direction. Just wondering if processing time delays or interrupts on one side or the other is causing this and if I should try a different implementation. Most mech guys use double arduinos and xbees, right? Thanks for any help, I promise I scoured these forums and google first.

Here's the TX code: (connected to ps2 controller)



#include <PS2X_lib.h>PS2X ps2x; // create PS2 Controller Class
int error = 0;
byte type = 0;
byte vibrate = 0;
int servo_pan_pos = 1533; //initial position in ms, tweak as necessary, set equal to RX sketch
int servo_tilt_pos = 1725; //initial position in ms, tweak as necessary, set equal to RX sketch
int panAdjust; // Pan adjustment value
int tiltAdjust; // Tilt adjustmenet value


// these variables control the adjustment amount, ie the speed of the servos, increase values to increase speed.
//When thumbstick is all the way in a direction the servo
// receives the high speed value, anything less is low speed. these values to make the servos move faster.


int xHI = 9;
int xLO = 3;
int yHI = 9;
int yLO = 3;
int writedelay = 15; // delay between servo writes


void setup() {
Serial.begin(9600);


error = ps2x.config_gamepad(9,11,10,12, true, true); //setup pins and settings: GamePad(clock, command, attention, data, Pressures?, Rumble?) check for error
String errorVal;
errorVal=errorFunc(error); // sends error value to errorFunc() and returns string
Serial.println(errorVal); //prints error msg to serial monitor


type = ps2x.readType(); //checks type of controller and prints type to Serial Monitor
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




// ------------------------------------- START: Servo Control Code --------------------------//


int panX = ps2x.Analog(PSS_RX); //assigns right analog stick X values to variable
int tiltY = ps2x.Analog(PSS_RY); //assigns right analog stick Y values to variable


// Code for panning right
if (panX > 146){
if (panX > 253){
panAdjust = xHI; // if analog stick input is greater than 253 the adjustment value is high
}
else{
panAdjust = xLO; // if analog stick input is between 130-253 the adjustment value is low
}


char delim = '/';
char servoNum = 'A';
char pos_adjust;
if (panAdjust == xHI){
pos_adjust = '2';
}
if (panAdjust == xLO){
pos_adjust = '1';
}

// Code to write serial data //


Serial.write(servoNum);
delay(10);
Serial.write(delim);
delay(10);
Serial.write(pos_adjust);
delay(10);
Serial.write(delim);
delay(10);
}



//--------------------------------------END: Servo Control Code ----------------------------//




} // end controller check else statement
} // end void loop()






String errorFunc(int x){ // controller error che
String y;
if(x == 0){
y = "Found Controller, configured successful."
" Try out all the buttons, X will vibrate the controller, faster as you press harder"
" holding L1 or R1 will print out the analog stick values. ";
}


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


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


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


return y;
}





And the RX code (connected to servo):




#include <Servo.h>


Servo servo_pan; // create servo object to control a pan servo
Servo servo_tilt; // create servo object to control tilt servo


//These are variables that hold the servo IDs.
char tiltChannel=0, panChannel=1;
char serialChar=0; //This is a character that will hold data from the Serial port.
int pin_pan = 6; // assigns output pin to pan servo - should be a (~ PWM) pin
int pin_tilt = 5; // assigns output pin to tilt servo - should be a (~ PWM) pin
int SERVO_PAN_POS = 1533; //initial position, tweak as necessary
int SERVO_TILT_POS = 1725; //initial position, tweak as necessary
int xLO = 10; // adjustment values
int xHI = 30;
int yLO = 3;
int yHI = 10;
int chan_add;
int WRITE_DELAY = 60;
int SER_AVAIL_DELAY = 10;
char SERVO_NUM = 0;
int SERVO_VEL = 0;
int SERVO_POS;




// - FUNCTIONS -
void ServoStartup(void); // servo startup sequence
char GetSerial(void); // returns next char from buffer
void WriteServo(char servo, int pos); // writes to specified servo a given position
void Parse(); // parses out next 4 chars in serial stream and assigns current servo num and velocity
int ParseVel(int vel, char servonum, int pan_cur, int tilt_cur, int lo, int hi); // interprets the velocity number passed and returns position to write to servo






// these variables control the adjustment amount, ie the speed of the servos, increase values to increase speed.
//When thumbstick is all the way in a direction the servo
// receives the high speed value, anything less is low speed. these values to make the servos move faster.








void setup() {
Serial.begin(9600);
ServoStartup(); // run startup sequence
}


void loop() {
while (Serial.available() > 0)
{


Parse(); // parses out 4 chars of serial stream and assigns SERVO_NUM and SERVO_VEL
int x = SERVO_POS;
SERVO_POS = ParseVel(SERVO_VEL, SERVO_NUM, SERVO_PAN_POS, SERVO_TILT_POS, xLO, xHI);
// takes all the servo number/velocity and current position info and parses out the next position to send to WriteServo


delay(5);
if( x /= SERVO_POS)
{
// Serial.print("SERVO_POS: "); Serial.println(SERVO_POS);
WriteServo(SERVO_NUM, SERVO_POS );

}


}

// }
}
//int ParseVel(int vel, char servonum, int pan_cur, int tilt_cur, int lo, int hi)
//ParseVel(SERVO_VEL, SERVO_NUM, SERVO_PAN_POS, SERVO_TILT_POS, xLO, xHI);
int ParseVel(int vel, char servonum, int pan_cur, int tilt_cur, int lo, int hi){
int pos_new = 0;
int servo_cur;
// Serial.println("ParseVel: ");Serial.print(vel);Serial.print(", ");Serial.print(servonum);Serial.print(", ");Serial.print(pan_cur);Serial.print(", ");Serial.print(tilt_cur);
// Serial.print(", ");Serial.print(lo);Serial.print(", ");Serial.println(hi);



if (servonum == 'A')
{
servo_cur = servo_pan.readMicroseconds();
}
else if( servonum == 'B')
{
servo_cur = servo_tilt.readMicroseconds();
}

switch(vel)// vel = direction and speed category
{
case 1 :
pos_new = servo_cur + lo;
break;
case 2 :
pos_new = servo_cur + hi;
break;
case 3 :
pos_new = servo_cur - lo;
break;
case 4 :
pos_new = servo_cur - hi;
break;
default :
Serial.println("ParseVel function pos input out of range");
}
return pos_new;
}



void Parse(){
char BUFFER_VAL;
while (Serial.available()){
delay(SER_AVAIL_DELAY);
BUFFER_VAL = GetSerial();
if (BUFFER_VAL == '/'){
break;
}
if (BUFFER_VAL == 'A' || BUFFER_VAL == 'B' ){
SERVO_NUM = BUFFER_VAL;
// Serial.print(" Parse-SERVO_NUM: : "); Serial.print(SERVO_NUM);
}
}
while (Serial.available()){
BUFFER_VAL = GetSerial();
if (BUFFER_VAL == '/'){
break;
}
if (BUFFER_VAL >= '1' && BUFFER_VAL <= '8' ){ // velocity input range
int x = BUFFER_VAL - '0'; // to convert char to int
SERVO_VEL = x;
// Serial.print(" Parse-SERVO_VEL: : "); Serial.print(SERVO_VEL);Serial.print(", ");
}
}
}



char GetSerial(){
char serialdata = 0;
if (Serial.available() >0){ // 1. if there is data in the serial buffer
delay(SER_AVAIL_DELAY);
char inbyte = Serial.read();
// serialdata = inbyte - '0';
serialdata = inbyte;
inbyte = 0;
// Serial.print(" serialdata: "); Serial.print(serialdata);Serial.print(", ");
return serialdata;



}
}


void WriteServo(char servo, int pos){ // function, pass pan or tilt and position value (0-180) and it attaches, writes, delays and detaches servo
int delaytime = 0;
if (servo == 'A'){
delaytime = 10;//abs(pos - servo_pan.read()) * 4; // delay time dynamic to servo delay spec (deg/sec), adjust to your specific servo
servo_pan.attach(pin_pan);
servo_pan.writeMicroseconds(pos);
delay(WRITE_DELAY);
// servo_pan.detach();
// Serial.print("servo = ");
//Serial.println(servo);
// Serial.print("position written = ");
// Serial.println(pos);
// Serial.println(servo);
}
if (servo == 'B'){
delaytime = abs(pos - servo_tilt.read()) * 4; // delay time dynamic to servo delay spec (deg/sec), adjust to your specific servo
servo_tilt.attach(pin_tilt);
servo_tilt.write(pos);
delay(delaytime);
servo_tilt.detach();
// Serial.println(servo);
}
}



void ServoStartup(){
servo_pan.write(50); // writes position before attaching so servo travels to this known position
servo_pan.attach(pin_pan); //attaches servo_pan to output pin
delay(160);
servo_pan.write(120);
delay(160);
servo_tilt.write(50);// writes position before attaching so servo travels to this known position
servo_tilt.attach(pin_tilt); //attaches servo_tilt to output pin
delay(160);
servo_tilt.write(160);
delay(160);
servo_pan.writeMicroseconds(SERVO_PAN_POS); //set initial servo position if desired
delay(150);
servo_tilt.writeMicroseconds(SERVO_TILT_POS); //set initial servo position if desired
delay(150); // time for the servo to travel to center before detaching
servo_pan.detach(); // detach servos when not in use to avoid search noise
servo_tilt.detach();
Serial.print("Initial Pan Position: ");
Serial.println(servo_pan.readMicroseconds());
Serial.print("Initial Tilt Position: ");
Serial.println(servo_tilt.readMicroseconds());
delay(10);
}

tician
06-30-2015, 01:30 PM
Most people are using serial-bus controlled robot servos like dynamixel or herkulex servos with an Arduino-derivative, or something like the RaspberryPi, being sent messages over XBee or WiFi from a control station (usually some combination of PC, XBee hand-held controller, and/or 5.8GHz FPV video receiver with cheap monitor or usb vidcap card). Not very familiar with the arduino 'servo' library...

Might want to remove the 'delay(10);' between each of your Serial.write() commands in the PS2 side's code as it can really only hurt performance in your situation. Moving to a higher baudrate like 38400 or 57600 bps might also help if your radio transceivers can handle that.