// calculate left and right wheel commands
void combine(uint32_t x, uint32_t y)
{
int32_t xError, yError, axesIn[2], axesOut[2];
xError = X_CENTER-x;
yError = Y_TRACK-y;
//cprintf("x: %d y: %d xError: %d yError: %d\n", x, y, xError, yError);
axesIn[0] = g_transLoop.update(yError);
axesIn[1] = g_rotLoop.update(xError);
axisMap(axesIn, axesOut);
rcs_setPos(LEFT_AXIS, scaleMotor(axesOut[0]));
rcs_setPos(RIGHT_AXIS, scaleMotor(axesOut[1]));
}
void motor(uint32_t x, uint32_t y)
{
static MotorLoop rloop(1600, 4500);
static MotorLoop tloop(4000, 4000);
int32_t xerror, yerror, axesIn[2], axesOut[2];
xerror = XCENTER-x;
yerror = YTRACK-y;
axesIn[0] = tloop.update(yerror);
axesIn[1] = rloop.update(xerror);
axisMap(axesIn, axesOut);
rcs_setPos(0, scaleMotor(axesOut[0]));
rcs_setPos(1, scaleMotor(axesOut[1]));
}
void chaseLoadParams()
{
prm_add("Translation P gain", 0,
"@c Chase_demo tranlational proportional gain (default 500)", INT32(500), END);
prm_add("Translation D gain", 0,
"@c Chase_demo translational derivative gain (default 800)", INT32(800), END);
prm_add("Rotation P gain", 0,
"@c Chase_demo rotational proportional gain (default 500)", INT32(500), END);
prm_add("Rotation D gain", 0,
"@c Chase_demo rotational derivative gain (default 800)", INT32(800), END);
int32_t pgain, dgain;
prm_get("Translation P gain", &pgain, END);
prm_get("Translation D gain", &dgain, END);
g_transLoop.setGains(pgain, dgain);
prm_get("Rotation P gain", &pgain, END);
prm_get("Rotation D gain", &dgain, END);
g_rotLoop.setGains(pgain, dgain);
}
