hMove and cAngle
Diff: gyro.cpp
- Revision:
- 0:6371d70a58f2
- Child:
- 1:1b01dbd85589
diff -r 000000000000 -r 6371d70a58f2 gyro.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/gyro.cpp Mon Jun 10 14:23:00 2019 +0000 @@ -0,0 +1,54 @@ +#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; +} + +double gyro::cAngle(double angle) +{ + toChange = 180 - ideal; + changed = angle + toChange; + if (changed > 180) + { + changed -= 360; + } + if(fabs(changed) < 179) + { + 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; +}