Hello,

I am using the robotc code form Xander's blog entry: "Pan and Tilt Rig for NXTCamV3 and Firmware Tests". The tracking PID algorithm is working fine for me. However, the values returned by the motor encoders do not make sense. I would like to incorporate a way to limit the tilt/pan angle using the encoders. I am using version RobotC 3.08 and F/W 9.12.
Here is part of the code where the encoders are read:
…
else {
//Calculate blob center
_x = (_blobs[0].x2 + _blobs[0].x1)/2;
_y = (_blobs[0].y2 + _blobs[0].y1)/2;

// Calculate Pan error - power
err_x = _x - sp_x;
aerr_x += err_x;
pwr_x = (err_x * p_x) + ((err_x - perr_x) * d_x) + (aerr_x * i_x);

//Calculate Tilt error - power
err_y = _y - sp_y;
aerr_y += err_y;
pwr_y = (err_y * p_y) + ((err_y - perr_y) * d_y) + (aerr_y * i_y);

m_x = nMotorEncoder[M_PAN];
m_y = nMotorEncoder[M_TILT];

// Move motors according calculated power for Pan and Tilt
motor[M_TILT] = pwr_y;
motor[M_PAN] = pwr_x;

perr_y = err_y;
perr_x = err_x;
}

What could be the issue with the values returned by the encoders?

Thanks.

This is the source code

#pragma config(Sensor, S2, cam, sensorI2CCustomFastSkipStates)
#pragma config(Sensor, S3, HTAC, sensorI2CHiTechnicAccel)
#pragma config(Motor, motorA, M_TILT, tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor, motorB, , tmotorNormal, openLoop)
#pragma config(Motor, motorC, M_PAN, tmotorNormal, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//

#include "drivers/common.h"
#include "drivers/NXTCAM-driver.h"
#include "drivers/HTAC-driver.h"


/*
Main task
*/
task main ()
{
blob_array _blobs;
memset(_blobs, 0, sizeof(blob_array));

float pi180 = 565.486677;
float temp;
float _pitch, _roll, _yaw;

int _x_axis = 0;
int _y_axis = 0;
int _z_axis = 0;

//blob_array _blobs;
int _x, _y;
int _nblobs;

int m_x,
m_y;

// pid variables
float pwr_x,
err_x,
perr_x,
aerr_x = 0.0,
sp_x,
p_x,
i_x,
d_x = 0.0;

float pwr_y,
err_y,
perr_y,
aerr_y = 0.0,
sp_y,
p_y,
i_y,
d_y = 0.0;

sp_y = 87;
sp_x = 87;
p_y = p_x = 0.5;
d_y = d_x = 1.8;
i_y = i_x = 0.05;

// Initialise the camera
NXTCAMinit(cam);
eraseDisplay();
nMotorEncoder[M_PAN] = 0;
nMotorEncoder[M_TILT] = 0;

while(true) {

/*
// Read all of the axes at once
if (!HTACreadAllAxes(HTAC, _x_axis, _y_axis, _z_axis)) {
nxtDisplayTextLine(4, "ERROR!!");
wait1Msec(2000);
StopAllTasks();
}
else {

if ((_x_axis != 0) && (_y_axis != 0) && (_z_axis != 0)) {

// Pitch calculation
temp = (float) -_x_axis / -_z_axis;
_pitch = atan(temp);
_pitch = 90 - _pitch / pi180;
if (_x_axis < 0) _pitch += 180;

// Yaw calculation
temp = (float) -_z_axis /-_x_axis;
_yaw = atan(temp);
_yaw = 90 - _yaw / pi180;
if (_z_axis < 0) _yaw += 180;
}
}
//nxtDisplayTextLine(1, "p:%d",_pitch);
//nxtDisplayTextLine(2, "y:%d",_yaw);
*/
// Fetch all the blobs, have the driver combine all
// the colliding blobs.
_nblobs = NXTCAMgetBlobs(cam, _blobs);

if (_nblobs == 0) {

motor[M_TILT] = 0;
motor[M_PAN] = 0;
pwr_x = 0;
pwr_y = 0;
}
else {
//Calculate blob center
_x = (_blobs[0].x2 + _blobs[0].x1)/2;
_y = (_blobs[0].y2 + _blobs[0].y1)/2;

// Calculate Pan error - power
err_x = _x - sp_x;
aerr_x += err_x;
pwr_x = (err_x * p_x) + ((err_x - perr_x) * d_x) + (aerr_x * i_x);

//Calculate Tilt error - power
err_y = _y - sp_y;
aerr_y += err_y;
pwr_y = (err_y * p_y) + ((err_y - perr_y) * d_y) + (aerr_y * i_y);

m_x = nMotorEncoder[M_PAN];
m_y = nMotorEncoder[M_TILT];

// Move motors according calculated power for Pan and Tilt
motor[M_TILT] = pwr_y;
motor[M_PAN] = pwr_x;

perr_y = err_y;
perr_x = err_x;
}
wait1Msec(50);
}
}

I have changed the m_x and m_y data type to long, and the problem still persist.

Hi uskjmp14,
You may know that my posts (including some answers to your question) were accidentally erased from the forum.
Anyway, did you solve your issue? what are the values returned by encoders?

Miki.

Hello,

I am still working on the datalog.

Regards.

Hello,

After looking at the data logs it seems that the encoders returned values according to the power(-/+) applied to the motors. What I described as inconsistent values can be seen in the "followlog_Combined" chart. After the point 481 the object I am tracking stays close to the starting position and it is not moving. However the lines plotted for the encoders show fluctuations between 50 and -50. Lighting conditions aren’t the best in the area where I am running this project. However, rather than looking for a perfect setting I would like to manage the noise in the sensor readings combined with an algorithm to control motor motion boundaries.

The goal of this project is to establish proper boundaries to the motor motion and also to come back to the initial absolute position when the tracked object is not in the NXTCam visual field. I have also added two charts that show the plotted values for horizontal only and vertical only tests. On each case the encoders show the tracked object motion without problems.

So my question is condensed to how I can manage the noise coming from the lighting conditions and what algorithm is the most appropriate in this case. Can you point me out some online literature to use in this case? Suggestions are also welcome.

Thanks.