hirromu takaki / gyro

hMove and cAngle

Dependents:   moveTest

gyro.cpp@0:6371d70a58f2, 2019-06-10 (annotated)

Committer:
THtakahiro702286
Date:
Mon Jun 10 14:23:00 2019 +0000
Revision:
0:6371d70a58f2
Child:
1:1b01dbd85589
hMove and cAngle

Who changed what in which revision?

UserRevisionLine numberNew contents of line
THtakahiro702286 0:6371d70a58f2 1 #include "gyro.h"
THtakahiro702286 0:6371d70a58f2 2
THtakahiro702286 0:6371d70a58f2 3 gyro::gyro(int num)
THtakahiro702286 0:6371d70a58f2 4 {
THtakahiro702286 0:6371d70a58f2 5 wheelRad = new double[num];
THtakahiro702286 0:6371d70a58f2 6 }
THtakahiro702286 0:6371d70a58f2 7
THtakahiro702286 0:6371d70a58f2 8 void gyro::setRad(int num,double rad)
THtakahiro702286 0:6371d70a58f2 9 {
THtakahiro702286 0:6371d70a58f2 10 wheelRad[num] = rad;
THtakahiro702286 0:6371d70a58f2 11 }
THtakahiro702286 0:6371d70a58f2 12
THtakahiro702286 0:6371d70a58f2 13 double gyro::hMove(double way, int num)
THtakahiro702286 0:6371d70a58f2 14 {
THtakahiro702286 0:6371d70a58f2 15 delete[] wheelRad;
THtakahiro702286 0:6371d70a58f2 16 if(way == 10) {return 0;}
THtakahiro702286 0:6371d70a58f2 17
THtakahiro702286 0:6371d70a58f2 18 return sin(way - wheelRad[num]);
THtakahiro702286 0:6371d70a58f2 19 }
THtakahiro702286 0:6371d70a58f2 20
THtakahiro702286 0:6371d70a58f2 21 void gyro::setIdeal(double angle)
THtakahiro702286 0:6371d70a58f2 22 {
THtakahiro702286 0:6371d70a58f2 23 ideal = angle;
THtakahiro702286 0:6371d70a58f2 24 }
THtakahiro702286 0:6371d70a58f2 25
THtakahiro702286 0:6371d70a58f2 26 double gyro::cAngle(double angle)
THtakahiro702286 0:6371d70a58f2 27 {
THtakahiro702286 0:6371d70a58f2 28 toChange = 180 - ideal;
THtakahiro702286 0:6371d70a58f2 29 changed = angle + toChange;
THtakahiro702286 0:6371d70a58f2 30 if (changed > 180)
THtakahiro702286 0:6371d70a58f2 31 {
THtakahiro702286 0:6371d70a58f2 32 changed -= 360;
THtakahiro702286 0:6371d70a58f2 33 }
THtakahiro702286 0:6371d70a58f2 34 if(fabs(changed) < 179)
THtakahiro702286 0:6371d70a58f2 35 {
THtakahiro702286 0:6371d70a58f2 36 if(changed >= 0)
THtakahiro702286 0:6371d70a58f2 37 {
THtakahiro702286 0:6371d70a58f2 38 if(180 - changed >= 25)
THtakahiro702286 0:6371d70a58f2 39 {
THtakahiro702286 0:6371d70a58f2 40 return -0.2;
THtakahiro702286 0:6371d70a58f2 41 }
THtakahiro702286 0:6371d70a58f2 42 return -0.1;
THtakahiro702286 0:6371d70a58f2 43 }
THtakahiro702286 0:6371d70a58f2 44 else
THtakahiro702286 0:6371d70a58f2 45 {
THtakahiro702286 0:6371d70a58f2 46 if(180 + changed >= 25)
THtakahiro702286 0:6371d70a58f2 47 {
THtakahiro702286 0:6371d70a58f2 48 return 0.2;
THtakahiro702286 0:6371d70a58f2 49 }
THtakahiro702286 0:6371d70a58f2 50 return 0.1;
THtakahiro702286 0:6371d70a58f2 51 }
THtakahiro702286 0:6371d70a58f2 52 }
THtakahiro702286 0:6371d70a58f2 53 return 0;
THtakahiro702286 0:6371d70a58f2 54 }