omni wheel library
Dependents: quadOmni_yanagi NHK2017_octopus hayatoShooter
オムニ用のライブラリです。
Diff: omni.cpp
- Revision:
- 6:59171f3235b6
- Parent:
- 5:f2fc4a0b6a6f
- Child:
- 7:1d0aa4275eb0
diff -r f2fc4a0b6a6f -r 59171f3235b6 omni.cpp --- a/omni.cpp Wed Jul 05 08:34:43 2017 +0000 +++ b/omni.cpp Wed Jul 05 08:55:53 2017 +0000 @@ -55,15 +55,15 @@ } double parallel[4] = {0}; - double parallelMax = 0; - double parallelMin = 0; + double parallelMax = -1; + double parallelMin = 1; for(int i = 0; i < wheels; i++) { parallel[i] = sin(toRadians(parallelVector[0] + initDegree + (360 / wheels) * i)) * parallelVector[1]; } - for(int i = 0; i < wheels - 1; i++) { - parallelMax = fmax(parallel[i], parallel[i + 1]); - parallelMin = fmin(parallel[i], parallel[i + 1]); + for(int i = 0; i < wheels; i++) { + parallelMax = fmax(parallel[i], parallelMax); + parallelMin = fmin(parallel[i], parallelMin); } if(parallelMax + moment > 1.0 || parallelMin + moment < -1.0) { for(int i = 0; i < wheels; i++) {