File content as of revision 1:1b01dbd85589:

#include "gyro.h"

gyro::gyro(int num)
{
    wheelRad = new double[num];
}

void gyro::setRad(int num,double rad)
{
    wheelRad[num] = rad;
}


double gyro::hMove(double way, int num)
{
    delete[] wheelRad;
    if(way == 10) {return 0;}

    return sin(way - wheelRad[num]);
}

void gyro::setIdeal(double angle)
{
    ideal = angle;
}

void gyro::setErrorRange(double range)
{
    errorRange = range;
}

double gyro::cAngle(double angle)
{
    toChange = 180 - ideal;
    changed = angle + toChange;
    if (changed > 180)
    {
        changed -= 360;
    }
    if(fabs(changed) < (180 - errorRange))
    {
        if(changed >= 0)
        {
            if(180 - changed >= 25)
            {
                return -0.2;
            }
            return -0.1;
        }
        else
        {
            if(180 + changed >= 25)
            {
                return 0.2;
            }
            return 0.1;
        }
    }
    return 0;
}

void gyro::limitPower(double limit)
{
    absLimit = limit / 180;
    _limit = limit;
}

double gyro::pAngle(double angle, double k)
{
    toChange =  -1 * ideal;
    changed = angle + toChange;
    if (changed > 180)
    {
        changed -= 360;
    }
    if(changed < -180)
    {
        changed += 360;
    }
    rVal = k * changed * absLimit;
    if((int)(angled + 0.5) == (int)(angle + 0.5))
    {
        rVal += redVal;
    }
    if(rVal < (-1 * _limit))
    {
        rVal = -1 * _limit;
    }
    else if(rVal > _limit)
    {
        rVal = _limit;
    }
    redVal = rVal;
    angled = angle;
    return rVal;
}

double gyro::lFunc(double num,double gain)
{
    rFunc = num * gain;
    if(rFunc >= 1)
    {
        rFunc = 1;
    }
    return rFunc;
}