NagaokaRoboticsClub_mbedTeam / omni

omni wheel library

Dependents:   quadOmni_yanagi NHK2017_octopus hayatoShooter

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

omni.cpp@8:ab46e745cfab, 2017-08-06 (annotated)

Committer:
UCHITAKE
Date:
Sun Aug 06 14:49:01 2017 +0000
Revision:
8:ab46e745cfab
Parent:
7:1d0aa4275eb0
Child:
10:4f0b55344c73
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 7:1d0aa4275eb0 25 inline double toDegrees(double radians) {
UCHITAKE 8:ab46e745cfab 26 return radians * (180.0 / M_PI);
UCHITAKE 7:1d0aa4275eb0 27 }
UCHITAKE 7:1d0aa4275eb0 28
UCHITAKE 0:979565e955a5 29 Omni::Omni(int wheels, double initDegree) :
UCHITAKE 0:979565e955a5 30 wheels(wheels), initDegree(initDegree)
UCHITAKE 0:979565e955a5 31 {
UCHITAKE 0:979565e955a5 32 for(int i = 0; i < wheels; i++) {
UCHITAKE 0:979565e955a5 33 wheel[i] = 0;
UCHITAKE 0:979565e955a5 34 }
UCHITAKE 0:979565e955a5 35 }
UCHITAKE 0:979565e955a5 36
UCHITAKE 0:979565e955a5 37 bool Omni::computeXY(double parallelVector[], double moment)
UCHITAKE 0:979565e955a5 38 {
UCHITAKE 0:979565e955a5 39 if(wheels < 3 || wheels > 4) {
UCHITAKE 0:979565e955a5 40 return 0;
UCHITAKE 0:979565e955a5 41 }
UCHITAKE 0:979565e955a5 42
UCHITAKE 0:979565e955a5 43 double polar[2];
UCHITAKE 0:979565e955a5 44
UCHITAKE 7:1d0aa4275eb0 45 polar[0] = toDegrees(atan2(parallelVector[1], parallelVector[0]));
UCHITAKE 0:979565e955a5 46 polar[1] = hypot(parallelVector[0], parallelVector[1]);
UCHITAKE 0:979565e955a5 47
UCHITAKE 0:979565e955a5 48 return computePolar(polar, moment);
UCHITAKE 0:979565e955a5 49 }
UCHITAKE 0:979565e955a5 50
UCHITAKE 0:979565e955a5 51 bool Omni::computePolar(double parallelVector[], double moment)
UCHITAKE 0:979565e955a5 52 {
UCHITAKE 0:979565e955a5 53 if(wheels < 3 || wheels > 4) {
UCHITAKE 0:979565e955a5 54 return 0;
UCHITAKE 0:979565e955a5 55 }
UCHITAKE 0:979565e955a5 56
UCHITAKE 0:979565e955a5 57 if(parallelVector[1] < 0 || parallelVector[1] > 1.0) {
UCHITAKE 0:979565e955a5 58 return 0;
UCHITAKE 0:979565e955a5 59 }
UCHITAKE 0:979565e955a5 60
UCHITAKE 0:979565e955a5 61 double parallel[4] = {0};
UCHITAKE 6:59171f3235b6 62 double parallelMax = -1;
UCHITAKE 6:59171f3235b6 63 double parallelMin = 1;
UCHITAKE 0:979565e955a5 64
UCHITAKE 0:979565e955a5 65 for(int i = 0; i < wheels; i++) {
UCHITAKE 5:f2fc4a0b6a6f 66 parallel[i] = sin(toRadians(parallelVector[0] + initDegree + (360 / wheels) * i)) * parallelVector[1];
UCHITAKE 0:979565e955a5 67 }
UCHITAKE 6:59171f3235b6 68 for(int i = 0; i < wheels; i++) {
UCHITAKE 6:59171f3235b6 69 parallelMax = fmax(parallel[i], parallelMax);
UCHITAKE 6:59171f3235b6 70 parallelMin = fmin(parallel[i], parallelMin);
UCHITAKE 0:979565e955a5 71 }
UCHITAKE 0:979565e955a5 72 if(parallelMax + moment > 1.0 || parallelMin + moment < -1.0) {
UCHITAKE 0:979565e955a5 73 for(int i = 0; i < wheels; i++) {
UCHITAKE 0:979565e955a5 74 parallel[i] *= (1.0 - fabs(moment));
UCHITAKE 0:979565e955a5 75 }
UCHITAKE 0:979565e955a5 76 }
UCHITAKE 0:979565e955a5 77 for(int i = 0; i < wheels; i++) {
UCHITAKE 0:979565e955a5 78 wheel[i] = parallel[i] + moment;
UCHITAKE 0:979565e955a5 79 }
UCHITAKE 0:979565e955a5 80 return 1;
UCHITAKE 0:979565e955a5 81 }
UCHITAKE 0:979565e955a5 82
UCHITAKE 0:979565e955a5 83 bool Omni::stop()
UCHITAKE 0:979565e955a5 84 {
UCHITAKE 0:979565e955a5 85 for(int i = 0; i < wheels; i++) {
UCHITAKE 0:979565e955a5 86 wheel[i] = 0;
UCHITAKE 0:979565e955a5 87 }
UCHITAKE 0:979565e955a5 88 return 1;
UCHITAKE 0:979565e955a5 89 }
UCHITAKE 0:979565e955a5 90
UCHITAKE 0:979565e955a5 91 void Omni::setWheels(int wheel)
UCHITAKE 0:979565e955a5 92 {
UCHITAKE 0:979565e955a5 93 wheels = wheel;
UCHITAKE 0:979565e955a5 94 }
UCHITAKE 0:979565e955a5 95
UCHITAKE 0:979565e955a5 96 void Omni::setInitDegree(double degree)
UCHITAKE 0:979565e955a5 97 {
UCHITAKE 0:979565e955a5 98 initDegree = degree;
UCHITAKE 0:979565e955a5 99 }
UCHITAKE 0:979565e955a5 100
UCHITAKE 0:979565e955a5 101 double Omni::getOutput(int num)
UCHITAKE 0:979565e955a5 102 {
UCHITAKE 0:979565e955a5 103 return wheel[num];
UCHITAKE 0:979565e955a5 104 }