Hi, please help me, I think I've implemented Pelco D on my mega but it don't work and I don't know why
I think it has something to do with the way I'm hadling bytes, I'm a bit lost I'm afraid. The hall effect joystick is working well and to design but just no talkie to the cam?
The MAX485 wiring
The Code
/*
The general idea is to have a hall effect joystick with X, Y and Z axis.
The Z axis is the zoom.
The joystick feeds into three analog inputs on the arduino.
Each axis has the following attributes :
1. AzisZero Range - this is the range between which we can assume the joystick is centered and not being used
2. AxisMax - this is the value which represents the joystick being moved to its maximum extent in the 'up/right/in' direction for Y, X and Z respectively
3. AxisMin - this is the value which represents the joystick being moved to its maximum extent in the 'down/left/out' direction for Y, X and Z respectively
The basic principal to which we will work is as follows :
We will calculate the direction to move based on whether the joystick is being moved left, right, up or down. If the joystick is no
at its centered position we can assume we need to send a 'move camera' command. The speed should be calculated based on how far between
AxisZero and AxisMaz/Min the stick is positioned.
Example.
AxisXZero Range = 520 - 530
AxisXMax = 1000 (Far Right)
AxisXMin = 0 (Far Left)
AxisYZero Range = 520 - 530
AxisYMax = 1000 (Top)
AxisYMin = 0 (Bottom)
Stick AxisXCurrent = 650
Stick AxisYCurrent = 400
This would mean we are asking the camera to move RIGHT (because 650 is greater than AxisXZero maximum of 530)
and DOWN (because 400 is less than AxisYZero minimum of 520).
The speed value the camera is expecting is between $00 (stop) to $3F (high speed). This gives us decimal 0-63.
The stick has an X axis movement range for travelling RIGHT of 530-1000, giving us 470 incements.
We can map the 470 increments to our 63 using the arduino function 'map(value, fromLow, fromHigh, toLow, toHigh)'
which re-maps a number from one range to another.
Actual Position in the right range = 650 - 530, = 120
map(120, 0, 470, 0, 63) = whatever!
*/
//set some Pelco command vars
int PelcoMaxSpeed = 63;
byte CurrCameraAddress = 02;
byte SynchByte = 00;
byte PanSpeed = 00;
byte TiltSpeed = 00;
byte ZoomSpeed = 00;
/*
command 1
7 Sense
6 Reserved
5 Reserved
4 Auto/Manual Scan
3 Camera On/Off
2 Iris Closed
1 Iris Open
0 Focus Near
*/
byte command1 = B00000000;
/*
command 2
7 Focus Far
6 Zoom Wide
5 Zoom Tele
4 Down
3 Up
2 Left
1 Right
0 Always 0
*/
byte command2 = B00000000;
//set our analog read pins
int XAxisPin = 7;
int XAxisVal = 0;
int YAxisPin = 1;
int YAxisVal = 0;
int ZAxisPin = 0;
int ZAxisVal = 0;
//set our axis zero ranges
int XAxisZeroMax = 520;
int XAxisZeroMin = 510;
int YAxisZeroMax = 525;
int YAxisZeroMin = 515;
int ZAxisZeroMax = 530;
int ZAxisZeroMin = 520;
//set our axis maximums
int XAxisMax = 910;
int YAxisMax = 1020;
int ZAxisMax = 1020;
//set our axis minimums
int XAxisMin = 120;
int YAxisMin = 10;
int ZAxisMin = 0;
//set up some working vars so we don't need to calculate them every loop itteration
int rightRange = 0;
int leftRange = 0;
int upRange = 0;
int downRange = 0;
int zoomInRange = 0;
int zoomOutRange = 0;
int MAX485 = 3;
void setup()
{
rightRange = XAxisMax - XAxisZeroMax;
leftRange = XAxisZeroMin - XAxisMin;
upRange = YAxisMax - YAxisZeroMax;
downRange = YAxisZeroMin - YAxisMin;
zoomInRange = ZAxisMax - ZAxisZeroMax;
zoomOutRange = ZAxisZeroMin - ZAxisMin;
Serial.begin(9600); // setup serial for debug
Serial3.begin(2400); // setup serial for commands
pinMode(MAX485, OUTPUT);
}
void loop()
{
//reset the command
command2 = B00000000;
//read the stick values
XAxisVal = analogRead(XAxisPin);
YAxisVal = analogRead(YAxisPin);
ZAxisVal = analogRead(ZAxisPin);
//X Axis
//are we within the zero range
if(XAxisVal < XAxisZeroMin || XAxisVal > XAxisZeroMax)
{
//we are not zerod, we need to send out a Pan command
//deturmine if it will be left or right
if(XAxisVal > XAxisZeroMax)
{
//we are going right, calculate the speed
int rightVal = XAxisVal - XAxisZeroMax;
PanSpeed = map(rightVal, 0, rightRange, 0, PelcoMaxSpeed);
//set the move right bit of the command
command2 = command2 | B00000010;
//debug
Serial.print("Move Right");
Serial.println(PanSpeed);
}
else
{
//we are going left
int leftVal = XAxisZeroMin - XAxisVal;
PanSpeed = map(leftVal, 0, leftRange, 0, PelcoMaxSpeed);
//set the move left bit of the command
command2 = command2 | B00000100;
//debug
Serial.print("Move Left");
Serial.println(PanSpeed);
}
}
//Y Axis
//are we within the zero range
if(YAxisVal < YAxisZeroMin || YAxisVal > YAxisZeroMax)
{
//we are not zerod, we need to send out a Pan command
//deturmine if it will be left or right
if(YAxisVal > YAxisZeroMax)
{
//we are going up, calculate the speed
int upVal = YAxisVal - YAxisZeroMax;
TiltSpeed = map(upVal, 0, upRange, 0, PelcoMaxSpeed);
//set the move right bit of the command
command2 = command2 | B00001000;
//debug
Serial.print("Move Up");
Serial.println(TiltSpeed);
}
else
{
//we are going left
int downVal = YAxisZeroMin - YAxisVal;
TiltSpeed = map(downVal, 0, downRange, 0, PelcoMaxSpeed);
//set the move left bit of the command
command2 = command2 | B00010000;
//debug
Serial.print("Move Down");
Serial.println(TiltSpeed);
}
}
//Z Axis
//are we within the zero range
if(ZAxisVal < ZAxisZeroMin || ZAxisVal > ZAxisZeroMax)
{
//we are not zerod, we need to send out a zoom command
//deturmine if it will be tele or wide
if(ZAxisVal > ZAxisZeroMax)
{
//we are going tele, calculate the speed
int teleVal = ZAxisVal - ZAxisZeroMax;
ZoomSpeed = map(teleVal, 0, zoomInRange, 0, PelcoMaxSpeed);
//set the move right bit of the command
command2 = command2 | B00100000;
//debug
Serial.print("Zoom In");
Serial.println(ZoomSpeed);
}
else
{
//we are going left
int wideVal = ZAxisZeroMin - ZAxisVal;
ZoomSpeed = map(wideVal, 0, zoomOutRange, 0, PelcoMaxSpeed);
//set the move left bit of the command
command2 = command2 | B01000000;
//debug
Serial.print("Zoom Out");
Serial.println(ZoomSpeed);
}
}
//set the MAX485 to send data
digitalWrite(MAX485, HIGH);
/*
SEND SPECIAL ZOOM SPEED COMMAND
Command W3 | W4 | W5 | W6
--------------------------------------
Set Preset 00 | 03 | 00 | 01 to 20
Clear Preset 00 | 05 | 00 | 01 to 20
Go To Preset 00 | 07 | 00 | 01 to 20
Set Zoom Speed 00 | 25 | 00 | 00 to 03
*/
Serial3.write((byte)SynchByte); // Synch Byte
Serial3.write((byte)CurrCameraAddress); //Camera Address
Serial3.write((byte)00);
Serial3.write((byte)25);
Serial3.write((byte)00);
Serial3.write((byte)ZoomSpeed);
byte checkSum = ((byte)SynchByte + (byte)CurrCameraAddress + (byte)00 + (byte)25 + (byte)00 + (byte)ZoomSpeed)%256;
Serial3.write((byte)checkSum); //check sum is the sum of bytes (excluding the synchronization byte) modulo 256
/*
SEND PTZ COMMAND
Byte 1 - Synch Byte
Byte 2 - Address
Byte 3 - Command 1
Byte 4 - Command 2
Byte 5 - Data 1/Pan Speed
Byte 6 - Data 2/Tilt Speed
Byte 7 - Check Sum
*/
Serial3.write((byte)SynchByte); // Synch Byte
Serial3.write((byte)CurrCameraAddress); //Camera Address
Serial3.write((byte)command1);
Serial3.write((byte)command2);
Serial3.write((byte)PanSpeed); //Pan Speed
Serial3.write((byte)TiltSpeed); //Tilt Speed
checkSum = ((byte)SynchByte + (byte)CurrCameraAddress + (byte)command1 + (byte)command2 + (byte)PanSpeed + (byte)TiltSpeed)%256;
Serial3.write((byte)checkSum); //check sum is the sum of bytes (excluding the synchronization byte) modulo 256
//set the MAX485 to STOP send data
digitalWrite(MAX485, LOW);
delay(500);
}