NagaokaRoboticsClub_mbedTeam / omni

omni wheel library

Dependents:   quadOmni_yanagi NHK2017_octopus hayatoShooter

オムニ用のライブラリです。

omni.cpp@6:59171f3235b6, 2017-07-05 (annotated)

Committer:
UCHITAKE
Date:
Wed Jul 05 08:55:53 2017 +0000
Revision:
6:59171f3235b6
Parent:
5:f2fc4a0b6a6f
Child:
7:1d0aa4275eb0
fix bug

Who changed what in which revision?

UserRevisionLine numberNew contents of line
UCHITAKE 0:979565e955a5 1 #include "omni.h"
UCHITAKE 0:979565e955a5 2
UCHITAKE 0:979565e955a5 3 inline double fmax(double a, double b)
UCHITAKE 0:979565e955a5 4 {
UCHITAKE 0:979565e955a5 5 if(a > b) {
UCHITAKE 0:979565e955a5 6 return a;
UCHITAKE 0:979565e955a5 7 } else {
UCHITAKE 0:979565e955a5 8 return b;
UCHITAKE 0:979565e955a5 9 }
UCHITAKE 0:979565e955a5 10 }
UCHITAKE 0:979565e955a5 11
UCHITAKE 0:979565e955a5 12 inline double fmin(double a, double b)
UCHITAKE 0:979565e955a5 13 {
UCHITAKE 0:979565e955a5 14 if(a < b) {
UCHITAKE 0:979565e955a5 15 return a;
UCHITAKE 0:979565e955a5 16 } else {
UCHITAKE 0:979565e955a5 17 return b;
UCHITAKE 0:979565e955a5 18 }
UCHITAKE 0:979565e955a5 19 }
UCHITAKE 0:979565e955a5 20
UCHITAKE 0:979565e955a5 21 inline double toRadians(double degrees) {
UCHITAKE 0:979565e955a5 22 return degrees * (M_PI / 180.0);
UCHITAKE 0:979565e955a5 23 }
UCHITAKE 0:979565e955a5 24
UCHITAKE 0:979565e955a5 25 Omni::Omni(int wheels, double initDegree) :
UCHITAKE 0:979565e955a5 26 wheels(wheels), initDegree(initDegree)
UCHITAKE 0:979565e955a5 27 {
UCHITAKE 0:979565e955a5 28 for(int i = 0; i < wheels; i++) {
UCHITAKE 0:979565e955a5 29 wheel[i] = 0;
UCHITAKE 0:979565e955a5 30 }
UCHITAKE 0:979565e955a5 31 }
UCHITAKE 0:979565e955a5 32
UCHITAKE 0:979565e955a5 33 bool Omni::computeXY(double parallelVector[], double moment)
UCHITAKE 0:979565e955a5 34 {
UCHITAKE 0:979565e955a5 35 if(wheels < 3 || wheels > 4) {
UCHITAKE 0:979565e955a5 36 return 0;
UCHITAKE 0:979565e955a5 37 }
UCHITAKE 0:979565e955a5 38
UCHITAKE 0:979565e955a5 39 double polar[2];
UCHITAKE 0:979565e955a5 40
UCHITAKE 0:979565e955a5 41 polar[0] = atan2(parallelVector[1], parallelVector[0]);
UCHITAKE 0:979565e955a5 42 polar[1] = hypot(parallelVector[0], parallelVector[1]);
UCHITAKE 0:979565e955a5 43
UCHITAKE 0:979565e955a5 44 return computePolar(polar, moment);
UCHITAKE 0:979565e955a5 45 }
UCHITAKE 0:979565e955a5 46
UCHITAKE 0:979565e955a5 47 bool Omni::computePolar(double parallelVector[], double moment)
UCHITAKE 0:979565e955a5 48 {
UCHITAKE 0:979565e955a5 49 if(wheels < 3 || wheels > 4) {
UCHITAKE 0:979565e955a5 50 return 0;
UCHITAKE 0:979565e955a5 51 }
UCHITAKE 0:979565e955a5 52
UCHITAKE 0:979565e955a5 53 if(parallelVector[1] < 0 || parallelVector[1] > 1.0) {
UCHITAKE 0:979565e955a5 54 return 0;
UCHITAKE 0:979565e955a5 55 }
UCHITAKE 0:979565e955a5 56
UCHITAKE 0:979565e955a5 57 double parallel[4] = {0};
UCHITAKE 6:59171f3235b6 58 double parallelMax = -1;
UCHITAKE 6:59171f3235b6 59 double parallelMin = 1;
UCHITAKE 0:979565e955a5 60
UCHITAKE 0:979565e955a5 61 for(int i = 0; i < wheels; i++) {
UCHITAKE 5:f2fc4a0b6a6f 62 parallel[i] = sin(toRadians(parallelVector[0] + initDegree + (360 / wheels) * i)) * parallelVector[1];
UCHITAKE 0:979565e955a5 63 }
UCHITAKE 6:59171f3235b6 64 for(int i = 0; i < wheels; i++) {
UCHITAKE 6:59171f3235b6 65 parallelMax = fmax(parallel[i], parallelMax);
UCHITAKE 6:59171f3235b6 66 parallelMin = fmin(parallel[i], parallelMin);
UCHITAKE 0:979565e955a5 67 }
UCHITAKE 0:979565e955a5 68 if(parallelMax + moment > 1.0 || parallelMin + moment < -1.0) {
UCHITAKE 0:979565e955a5 69 for(int i = 0; i < wheels; i++) {
UCHITAKE 0:979565e955a5 70 parallel[i] *= (1.0 - fabs(moment));
UCHITAKE 0:979565e955a5 71 }
UCHITAKE 0:979565e955a5 72 }
UCHITAKE 0:979565e955a5 73 for(int i = 0; i < wheels; i++) {
UCHITAKE 0:979565e955a5 74 wheel[i] = parallel[i] + moment;
UCHITAKE 0:979565e955a5 75 }
UCHITAKE 0:979565e955a5 76 return 1;
UCHITAKE 0:979565e955a5 77 }
UCHITAKE 0:979565e955a5 78
UCHITAKE 0:979565e955a5 79 bool Omni::stop()
UCHITAKE 0:979565e955a5 80 {
UCHITAKE 0:979565e955a5 81 for(int i = 0; i < wheels; i++) {
UCHITAKE 0:979565e955a5 82 wheel[i] = 0;
UCHITAKE 0:979565e955a5 83 }
UCHITAKE 0:979565e955a5 84 return 1;
UCHITAKE 0:979565e955a5 85 }
UCHITAKE 0:979565e955a5 86
UCHITAKE 0:979565e955a5 87 void Omni::setWheels(int wheel)
UCHITAKE 0:979565e955a5 88 {
UCHITAKE 0:979565e955a5 89 wheels = wheel;
UCHITAKE 0:979565e955a5 90 }
UCHITAKE 0:979565e955a5 91
UCHITAKE 0:979565e955a5 92 void Omni::setInitDegree(double degree)
UCHITAKE 0:979565e955a5 93 {
UCHITAKE 0:979565e955a5 94 initDegree = degree;
UCHITAKE 0:979565e955a5 95 }
UCHITAKE 0:979565e955a5 96
UCHITAKE 0:979565e955a5 97 double Omni::getOutput(int num)
UCHITAKE 0:979565e955a5 98 {
UCHITAKE 0:979565e955a5 99 return wheel[num];
UCHITAKE 0:979565e955a5 100 }