/*!
* Two house numbers are considered to be equal if
* * country, city, postcode, suburb, street, housenumber, name, and shop equal
* * or housenumber, street, name, and shop equal, and country, city, and postcode do not differ (ignoring empty values),
* and lat/lon difference is less than DISTANCE_THRESHOLD
*/
bool HouseNumber::isSameAddress(HouseNumber const& rhs) const {
if(getName().toLower()!=rhs.getName().toLower() ||
getShop().toLower()!=rhs.getShop().toLower() ||
getNumber().toLower()!=rhs.getNumber().toLower() ||
getStreet().toLower()!=rhs.getStreet().toLower() ||
getNumber()=="" || getStreet()=="") {
return false;
}
if(getPostcode().toLower()==rhs.getPostcode().toLower() && getPostcode()!="" &&
getCity().toLower()==rhs.getCity().toLower() && getCity()!="" &&
getSuburb().toLower()==rhs.getSuburb().toLower() &&
getCountry().toLower()==rhs.getCountry().toLower() && getCountry()!="") {
return true;
}
// consider two house numbers with similar address information and little distance to each other to be equal
if(myAbs(getLat()-rhs.getLat())>DISTANCE_THRESHOLD ||
myAbs(getLon()-rhs.getLon())>DISTANCE_THRESHOLD)
return false;
if(getPostcode()!="" && rhs.getPostcode()!="" && getPostcode().toLower()!=rhs.getPostcode().toLower())
return false;
if(getCity()!="" && rhs.getCity()!="" && getCity().toLower()!=rhs.getCity().toLower())
return false;
if(getSuburb()!="" && rhs.getSuburb()!="" && getSuburb()!=rhs.getSuburb())
return false;
if(getCountry()!="" && rhs.getCountry()!="" && getCountry().toLower()!=rhs.getCountry().toLower())
return false;
return true;
}
int predicate(float eps, MyRect& r1, MyRect& r2)
{
float delta = eps*(myMin(r1.width, r2.width) + myMin(r1.height, r2.height))*0.5;
return myAbs(r1.x - r2.x) <= delta &&
myAbs(r1.y - r2.y) <= delta &&
myAbs(r1.x + r1.width - r2.x - r2.width) <= delta &&
myAbs(r1.y + r1.height - r2.y - r2.height) <= delta;
}
bool player::Tplayer::killEnemy(player::Tcharacter enemy)
{
int radius = 1;
if (this->arrayChar[this->characActive].hasShotgun)
radius = 3;
if (myAbs(this->arrayChar[this->characActive].i - enemy.i)<=radius &&
myAbs(this->arrayChar[this->characActive].j - enemy.j)<=radius )
{
this->arrayChar[this->characActive].coins +=enemy.coins;
this->arrayChar[this->characActive].hasShotgun = false;
return true;
}
return false;
}
LL pollardRho(LL n, LL seed) {
LL x, y, head = 1, tail = 2; x = y = random() % (n - 1) + 1;
for ( ; ; ) {
x = addMod(multiplyMod(x, x, n), seed, n);
if (x == y) return n; LL d = gcd(myAbs(x - y), n);
if (1 < d && d < n) return d;
if (++head == tail) y = x, tail <<= 1;
}} vector<LL> divisors;
//FAST SINE APPROX
float mySin(float x){
float sinr = 0;
uint8_t g = 0;
while(x > myPI){
x -= 2*myPI;
g = 1;
}
while(!g&(x < -myPI)){
x += 2*myPI;
}
sinr = myDPI*x - myDPI2*x*myAbs(x);
sinr = 0.225*(sinr*myAbs(sinr)-sinr)+sinr;
return sinr;
}
int isDiagDom( int mat[][N])
{
int diagNum, rowSum, i, k ;
for (i = 0; i < N; i++)
{
diagNum = myAbs(mat[i][i]) ;
for (k = 0; k < N; k++)
{
if (k != i)
rowSum += myAbs( mat[i][k] ) ;
}
if (diagNum < rowSum)
return 0 ;
}
return 1;
}
int takeNext(int i, int taken)
{
int ret, next;
if (i >= N)
{
ret = myAbs(taken - (superSum - taken));
return ret;
}
ret = saved[i][taken];
if (ret >= 0) return ret; // Return saved
next = i + 1;
int withCurrent = takeNext(next, taken + myData[i]);
int withoutCurrent = takeNext(next, taken);
ret = myMin(withCurrent, withoutCurrent);
saved[i][taken] = ret; // Save
return ret;
}
//ROUND A NUMBER
int16_t myRound(float in){
int8_t s = in/myAbs(in);
return (int16_t)(s*(myAbs(in) + 0.5));
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
//L2L2OpticalFlow(u1,u2,tol,lambda,ux,uy,ut)
{
double *u1 = mxGetPr(prhs[0]);
double *u2 = mxGetPr(prhs[1]);
float tol = (float)mxGetScalar(prhs[2]);
float lambda = (float)mxGetScalar(prhs[3]);
int maxIterations = (int)mxGetScalar(prhs[4]);
const size_t *sizeImage = mxGetDimensions(prhs[0]);
double *inputV = 0;
double *inputY = 0;
int typeNorm = 1;
if (nrhs > 5)
{
typeNorm = (int)mxGetScalar(prhs[5]);
}
if (nrhs > 6)
{
inputV = mxGetPr(prhs[6]);
}
if (nrhs > 7)
{
inputY = mxGetPr(prhs[7]);
}
int nPx = (int)(sizeImage[0] * sizeImage[1]);
const size_t sizeY[2] = {5*nPx,1};
// Output v1
plhs[0] = mxCreateNumericArray(2, sizeImage, mxDOUBLE_CLASS, mxREAL);
double *Outv1 = mxGetPr(plhs[0]);
// Output v2
plhs[1] = mxCreateNumericArray(2, sizeImage, mxDOUBLE_CLASS, mxREAL);
double *Outv2 = mxGetPr(plhs[1]);
// Output Y
plhs[2] = mxCreateNumericArray(2, sizeY, mxDOUBLE_CLASS, mxREAL);
double *YOut = mxGetPr(plhs[2]);
float* v1 = new float[nPx];
float* v2 = new float[nPx];
float* u = new float[nPx];
float* ut = new float[nPx];
float* sigut = new float[nPx];
float* y11 = new float[nPx];
float* y12 = new float[nPx];
float* y21 = new float[nPx];
float* y22 = new float[nPx];
float* y5 = new float[nPx];
float* Kty1 = new float[nPx];
float* Kty2 = new float[nPx];
float* Kx11 = new float[nPx];
float* Kx12 = new float[nPx];
float* Kx21 = new float[nPx];
float* Kx22 = new float[nPx];
float* Kx5 = new float[nPx];
float tau = 1.0f / sqrt(8.0f);
float sigma = tau;
float dTau = 1.0f / tau;
float dSigma = 1.0f / sigma;
//residuals
float p = 0;
float d = 0;
float err = 1.0;
float ssl = 1.0f - sigma / (sigma + lambda);
#pragma omp parallel for
for (int j = 0; j < sizeImage[1]; ++j)
{
for (int i = 0; i < sizeImage[0]; ++i)
{
int tmpIndex = index2DtoLinear(sizeImage, i, j);
//Index for gradients
u[tmpIndex] = (float)u1[tmpIndex];
ut[tmpIndex] = (float)(u2[tmpIndex] - u1[tmpIndex]);
sigut[tmpIndex] = (float)(sigma*ut[tmpIndex]);
if (nrhs > 6)
{
v1[tmpIndex] = (float)inputV[tmpIndex];
v2[tmpIndex] = (float)inputV[nPx + tmpIndex];
}
else
{
v1[tmpIndex] = 0;
v2[tmpIndex] = 0;
}
Kty1[tmpIndex] = 0;
Kty2[tmpIndex] = 0;
if (nrhs > 7)
{
y11[tmpIndex] = (float)inputY[tmpIndex];
y12[tmpIndex] = (float)inputY[nPx + tmpIndex];
y21[tmpIndex] = (float)inputY[2 * nPx + tmpIndex];
y22[tmpIndex] = (float)inputY[3 * nPx + tmpIndex];
y5[tmpIndex] = (float)inputY[4 * nPx + tmpIndex];
}
else
{
y11[tmpIndex] = 0;
y12[tmpIndex] = 0;
y21[tmpIndex] = 0;
y22[tmpIndex] = 0;
y5[tmpIndex] = 0;
}
Kx11[tmpIndex] = 0;
Kx12[tmpIndex] = 0;
Kx21[tmpIndex] = 0;
Kx22[tmpIndex] = 0;
Kx5[tmpIndex] = 0;
}
}
int iterations = 0;
while (err > tol && iterations <= maxIterations)
{
++iterations;
if (iterations % 50 == 0)
{
p = 0;
d = 0;
}
//primal step
#pragma omp parallel for reduction(+:p)
for (int j = 0; j < sizeImage[1]; ++j)
{
for (int i = 0; i < sizeImage[0]; ++i)
{
int tmpIndex = index2DtoLinear(sizeImage, i, j);
float Kty1Old = Kty1[tmpIndex];
float Kty2Old = Kty2[tmpIndex];
//transpose equals -div
Kty1[tmpIndex] = -(dxm(y11, sizeImage, i, j) + dym(y12, sizeImage, i, j) + dycT(y5, u, sizeImage, i, j));
Kty2[tmpIndex] = -(dxm(y21, sizeImage, i, j) + dym(y22, sizeImage, i, j) + dxcT(y5, u, sizeImage, i, j));
float v1Old = v1[tmpIndex];
float v2Old = v2[tmpIndex];
v1[tmpIndex] = v1Old - tau*Kty1[tmpIndex];
v2[tmpIndex] = v2Old - tau*Kty2[tmpIndex];
if (iterations % 50 == 0)
{
//residuals
p += myAbs((v1Old - v1[tmpIndex]) * dTau - Kty1Old + Kty1[tmpIndex])
+ myAbs((v2Old - v2[tmpIndex]) * dTau - Kty2Old + Kty2[tmpIndex]);
}
}
}
//dual step
#pragma omp parallel for reduction(+:d)
for (int j = 0; j < sizeImage[1]; ++j)
{
for (int i = 0; i < sizeImage[0]; ++i)
{
int tmpIndex = index2DtoLinear(sizeImage, i, j);
float Kx11Old = Kx11[tmpIndex];
float Kx12Old = Kx12[tmpIndex];
float Kx21Old = Kx21[tmpIndex];
float Kx22Old = Kx22[tmpIndex];
float Kx5Old = Kx5[tmpIndex];
Kx11[tmpIndex] = dxp(v1, sizeImage, i, j);
Kx12[tmpIndex] = dyp(v1, sizeImage, i, j);
Kx21[tmpIndex] = dxp(v2, sizeImage, i, j);
Kx22[tmpIndex] = dyp(v2, sizeImage, i, j);
Kx5[tmpIndex] = dyc(v1, u, sizeImage, i, j) + dxc(v2, u, sizeImage, i, j);
float y11Old = y11[tmpIndex];
float y12Old = y12[tmpIndex];
float y21Old = y21[tmpIndex];
float y22Old = y22[tmpIndex];
float y5Old = y5[tmpIndex];
y11[tmpIndex] = ssl * (y11[tmpIndex] + sigma*(2 * Kx11[tmpIndex] - Kx11Old));
y12[tmpIndex] = ssl * (y12[tmpIndex] + sigma*(2 * Kx12[tmpIndex] - Kx12Old));
y21[tmpIndex] = ssl * (y21[tmpIndex] + sigma*(2 * Kx21[tmpIndex] - Kx21Old));
y22[tmpIndex] = ssl * (y22[tmpIndex] + sigma*(2 * Kx22[tmpIndex] - Kx22Old));
y5[tmpIndex] = myMax(-1.0f, myMin(1.0f, y5[tmpIndex] + sigma*(2 * Kx5[tmpIndex] - Kx5Old) + sigut[tmpIndex]));
if (iterations % 50 == 0)
{
d += myAbs((y11Old - y11[tmpIndex]) * dSigma - Kx11Old + Kx11[tmpIndex]) +
myAbs((y12Old - y12[tmpIndex]) * dSigma - Kx12Old + Kx12[tmpIndex]) +
myAbs((y21Old - y21[tmpIndex]) * dSigma - Kx21Old + Kx21[tmpIndex]) +
myAbs((y22Old - y22[tmpIndex]) * dSigma - Kx22Old + Kx22[tmpIndex]) +
myAbs((y5Old - y5[tmpIndex]) * dSigma - Kx5Old + Kx5[tmpIndex]);
}
}
}
if (iterations % 50 == 0)
{
err = (d*d + p*p) / nPx;
}
if (iterations % 1000 == 0)
{
mexPrintf("Iteration %d,Residual %e\n", iterations, err);
mexEvalString("drawnow;");
}
}
//write output
#pragma omp parallel for
for (int j = 0; j < sizeImage[1]; ++j)
{
for (int i = 0; i < sizeImage[0]; ++i)
{
int tmpIndex = index2DtoLinear(sizeImage, i, j);
YOut[tmpIndex] = (double)y11[tmpIndex];
YOut[tmpIndex + nPx] = (double)y12[tmpIndex];
YOut[tmpIndex + 2 * nPx] = (double)y21[tmpIndex];
YOut[tmpIndex + 3 * nPx] = (double)y22[tmpIndex];
YOut[tmpIndex + 4 * nPx] = (double)y5[tmpIndex];
Outv1[tmpIndex] = (double) v1[tmpIndex];
Outv2[tmpIndex] = (double) v2[tmpIndex];
}
}
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
//L2L2OpticalFlow(u1,u2,tol,lambda,ux,uy,ut)
{
double *u1 = mxGetPr(prhs[0]);
double *u2 = mxGetPr(prhs[1]);
float tol = (float)mxGetScalar(prhs[2]);
float lambda = (float)mxGetScalar(prhs[3]);
int maxIterations = (int)mxGetScalar(prhs[4]);
const mwSize *sizeImage = mxGetDimensions(prhs[0]);
double *inputV = 0;
double *inputY = 0;
int typeNorm = 1;
if (nrhs > 5)
{
typeNorm = (int)mxGetScalar(prhs[5]);
}
if (nrhs > 6)
{
inputV = mxGetPr(prhs[6]);
}
if (nrhs > 7)
{
inputY = mxGetPr(prhs[7]);
}
int nPx = (int)(sizeImage[0] * sizeImage[1]);
const mwSize sizeY[2] = {4*nPx,1};
// Output v1
plhs[0] = mxCreateNumericArray(2, sizeImage, mxDOUBLE_CLASS, mxREAL);
double *Outv1 = mxGetPr(plhs[0]);
// Output v2
plhs[1] = mxCreateNumericArray(2, sizeImage, mxDOUBLE_CLASS, mxREAL);
double *Outv2 = mxGetPr(plhs[1]);
// Output Y
plhs[2] = mxCreateNumericArray(2, sizeY, mxDOUBLE_CLASS, mxREAL);
double *YOut = mxGetPr(plhs[2]);
float* v1 = new float[nPx];
float* v2 = new float[nPx];
float* ux = new float[nPx];
float* uy = new float[nPx];
float* ut = new float[nPx];
float* uxut = new float[nPx];
float* uyut = new float[nPx];
float* c1 = new float[nPx];
float* c2 = new float[nPx];
float* c3 = new float[nPx];
float* teiler = new float[nPx];
float* y11 = new float[nPx];
float* y12 = new float[nPx];
float* y21 = new float[nPx];
float* y22 = new float[nPx];
float* Kty1 = new float[nPx];
float* Kty2 = new float[nPx];
float* Kx11 = new float[nPx];
float* Kx12 = new float[nPx];
float* Kx21 = new float[nPx];
float* Kx22 = new float[nPx];
float tau = 1.0f / sqrt(8.0f);
float sigma = tau;
float dTau = 1.0f / tau;
float dSigma = 1.0f / sigma;
//residuals
float p = 0;
float d = 0;
float err = 1.0;
float ssl = 1.0f - sigma / (sigma + lambda);
int i, j;
#pragma omp parallel for private(i,j)
for (j = 0; j < sizeImage[1]; ++j)
{
for (i = 0; i < sizeImage[0]; ++i)
{
int tmpIndex = index2DtoLinear(sizeImage, i, j);
//Index for gradients
ut[tmpIndex] = (float)(u2[tmpIndex] - u1[tmpIndex]);
if (i>0 && i < sizeImage[0] - 1)
{
uy[tmpIndex] = (float)(0.5f * (u1[index2DtoLinear(sizeImage, i + 1, j)] - u1[index2DtoLinear(sizeImage, i - 1, j)]));
}
else
{
uy[tmpIndex] = 0.0f;
}
if (j>0 && j < sizeImage[1] - 1)
{
ux[tmpIndex] = (float)(0.5f * (u1[index2DtoLinear(sizeImage, i, j + 1)] - u1[index2DtoLinear(sizeImage, i, j - 1)]));
}
else
{
ux[tmpIndex] = 0.0f;
}
uxut[tmpIndex] = ux[tmpIndex] * ut[tmpIndex];
uyut[tmpIndex] = uy[tmpIndex] * ut[tmpIndex];
c1[tmpIndex] = 1.0f + tau * ux[tmpIndex] * ux[tmpIndex];
c2[tmpIndex] = tau * ux[tmpIndex] * uy[tmpIndex];
c3[tmpIndex] = 1.0f + tau * uy[tmpIndex] * uy[tmpIndex];
teiler[tmpIndex] = 1.0f / (c1[tmpIndex] * c3[tmpIndex] - c2[tmpIndex] * c2[tmpIndex]);
if (nrhs > 6)
{
v1[tmpIndex] = (float)inputV[tmpIndex];
v2[tmpIndex] = (float)inputV[nPx + tmpIndex];
}
else
{
v1[tmpIndex] = 0.0f;
v2[tmpIndex] = 0.0f;
}
Kty1[tmpIndex] = 0.0f;
Kty2[tmpIndex] = 0.0f;
if (nrhs > 7)
{
y11[tmpIndex] = (float)inputY[tmpIndex];
y12[tmpIndex] = (float)inputY[nPx + tmpIndex];
y21[tmpIndex] = (float)inputY[2 * nPx + tmpIndex];
y22[tmpIndex] = (float)inputY[3 * nPx + tmpIndex];
}
else
{
y11[tmpIndex] = 0.0f;
y12[tmpIndex] = 0.0f;
y21[tmpIndex] = 0.0f;
y22[tmpIndex] = 0.0f;
}
Kx11[tmpIndex] = 0.0f;
Kx12[tmpIndex] = 0.0f;
Kx21[tmpIndex] = 0.0f;
Kx22[tmpIndex] = 0.0f;
}
}
int iterations = 0;
while (err > tol && iterations <= maxIterations)
{
++iterations;
if (iterations % 50 == 0)
{
p = 0.0f;
d = 0.0f;
}
//primal step
#pragma omp parallel for reduction(+:p) private(i,j)
for (j = 0; j < sizeImage[1]; ++j)
{
for (i = 0; i < sizeImage[0]; ++i)
{
int tmpIndex = index2DtoLinear(sizeImage, i, j);
float Kty1Old = Kty1[tmpIndex];
float Kty2Old = Kty2[tmpIndex];
//transpose equals -div
Kty1[tmpIndex] = -(dxm(y11, sizeImage, i, j) + dym(y12, sizeImage, i, j));
Kty2[tmpIndex] = -(dxm(y21, sizeImage, i, j) + dym(y22, sizeImage, i, j));
float b1 = v1[tmpIndex] - tau*(Kty1[tmpIndex] + uxut[tmpIndex]);
float b2 = v2[tmpIndex] - tau*(Kty2[tmpIndex] + uyut[tmpIndex]);
float v1Old = v1[tmpIndex];
float v2Old = v2[tmpIndex];
v1[tmpIndex] = (b1 * c3[tmpIndex] - c2[tmpIndex] * b2) * teiler[tmpIndex];
v2[tmpIndex] = (b2 * c1[tmpIndex] - c2[tmpIndex] * b1) * teiler[tmpIndex];
if (iterations % 50 == 0)
{
//residuals
p += myAbs((v1Old - v1[tmpIndex]) * dTau - Kty1Old + Kty1[tmpIndex])
+ myAbs((v2Old - v2[tmpIndex]) * dTau - Kty2Old + Kty2[tmpIndex]);
}
}
}
//dual step
#pragma omp parallel for reduction(+:d) private(i,j)
for (j = 0; j < sizeImage[1]; ++j)
{
for (i = 0; i < sizeImage[0]; ++i)
{
int tmpIndex = index2DtoLinear(sizeImage, i, j);
float Kx11Old = Kx11[tmpIndex];
float Kx12Old = Kx12[tmpIndex];
float Kx21Old = Kx21[tmpIndex];
float Kx22Old = Kx22[tmpIndex];
Kx11[tmpIndex] = dxp(v1, sizeImage, i, j);
Kx12[tmpIndex] = dyp(v1, sizeImage, i, j);
Kx21[tmpIndex] = dxp(v2, sizeImage, i, j);
Kx22[tmpIndex] = dyp(v2, sizeImage, i, j);
float y11Old = y11[tmpIndex];
float y12Old = y12[tmpIndex];
float y21Old = y21[tmpIndex];
float y22Old = y22[tmpIndex];
y11[tmpIndex] = ssl * (y11[tmpIndex] + sigma*(2 * Kx11[tmpIndex] - Kx11Old));
y12[tmpIndex] = ssl * (y12[tmpIndex] + sigma*(2 * Kx12[tmpIndex] - Kx12Old));
y21[tmpIndex] = ssl * (y21[tmpIndex] + sigma*(2 * Kx21[tmpIndex] - Kx21Old));
y22[tmpIndex] = ssl * (y22[tmpIndex] + sigma*(2 * Kx22[tmpIndex] - Kx22Old));
if (iterations % 50 == 0)
{
d += myAbs((y11Old - y11[tmpIndex]) * dSigma - Kx11Old + Kx11[tmpIndex]) +
myAbs((y12Old - y12[tmpIndex]) * dSigma - Kx12Old + Kx12[tmpIndex]) +
myAbs((y21Old - y21[tmpIndex]) * dSigma - Kx21Old + Kx21[tmpIndex]) +
myAbs((y22Old - y22[tmpIndex]) * dSigma - Kx22Old + Kx22[tmpIndex]);
}
}
}
if (iterations % 50 == 0)
{
err = (d*d + p*p) / nPx;
}
if (iterations % 1000 == 0)
{
mexPrintf("Iteration %d,Residual %e\n", iterations, err);
mexEvalString("drawnow;");
}
}
//write output
#pragma omp parallel for private(i,j)
for (j = 0; j < sizeImage[1]; ++j)
{
for (i = 0; i < sizeImage[0]; ++i)
{
int tmpIndex = index2DtoLinear(sizeImage, i, j);
YOut[tmpIndex] = (double)y11[tmpIndex];
YOut[tmpIndex + nPx] = (double)y12[tmpIndex];
YOut[tmpIndex + 2 * nPx] = (double)y21[tmpIndex];
YOut[tmpIndex + 3 * nPx] = (double)y22[tmpIndex];
Outv1[tmpIndex] = (double)v1[tmpIndex];
Outv2[tmpIndex] = (double)v2[tmpIndex];
}
}
delete[] v1;
delete[] v2;
delete[] ux;
delete[] uy;
delete[] ut;
delete[] uxut;
delete[] uyut;
delete[] c1;
delete[] c2;
delete[] c3;
delete[] teiler;
delete[] y11;
delete[] y12;
delete[] y21;
delete[] y22;
delete[] Kty1;
delete[] Kty2;
delete[] Kx11;
delete[] Kx12;
delete[] Kx21;
delete[] Kx22;
}
void applyRotation(int actuator) {
float t = getLocalTime();
//int pos = dynamixelApi_getPositionEstimate(actuator, rotations[actuator].dir);
int pos = dynamixelApi_getPosition(actuator);
float f = rotations[actuator].frequency;
float pd = rotations[actuator].phaseDiff/6.28f; //0-1
float dt = controllers[actuator].lastUpdateTime-t;
//if(actuator==1) printf("%f sec\n",t);
float percentPos;
percentPos = f*t+pd;
int goalPos = ((int)(percentPos*DYNAMIXEL_MAX_POS_FULL))%DYNAMIXEL_MAX_POS_FULL;
if(rotations[actuator].dir) goalPos = DYNAMIXEL_MAX_POS_FULL-goalPos;
if(goalPos>DYNAMIXEL_MAX_POS_VALID) goalPos =DYNAMIXEL_MAX_POS_VALID;
if(goalPos<0) goalPos = 0;
int error = (goalPos-pos);
int speed;
bool atValidAngle = dynamixelApi_isAtValidAngle(actuator, rotations[actuator].dir);
if(atValidAngle) {
if(rotations[actuator].dir == true) {
if(error>512) {
error = error-DYNAMIXEL_MAX_POS_VALID;
}
if(error<0 && error>-512) {
controllers[actuator].integral = controllers[actuator].integral + myAbs(error)*dt;
float derivative = (myAbs(error) - controllers[actuator].previousError)/dt;
speed = controllers[actuator].P*myAbs(error) + controllers[actuator].I*controllers[actuator].integral + controllers[actuator].D*derivative;
speed += controllers[actuator].avrSpeed;
controllers[actuator].previousError = myAbs(error);
}
else {
speed = -controllers[actuator].P*myAbs(error);
speed += controllers[actuator].avrSpeed;
}
}
else if(rotations[actuator].dir == false) {
if(error<-512) {
error = error+DYNAMIXEL_MAX_POS_VALID;
}
if(error>0&&error<512) {
controllers[actuator].integral = controllers[actuator].integral + myAbs(error)*dt;
float derivative = (myAbs(error) - controllers[actuator].previousError)/dt;
speed = controllers[actuator].P*myAbs(error) + controllers[actuator].I*controllers[actuator].integral + controllers[actuator].D*derivative;
speed += controllers[actuator].avrSpeed;
controllers[actuator].previousError = myAbs(error);
}
else {
speed = -controllers[actuator].P*myAbs(error);
speed += controllers[actuator].avrSpeed;
}
}
if(speed>1023) speed = 1023;
if(speed<=150) speed = 150;
controllers[actuator].previousError = myAbs(error);
rotations[actuator].speed = speed;
rotations[actuator].changed = true;
}
else {
speed = controllers[actuator].avrSpeed;// controllers[actuator].avrSpeed;
if(speed<150) {
speed = 150;
}
controllers[actuator].integral = 0;
controllers[actuator].previousError = 0;
rotations[actuator].speed = speed;
rotations[actuator].changed = true;
error = 0;
}
controllers[actuator].lastUpdateTime = t;
if(!rotations[actuator].pause) {
controllers[actuator].avrSpeed = (1.0-0.05)*controllers[actuator].avrSpeed+0.05*speed;
if(rotations[actuator].changed ) {
dynamixelApi_wheelMove(actuator, rotations[actuator].speed, rotations[actuator].dir);
rotations[actuator].changed = false;
}
}
else {
float periodeTime = 1.0f/rotations[actuator].frequency;
if(t-rotations[actuator].pauseStart > periodeTime) {
rotations[actuator].pause = false;
}
dynamixelApi_wheelMove(actuator, 0, rotations[actuator].dir);
}
int loopTimeMs=1000*(getLocalTime()-t);
if(actuator==0||actuator==0) {
//printf("%i: avr speed = %f, error = %i\n",actuator,controllers[actuator].avrSpeed, error);
}
if(loopTimeMs>20) {
printf("%i: %i ms loop time (warning)\n",actuator,loopTimeMs);
}
//printf("{%i, %li, %i, %i, %i, %i, %i},\n",actuator,getLocalMsTime(),dynamixelApi_getPosition(actuator),dynamixelApi_getPositionEstimate(actuator,rotations[actuator].dir),goalPos,error,speed);
//fprintf(posLog, "%i %li %i %i %i %i %i %i %f\n",actuator,getLocalMsTime(),dynamixelApi_getPosition(actuator),dynamixelApi_getPositionEstimate(actuator,rotations[actuator].dir),goalPos,error,speed,1023*(int)atValidAngle,controllers[actuator].avrSpeed);
//fflush(posLog);
//ase_printf("%i: Dynamixel at %i \n", i, pos);
}
double calculate(int numInputTokens, char **inputString)
{
int i;
double result = 0.0;
char *s;
struct DynArr *stack;
//set up the stack
stack = createDynArr(20);
// start at 1 to skip the name of the calculator calc
for(i=1;i < numInputTokens;i++)
{
s = inputString[i];
// Hint: General algorithm:
// (1) Check if the string s is in the list of operators.
// (1a) If it is, perform corresponding operations.
// (1b) Otherwise, check if s is a number.
// (1b - I) If s is not a number, produce an error.
// (1b - II) If s is a number, push it onto the stack
if(strcmp(s, "+") == 0)
add(stack);
else if(strcmp(s,"-") == 0)
subtract(stack);
else if(strcmp(s, "/") == 0)
divide(stack);
else if(strcmp(s, "x") == 0)
time(stack);
else if(strcmp(s, "^") == 0)
power(stack);
else if(strcmp(s, "^2") == 0)
/* FIXME: replace printf with your own function */
squaring(stack);
else if(strcmp(s, "^3") == 0)
/* FIXME: replace printf with your own function */
cubing(stack);
else if(strcmp(s, "abs") == 0)
/* FIXME: replace printf with your own function */
myAbs(stack);
else if(strcmp(s, "sqrt") == 0)
/* FIXME: replace printf with your own function */
squareRoot(stack);
else if(strcmp(s, "exp") == 0)
/* FIXME: replace printf with your own function */
exponential(stack);
else if(strcmp(s, "ln") == 0)
/* FIXME: replace printf with your own function */
naturalLog(stack);
else if(strcmp(s, "log") == 0)
/* FIXME: replace printf with your own function */
baseLog(stack);
else
{
// FIXME: You need to develop the code here (when s is not an operator)
// Remember to deal with special values ("pi" and "e")
double *tmp = (double *)malloc(sizeof(double));
if (isNumber(s, tmp))
pushDynArr(stack, *tmp);
else if(strcmp(s, "pi") == 0)
pushDynArr(stack, 3.14159265);
else if(strcmp(s, "e") == 0)
pushDynArr(stack, 2.7182818);
else
flag = 0;
}
} //end for
/* FIXME: You will write this part of the function (2 steps below)
* (1) Check if everything looks OK and produce an error if needed.
* (2) Store the final value in result and print it out.
*/
if (sizeDynArr(stack) != 1)
printf("There is an error in your input !\n");
else {
if (flag) {
result = topDynArr(stack);
popDynArr(stack);
printf("The result is : %f\n", result);
}
else
printf("There is an error in your input !\n");
}
return result;
}
