NagaokaRoboticsClub_mbedTeam / omni

omni wheel library

Dependents:   quadOmni_yanagi NHK2017_octopus hayatoShooter

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

omni.cpp@9:a30169882a15, 2017-08-23 (annotated)

Committer:
UCHITAKE
Date:
Wed Aug 23 07:24:34 2017 +0000
Revision:
9:a30169882a15
Parent:
0:979565e955a5 
new version; plz read doc

Who changed what in which revision?

UserRevisionLine numberNew contents of line
UCHITAKE 0:979565e955a5 1 #include "omni.h"
UCHITAKE 0:979565e955a5 2
UCHITAKE 9:a30169882a15 3 Omni::Omni(int wheels) : wheels(wheels)
UCHITAKE 9:a30169882a15 4 {
UCHITAKE 9:a30169882a15 5 for(int i = 0; i < wheels; i++) {
UCHITAKE 9:a30169882a15 6 wheel[i] = 0;
UCHITAKE 9:a30169882a15 7 radian[i] = 0;
UCHITAKE 9:a30169882a15 8 }
UCHITAKE 9:a30169882a15 9 }
UCHITAKE 9:a30169882a15 10
UCHITAKE 0:979565e955a5 11 inline double fmax(double a, double b)
UCHITAKE 0:979565e955a5 12 {
UCHITAKE 0:979565e955a5 13 if(a > b) {
UCHITAKE 0:979565e955a5 14 return a;
UCHITAKE 0:979565e955a5 15 } else {
UCHITAKE 0:979565e955a5 16 return b;
UCHITAKE 0:979565e955a5 17 }
UCHITAKE 0:979565e955a5 18 }
UCHITAKE 0:979565e955a5 19
UCHITAKE 0:979565e955a5 20 inline double fmin(double a, double b)
UCHITAKE 0:979565e955a5 21 {
UCHITAKE 0:979565e955a5 22 if(a < b) {
UCHITAKE 0:979565e955a5 23 return a;
UCHITAKE 0:979565e955a5 24 } else {
UCHITAKE 0:979565e955a5 25 return b;
UCHITAKE 0:979565e955a5 26 }
UCHITAKE 0:979565e955a5 27 }
UCHITAKE 0:979565e955a5 28
UCHITAKE 9:a30169882a15 29 bool isInRange(float X, float Y)
UCHITAKE 9:a30169882a15 30 {
UCHITAKE 9:a30169882a15 31 if(X < -1 || X > 1 || Y < -1 || Y > 1) return 0;
UCHITAKE 9:a30169882a15 32 if(hypot(X, Y) > 1.0) return 0;
UCHITAKE 9:a30169882a15 33 return 1;
UCHITAKE 9:a30169882a15 34 }
UCHITAKE 9:a30169882a15 35
UCHITAKE 9:a30169882a15 36 bool isInRange(float r)
UCHITAKE 9:a30169882a15 37 {
UCHITAKE 9:a30169882a15 38 if(r < 0 || r > 1) return 0;
UCHITAKE 9:a30169882a15 39 return 1;
UCHITAKE 9:a30169882a15 40 }
UCHITAKE 9:a30169882a15 41
UCHITAKE 9:a30169882a15 42
UCHITAKE 9:a30169882a15 43 bool Omni::setWheelRadian(float rad1, float rad2, float rad3)
UCHITAKE 9:a30169882a15 44 {
UCHITAKE 9:a30169882a15 45 if(wheels != 3) return 0;
UCHITAKE 9:a30169882a15 46 radian[0] = rad1;
UCHITAKE 9:a30169882a15 47 radian[1] = rad2;
UCHITAKE 9:a30169882a15 48 radian[2] = rad3;
UCHITAKE 9:a30169882a15 49 return 1;
UCHITAKE 0:979565e955a5 50 }
UCHITAKE 0:979565e955a5 51
UCHITAKE 9:a30169882a15 52 bool Omni::setWheelRadian(float rad1, float rad2, float rad3, float rad4)
UCHITAKE 9:a30169882a15 53 {
UCHITAKE 9:a30169882a15 54 if(wheels != 4) return 0;
UCHITAKE 9:a30169882a15 55 radian[0] = rad1;
UCHITAKE 9:a30169882a15 56 radian[1] = rad2;
UCHITAKE 9:a30169882a15 57 radian[2] = rad3;
UCHITAKE 9:a30169882a15 58 radian[3] = rad4;
UCHITAKE 9:a30169882a15 59 return 1;
UCHITAKE 9:a30169882a15 60 }
UCHITAKE 9:a30169882a15 61
UCHITAKE 9:a30169882a15 62 bool Omni::setWheelRadian(int wheelNumber, float rad)
UCHITAKE 0:979565e955a5 63 {
UCHITAKE 9:a30169882a15 64 if(wheelNumber > 3|| wheelNumber <= 0) return 0;
UCHITAKE 9:a30169882a15 65 radian[wheelNumber] = rad;
UCHITAKE 9:a30169882a15 66 return 1;
UCHITAKE 9:a30169882a15 67 }
UCHITAKE 9:a30169882a15 68
UCHITAKE 9:a30169882a15 69 bool Omni::computeXY(float X, float Y, float moment)
UCHITAKE 9:a30169882a15 70 {
UCHITAKE 9:a30169882a15 71 if(isInRange(X, Y)) {
UCHITAKE 9:a30169882a15 72 return computeCircular(hypot(X, Y), atan2(Y, X), moment);
UCHITAKE 9:a30169882a15 73 } else {
UCHITAKE 9:a30169882a15 74 return 0;
UCHITAKE 0:979565e955a5 75 }
UCHITAKE 0:979565e955a5 76 }
UCHITAKE 0:979565e955a5 77
UCHITAKE 9:a30169882a15 78
UCHITAKE 9:a30169882a15 79 bool Omni::computeCircular(float r, float rad, float moment)
UCHITAKE 0:979565e955a5 80 {
UCHITAKE 9:a30169882a15 81 if(wheels <= 0) return 0;
UCHITAKE 9:a30169882a15 82 if(isInRange(r)) {
UCHITAKE 9:a30169882a15 83 double parallel[4] = {0};
UCHITAKE 9:a30169882a15 84 double parallelMax = -1;
UCHITAKE 9:a30169882a15 85 double parallelMin = 1;
UCHITAKE 0:979565e955a5 86
UCHITAKE 9:a30169882a15 87 for(int i = 0; i < wheels; i++) {
UCHITAKE 9:a30169882a15 88 parallel[i] = sin(rad + radian[i]) * r;
UCHITAKE 9:a30169882a15 89 }
UCHITAKE 9:a30169882a15 90 for(int i = 0; i < wheels; i++) {
UCHITAKE 9:a30169882a15 91 parallelMax = fmax(parallel[i], parallelMax);
UCHITAKE 9:a30169882a15 92 parallelMin = fmin(parallel[i], parallelMin);
UCHITAKE 9:a30169882a15 93 }
UCHITAKE 9:a30169882a15 94 if(parallelMax + moment > 1.0 || parallelMin + moment < -1.0) {
UCHITAKE 9:a30169882a15 95 for(int i = 0; i < wheels; i++) {
UCHITAKE 9:a30169882a15 96 parallel[i] *= (1.0 - fabs(moment));
UCHITAKE 9:a30169882a15 97 }
UCHITAKE 9:a30169882a15 98 }
UCHITAKE 9:a30169882a15 99 for(int i = 0; i < wheels; i++) {
UCHITAKE 9:a30169882a15 100 wheel[i] = parallel[i] + moment;
UCHITAKE 9:a30169882a15 101 }
UCHITAKE 9:a30169882a15 102 return 1;
UCHITAKE 9:a30169882a15 103 } else {
UCHITAKE 0:979565e955a5 104 return 0;
UCHITAKE 0:979565e955a5 105 }
UCHITAKE 0:979565e955a5 106 }
UCHITAKE 0:979565e955a5 107
UCHITAKE 0:979565e955a5 108
UCHITAKE 9:a30169882a15 109 float Omni::getOutput(int wheelNumber)
UCHITAKE 0:979565e955a5 110 {
UCHITAKE 9:a30169882a15 111 return wheel[wheelNumber];
UCHITAKE 9:a30169882a15 112 }