Код движения Zero Robotics не работает

Question

Код движения Zero Robotics не работает

Я не понимаю, почему этот код не будет работать. Я пишу код движения для моей команды Zero Robotics, и моя текущая система состоит в том, чтобы установить целевую скорость на коэффициент расстояния для каждого начального расстояния от целевой позиции. Я не ищу критику в коде, но я хотел бы понять, почему абсолютно ничего не происходит когда я запускаю этот код — я ожидаю, что что-то произойдет, по крайней мере. Сфера просто сидит там, ничего не делая.


float tarPos[3];
float currDist[3];
float currPos[3];
float initPos[3];
float initDist[3];
float tarVelo[3];
float myZRState[12];
float powfResult[3];

void init()
{
api.getMyZRState(myZRState);

tarPos[0]=0.5f;
tarPos[1]=0.0f;
tarPos[2]=0.5f;
}

void moveTo(float target[3])
{
api.getMyZRState(myZRState);

currPos[0]=myZRState[0];
currPos[1]=myZRState[1];
currPos[2]=myZRState[2];

currDist[0]=tarPos[0]-currPos[0];
currDist[1]=tarPos[1]-currPos[1];
currDist[2]=tarPos[2]-currPos[2];

//tarVelo[0]=(100)*(currDist[0]);
//tarVelo[1]=(100)*(currDist[1]);
//tarVelo[2]=(100)*(currDist[2]);

tarVelo[0]=powfResult[0]*currDist[0];
tarVelo[1]=powfResult[1]*currDist[1];
tarVelo[2]=powfResult[2]*currDist[2];

api.setVelocityTarget(tarVelo);

//DEBUG (( "powf result 1, powf result 2, powf result 3, init dist 1, init dist 2, init dist 3 \t %f %f %f %f %f %f" , powfResult[0] , powfResult[1] , powfResult[2] , initDist[0] , initDist[1] , initDist[2] ));
}

void loop()
{
if (api.getTime()==0)
{
api.getMyZRState(myZRState);

initPos[0]=myZRState[0];
initPos[1]=myZRState[1];
initPos[2]=myZRState[2];

initDist[0]=tarPos[0]-initPos[0];
initDist[1]=tarPos[1]-initPos[1];
initDist[2]=tarPos[2]-initPos[2];

if (initDist[0]==0)
powfResult[0]=0;
else
powfResult[0]=(0.1508*(powf( initDist[0] , -0.56 )));
if (initDist[1]==0)
powfResult[1]=0;
else
powfResult[1]=(0.1508*(powf( initDist[1] , -0.56 )));
if (initDist[2]==0)
powfResult[2]=0;
else
powfResult[2]=(0.1508*(powf( initDist[2] , -0.56 )));
}
moveTo(tarPos);
}

[/ NOEDIT]

РЕДАКТИРОВАТЬ: Решение найдено! Я передал это программисту в моей школе (его зовут Брайс, вы, возможно, захотите следить за ним в будущем. Совершенно гениально) вместе с моим учителем компьютерных технологий, и они выяснили, что должен быть абсолютное значение вокруг [code] initDist [/ code] в инициализации [code] powfResult [/ code]. Спасибо за вашу помощь!

-2

c++robotics

Решение

Другие решения

Других решений пока нет …

Источник

Accepted Answer

Моя первая программа может выглядеть примерно так:

void init()
{
DEBUG(("init entered\n"));
DEBUG(("init leaving\n"));
}
void moveTo(float target[3])
{
DEBUG (( "moveTo entered" ));
DEBUG (( "moveTo leaving" ));
}
void loop()
{
DEBUG (( "loop for David Bluhm entered\n" ));
DEBUG (( "loop leaving\n" ));
}

Если бы я смог найти результат после его запуска, я бы немного поверил в некоторые API. (Я определенно буду выводить отладочные состояния в первые несколько раз, когда мне нужно что-то переместить)

PS — почему ты звонишь: setVelocityTarget()

вместо: setPositionTarget()? (Это, видимо, самый базовый контроль местоположения)

0