gyro.cpp

00001 #include "gyro.h"
00002 
00003 gyro::gyro(int num)
00004 {
00005     wheelRad = new double[num];
00006 }
00007 
00008 void gyro::setRad(int num,double rad)
00009 {
00010     wheelRad[num] = rad;
00011 }
00012 
00013 
00014 double gyro::hMove(double way, int num)
00015 {
00016     delete[] wheelRad;
00017     if(way == 10) {return 0;}
00018 
00019     return sin(way - wheelRad[num]);
00020 }
00021 
00022 void gyro::setIdeal(double angle)
00023 {
00024     ideal = angle;
00025 }
00026 
00027 void gyro::setErrorRange(double range)
00028 {
00029     errorRange = range;
00030 }
00031 
00032 double gyro::cAngle(double angle)
00033 {
00034     toChange = 180 - ideal;
00035     changed = angle + toChange;
00036     if (changed > 180)
00037     {
00038         changed -= 360;
00039     }
00040     if(fabs(changed) < (180 - errorRange))
00041     {
00042         if(changed >= 0)
00043         {
00044             if(180 - changed >= 25)
00045             {
00046                 return -0.2;
00047             }
00048             return -0.1;
00049         }
00050         else
00051         {
00052             if(180 + changed >= 25)
00053             {
00054                 return 0.2;
00055             }
00056             return 0.1;
00057         }
00058     }
00059     return 0;
00060 }
00061 
00062 void gyro::limitPower(double limit)
00063 {
00064     absLimit = limit / 180;
00065     _limit = limit;
00066 }
00067 
00068 double gyro::pAngle(double angle, double k)
00069 {
00070     toChange =  -1 * ideal;
00071     changed = angle + toChange;
00072     if (changed > 180)
00073     {
00074         changed -= 360;
00075     }
00076     if(changed < -180)
00077     {
00078         changed += 360;
00079     }
00080     rVal = k * changed * absLimit;
00081     if((int)(angled + 0.5) == (int)(angle + 0.5))
00082     {
00083         rVal += redVal;
00084     }
00085     if(rVal < (-1 * _limit))
00086     {
00087         rVal = -1 * _limit;
00088     }
00089     else if(rVal > _limit)
00090     {
00091         rVal = _limit;
00092     }
00093     redVal = rVal;
00094     angled = angle;
00095     return rVal;
00096 }
00097 
00098 double gyro::lFunc(double num,double gain)
00099 {
00100     rFunc = num * gain;
00101     if(rFunc >= 1)
00102     {
00103         rFunc = 1;
00104     }
00105     return rFunc;
00106 }