robot

Dependencies:   FastPWM3 mbed

MathHelpers/MathHelpers.cpp

Committer:
bwang
Date:
2017-05-04
Revision:
155:7c6005933d4c
Parent:
64:b4175385d718
Child:
82:5e741c5ffd9f
Child:
159:9dbc0657238c

File content as of revision 155:7c6005933d4c:

#include "MathHelpers.h"
#include "math.h"

float map(float x, float in_min, float in_max, float out_min, float out_max)
{
    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

float constrain(float in, float min, float max)
{
    if(in > max) return max;
    if(in < min) return min;
    return in;
}

float constrain_norm(float *x, float *y, float coeffx, float coeffy, float max) {
    float norm = sqrtf(coeffx * *x * *x + coeffy * *y * *y);
    if (norm > max) {
        *x /= norm;
        *y /= norm;
    }
    return norm;
}

float fminf(float a, float b) {
    if(a < b) return a;
    return b;
}

float fmaxf(float a, float b) {
    if(a > b) return a;
    return b;
}

float acbrt(float x0) {
    union { int ix; float x; };

    x = x0;
    ix = 0x2a5137a0 + ix / 3;
    x = 0.33333333f*(2.0f*x + x0 / (x * x));

    return x;
}

float aatan2(float q, float d) {
    if (q > 0.f && d < 0.f && -d < q) {
        return d * q / (q * q + 0.28125f * d * d);;
    }
    else if (q > 0.f && d < 0.f && -d > q) {
        return -M_PI / 2.f - d * q / (d * d + 0.28125f * q * q);;
    }
    else if (q < 0.f && d < 0.f && -d > -q) {
        return M_PI / 2.f - d * q / (d * d + 0.28125f * q * q);
    }
    else if (q < 0.f && d < 0.f && -d < -q) {
        return d * q / (q * q + 0.28125f * d * d);;
    }
    else if (q < 0.f && d > 0.f && d < -q) {
        return d * q / (q * q + 0.28125f * d * d);;
    }
    else if (q < 0.f && d > 0.f && d > -q) {
        return -M_PI / 2.f - d * q / (d * d + 0.28125f * q * q);;
    }
    else if (q > 0.f && d > 0.f && d > q) {
        return M_PI / 2.f - d * q / (d * d + 0.28125f * q * q);;
    }
    else {
        return d * q / (q * q + 0.28125f * d * d);;
    }
}