Mini Maestro 12 - Pan/Tilt program

HI,
I want to program the Arduino Mega to control two servos pan / tilt from a two axis joystick input. (in essence two pots) The input voltage would be from 0 to 1023 on each axis. As I move the joystick the camera would pan/tilt accordingly. I am receiving data via xbee transmitter and receiver. The joystick is attached to the transmitting xbee, the xbee receiver is attached to the arduino Mega. The Maestro is attached to the arduino mega.

I am struggling with the program.

The servo specs from 600us to 2400 us for a full 180 degrees, or 2400 to 9600 quarter micro sec.

Question: I guess I need to map the voltage from 0/1023 to 2400 / 9600 and somehow break this up into bytes?
ie panmap = map(panmap,0,1023,2400,9600).

but panmap is one number. How do I parse pinmap to high and low bytes to do a serialWrite of the following arrays so as I move the joystick the pan/tilt works?

Pan Servo

panmap[0] = 0x84; Command
panmap[1] = 0x02; Channel #
panmap[2] = 0x??; This is joystick low byte
panmap[3] = 0x?? This is joystick high byte

Tilt Servo

tiltmap[0] = 0x84;
tiltmap[1] = 0x03;
tiltmap[2] = 0x??; This is joyhstick low byte
tiltmap[3] = 0x?? This is joystick high byte

thank you for helping
Scott

Hello, Scott.

In order to split your target position (panmap) into two bytes, you would first convert it to binary. The lower 7 bits of the number would be held by your panmap[2] byte and bits 7-13 would be held by panmap[3].

As an example, lets say you wanted to send a target position of 6000 (1500us). You could first convert it to binary, which is 01011101110000. Then, this would be split up into your panmap[2] and panmap[3] bytes of your serial command. Your panmap[2] byte would hold the lower 7 bits (1110000), while panmap[3] would hold bits 7-13 of the target (0101110).

Please note that these values do not need to be represented in binary; you could send the bytes in a hexadecimal (0x70 and 0x2E) or decimal (112 and 46) representation as well.

If you have not already done so, I would suggest reading the “Serial Servo Commands” section of the Maestro user’s guide for more information on this.

-Brandon

Hi Brandon,
Thank you so much for the great explanation! I read the “serial servo commands”. I understand how the maestro works and the commands, binary bits 0-7 for low byte… and to Serialwrite a target position, then Serialwrite the next target position. But I can’t seem to figure out how to make a wireless joystick work because I can’t write target positions for every pulse.

  1. When I move the joystick, the xbee transmits a two byte numbers between 0 - 1023 for a single axis. I need to take these two transmitted bytes,(high and low byte) parse bytes 0-6 (7 bits) and 7-13 (7 bits) make them decimal or hex, and plug them into the array above as you said. All this has to happen automatically so when I move the joystick, the data is transmitted through the xbee to the receiving xbee, to the arduino which then writes the array and the camera pans and tilts seamlessly. A wireless joystick. If I had a single byte to work with, it would be easy but there are two bytes which are modified with only 7 bits.

Can you give me some ideas on how to accomplish this?
Thanks,
Scott

I am not sure I follow what you are describing. Can you post a simple test case (with actual values) that makes it clear what you have to work with and what you are trying to do? For example:

“The joystick position is 1023, which the xbee transmits to my Arduino as two bytes: 0x03, 0xFF. I now need to convert these bytes into blah to do blah.”

-Brandon

HI Brandon,
I will give it a try. I am trying to control a pan/tilt wirelessly via xbee, arduino and maestro. For simplicity sake, lets use only one axis, one servo. The xbee tranmits an API frame that starts with 7E and ends with a check sum. The data lets say 180 degrees is represented by 1023 when I move the joystick to the right. The hex data is or 0x03, 0xFF. I can combine this back together again on the receive side, using the “word” function to one numer (1023) and map this to 180 degrees and serial.write to the servo. Works great.

The question is how do make it work with the maestro and not use the arduino servo library. If I was using only the Arduino it’s simple because the code only needs one value to make the servo work between 0 and 1023.

With the maestro it needs an array of 4 values, the last two are modified bytes so it’s more difficult. Here is example arduino code using #include <servo.h>

#include <Servo.h> 
Servo myservo;   
int potpin = 0;  
int val;    
 
void setup() 
{ 
  myservo.attach(9);  
} 
 
void loop() 
{ 
  val = analogRead(potpin);            // reads the value of the potentiometer (value between 0 and 1023) 
  val = map(val, 0, 1023, 0, 179);     // scale it to use it with the servo (value between 0 and 180) 
  myservo.write(val);                  // sets the servo position according to the scaled value 
  delay(15);                           // waits for the servo to get there 
}

You can see the “val” is one number, two bytes, the exact value that was sent by the xbee. However the maestro uses modified bytes: Here is the maestro command.

[code]
servoarm0[0] = 0x84; // 84 command is for target
servoarm0[1] = 0x00; //servo 0
servoarm0[2] = 0x7F; //target low bits (first 7 bits 0 - 6)
servoarm0[3] = 0x07; // second seven bits (7 - 13 )

[code]

The low and high bit using the maestro code are 0x7F, 0x07 which is not 1023, however using the arduino code for the same number is 0xFF 0x03.
If I used the received data 0x03, 0xFF it would be easy to plug it into the 4 line array above, but the maestro needs modified bytes of 0x7F, and 0x07.

Thats the magic question how can I take the data 0x03, 0xFF (1023 decimal), and get 0x7F, and 0x07 which the maestro needs and the program does this automatically. Presto…I move the pot which the xbee transmits the data. I take the data (two byte hex value), and modify it for the maestro?

Whew… I hope I explained it ok.
Thanks,
Scott

If the value you receive from your potentiometer (joystick) is 1023, you should take that value and map it to what you want to equivalent servo position to be. That should give you a output value of 9600 for an input value of 1023. Now you need to split 9600 into its lower 7 bits and higher 7 bits and send it to the Maestro. Formatting the bytes to send to the Maestro could look something like this:

unsigned char serialBytes[4];

...

int servoPosition = map(potValue, 0, 1023, 2400, 9600);  // map your pot value to servoPosition

serialBytes[0] = 0x84;  // "Set Target" command
serialBytes[1] = 0x02;  // channel 2
serialBytes[2] = (unsigned char)(servoPosition & 0x7F);  // byte containing the 7 least significant position bits
serialBytes[3] = (unsigned char)((servoPosition >> 7) & 0x7F);  // byte containing the 7 most significant position bits. 

The “& 0x7F” part of the command in servoPosition[3] is optional; it will make sure that only 7 bits get passed, which stops invalid values that consist of more than 14 bits from getting passed.

-Brandon

HI Brandon,
Thank you! I had to brush up on bit shifting, and have not tried it yet, but your example will work for sure if the arduino can do it fast enough. I will try it tomorrow.
Thanks again.
Scott

Hi Brandon,
I do have one question. Why are you setting the array to a char array and not a byte array? each element in the array is a byte I think?
Thanks,
Scott

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.

[code]//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);
    
}      

}
}[/code]

Thank you so much,
Scott

The difference between “byte” and “unsigned char” is that “byte” is not a standard C type, so I generally avoid it (though the Arduino environment uses a typedef to allow people to use “byte” as a synonym for “unsigned char”).

I have not thoroughly looked through your entire code, but it looks like you are using “& 0xFF” instead of “& 0x7F”, when you are trying to set servopan[2] to the lower 7 bytes. You might also want to include the parentheses for your servo[3] value, to be sure the command is being properly executed:

servopan[3] = (Tiltmapped >> 7) & 0x7F;

If fixing this does not solve your problem completely, I would recommend simplifying your code down to just one input and one servo.

-Brandon

Hi Brandon,
Yesssss!! it works. Both pan and tilt. The main reason why I did not want to use the arduino to control pan/tilt was noise completely making the pan/tilt shake and go nuts. I put 3 caps on each motor but still the pan/tilt servos would go crazy when I ran the motors. The arduino is connected to the pololu motor controller via TTL. But now I am using the Maestro and not the arduino, I can run the motors at full speed AND use the pan/tilt without noise on the pan/tilt line. For some reason the arduino would pick up the noise and mess with the pan/tilt… Interesting because all the wires are so close together. Interesting but true. I just could not get rid of the noise using the arduino to drive them.
Thanks again!!!

ps. This is why I buy from Pololu, because of the excellent help…it’s worth it for sure!!!
Scott

I am glad everything is working well now. I suspect that the “shaking” you were seeing when controlling the servos with the Arduino was not due to noise, but just servo jitter from the Arduino. In general, the Arduino boards are not good for generating very clean servo pulses.

-Brandon