Hi Brandon,
Thanks for bearing with me. After much trying, I think it’s written correctly but still can’t get the pan/tilt to work. Here is the entire cold for my robot. Most of the components are from Pololu. I am very close but no cigar yet. Can you please help? The pan/tilt is at the bottom of the code. I know the received data is good coming from the xbee through the arduino. Everything works except the pan/tilt? Thank you for taking the time to help me.
//this 4 wheel drive rover project consists of 4 motors, two motor controllers, 1 maestro servo controller, one arduino, two xbees, two joysticks for tank style drive, 1 joystick for pan tilt, and one switch for servo arm up and down.
// THE BELOW SECTION FOR PAN/TILT SERVO CONTROL
byte panhiByte = 0; //defines xbee pan/tilt variables
byte panloByte = 0;
byte tilthiByte = 0;
byte tiltloByte = 0;
unsigned int Pan = 0;
unsigned int Tilt = 0;
unsigned int Panmapped = 0;
unsigned int Tiltmapped = 0;
//---------------------------------------PAN/TILT SERVO CONTROL ABOVE
//--------------------------------------- ARM CONTROL BELOW
// The lower servo is servoarm0 running a speed of 25, 4480, 0 degrees, 8192 90 degrees
// the upper servo is servoarm1 on the arm running a speed of 47, 1792 0 degrees, 8896 90 degrees
byte servoarm0[4];
byte servoarm1[4];
unsigned char servopan[4];
unsigned char servotilt[4];
int upval = 0; // from transmitter toggle switch for arm. If up = low, arms go up
int downval = 0; // if down val = low, arms go down
byte digitalhibyte = 0; // digital data high byte of xbee input
byte digitallobyte = 0; // digital data low byte of xbee input
//----------------------------------------------------------- ARM CONTROL ABOVE
byte M1loByte = 0; // Motor 1 low byte not counting 7E, byte 11
byte M1hiByte = 0; // Motor 1 hi byte not counting 7E, byte 12
byte M2loByte = 0; // Motor 2
byte M2hiByte = 0; // Motor 2
unsigned int M1mapval = 0;
unsigned int M2mapval = 0;
unsigned int M1 = 0;
unsigned int M2 = 0;
unsigned int data = 0; // raw data gathered from serial read command
int mapvalueFwd = 0; // map 0-1023 to 0-100: Motor controller 100% = full speed 0 = stop
int mapvalueRev = 0; //map 1023-0 to 0-100: Motor controller 100% = full speed 0 = stop
void setup()
{
Serial.begin(9600);
Serial1.begin(9600);
Serial2.begin(9600);
//-----------------------------ARM CONTROL BELOW
}
void loop()
{
if(Serial1.available() > 0)
{
data = Serial1.read(); //reads data from serial port on the mega
if (data==0x7E) // start delimeter. read byte and throws it away
{
for(int i = 0; i < 12; i++) // counter
{
Serial1.read();
delay(1);
}
digitalhibyte = Serial1.read(); // output hi byte of xbee digital data input
delay(1);
digitallobyte = Serial1.read(); // output lo byte of xbee digital data input
delay(1);
M1hiByte = Serial1.read(); //byte #11 motor 1 - xbee pin 20
delay(1);
M1loByte = Serial1.read(); // byhte #12 motor 1 - xbee pin 20
delay(1);
M2hiByte = Serial1.read(); // byte #13 motor 2 - xbee pin 19
delay(1);
M2loByte = Serial1.read(); // byte #14 motor 2 - xbee pin 19
delay(1);
//-----------------------------------------------------
panhiByte = Serial1.read(); // byte #15 xbee pin 18
delay(1);
panloByte = Serial1.read(); // byte #16 xbee pin 18
delay(1);
tilthiByte = Serial1.read(); //byte # 17 xbee pin 17
delay(1);
tiltloByte = Serial1.read(); // byte #18 xbee pin 17
delay(1);
/*
if(panhiByte <=0xF)
{
Serial.print("0");
}
Serial.print(panhiByte,HEX);
Serial.print("\t");
if(panloByte <=0xF)
{
Serial.print("0");
}
Serial.print(panloByte,HEX);
Serial.print("\t");
if(tilthiByte <=0xF)
{
Serial.print("0");
}
Serial.print(tilthiByte,HEX);
Serial.print("\t");
if(tiltloByte <=0xF)
{
Serial.print("0");
}
Serial.println(tiltloByte,HEX);
*/
M1 = word(M1hiByte,M1loByte); //combines hi and lo bytes ie 0A + 16. word command = 0A16
// left side of robot, two motors each called motor 1
M2 = word(M2hiByte,M2loByte); // right side of robot, two motors each called motor 2
//-----------------------------------------------------------
Pan = word(panhiByte,panloByte); //SERVO DATA
Tilt = word(tilthiByte,tiltloByte);
//---------------------------------------------------------
Serial.print(digitalhibyte,HEX);
Serial.print("\t");
Serial.print(digitallobyte,HEX);
Serial.print("\t");
Serial.print(M1);
Serial.print("\t");
Serial.print(M2);
Serial.print("\t");
if(M1 >=450 && M1 <=530)
{
Serial1.write(170); //pololu protocol byte baud auto indication rate
Serial1.write(1); // device number 1 (motor controller 1
Serial1.write(5); // forward byteof 0x85 with most significant bit cleared
Serial1.write(0); // speed byte
Serial1.write(0); // speed byte
Serial1.write(170);
Serial1.write(2); // motor controller 2
Serial1.write(5);
Serial1.write(0);
Serial1.write(0);
}
if(M1 >530)
{
M1mapval = map(M1,531,1023,0,100);
Serial1.write(170);
Serial1.write(1); //motor controller 1
Serial1.write(5);
Serial1.write(0); // speed byte
Serial1.write(M1mapval); //speed byte. pot value
}
if(M2 > 530)
{
M2mapval = map(M2,531,1023,0,100);
Serial1.write(170);
Serial1.write(2); //motor controller 2
Serial1.write(5);
Serial1.write(0);
Serial1.write(M2mapval); // speed byte
}
//--------------------------------------------------------------------------
if(M1 < 450)
{
M1mapval = map(M1,449,0,0,100);
Serial1.write(170);
Serial1.write(1);
Serial1.write(6); //reverse data byte 86 with most significant digit cleared.
Serial1.write(0);
Serial1.write(M1mapval);
}
if(M2 < 450)
{
M2mapval = map(M2,449,0,0,100);
Serial1.write(170);
Serial1.write(2);
Serial1.write(6); //reverse byte
Serial1.write(0);
Serial1.write(M2mapval);
}
//-------------------------------------- SERVO COMMANDS BTWN LINES
if(digitalhibyte == LOW) // SWITCH FOR UP AND DOWN - ARMS MOVE UP TO VERTICAL
{
// below sets the speed for the two arms SPEED SET BELOW
Serial2.write(0x87); // 87 command is for speed
Serial2.write(0x00);
Serial2.write(0x19); //speed of 25 low data byte
Serial2.write(0x00); // high data byte
Serial2.write(0x87); // 87 command is for speed
Serial2.write(0x01);
Serial2.write(0x2F); //speed of 47 low data byte
Serial2.write(0x00); // high data byte
//----------------------------------------------------- SPEED SET ABOVE
servoarm0[0] = 0x84; // 84 command is for target
servoarm0[1] = 0x00; //servo 0
servoarm0[2] = 0x00; //target low bits (first 7 bits 0 - 6)
servoarm0[3] = 0x23; // second seven bits (7 - 13 )high data byte0x23 1120us times 4 = 4480. Servo 0 horizontal
servoarm1[0] = 0x84; //84 command is for target
servoarm1[1] = 0x01; //servo 1
servoarm1[2] = 0x00;
servoarm1[3] = 0x0E; // high data byte 1792 448us times 4 = 1792 servo 1, horizontal folded in on
// top of servo 0
Serial2.write(servoarm0,4);
Serial2.write(servoarm1,4);
delay(5);
}
if(digitalhibyte == HIGH)
{
Serial2.write(0x87); // 87 command is for speed
Serial2.write(0x00);
Serial2.write(0x19); //speed of 25 low data byte
Serial2.write(0x00); // high data byte
Serial2.write(0x87); // 87 command is for speed
Serial2.write(0x01);
Serial2.write(0x2F); //speed of 47 low data byte
Serial2.write(0x00); // high data byte
//----------------------------------------------------- SPEED SET ABOVE
servoarm0[0] = 0x84;
servoarm0[1] = 0x00;
servoarm0[2] = 0x00;
servoarm0[3] = 0x40; //high data byte 2048us times 4 = 8192, servo 0 straight up vertical
servoarm1[0] = 0x84;
servoarm1[1] = 0x01;
servoarm1[2] = 0x40; // target 8192
servoarm1[3] = 0x45; // high data byte 2224us times 4 = 8896 servo 1 straight up on top of servo 0
Serial2.write(servoarm0,4);
Serial2.write(servoarm1,4);
delay(5);
}
Panmapped = map(Pan, 0,1023,2400,9600);
Tiltmapped = map(Tilt, 0,1023,2400,9600);
delay(1);
Serial.print(Panmapped);
Serial.print("\t");\
Serial.println(Tiltmapped);
servopan[0] = 0x84; //target command
servopan[1] = 0x04; // channel 4
servopan[2] = (Panmapped & 0xFF); //low byte
servopan[3] = ((Panmapped >> 7) & 0x7F); //high byte
Serial2.write(servopan,4);
delay(2);
servopan[0] = 0x84;
servopan[1] = 0x05;
servopan[2] = Tiltmapped & 0xFF;
servopan[3] = Tiltmapped >> 7 & 0x7F;
Serial2.write(servotilt,4);
delay(2);
}
}
}
Thank you so much,
Scott
