NagaokaRoboticsClub_mbedTeam / omni

omni wheel library

Dependents:   quadOmni_yanagi NHK2017_octopus hayatoShooter

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

omni.cpp@10:4f0b55344c73, 2017-08-23 (annotated)

Committer:
UCHITAKE
Date:
Wed Aug 23 07:26:26 2017 +0000
Revision:
10:4f0b55344c73
Parent:
8:ab46e745cfab 
fix commit

Who changed what in which revision?

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