YUTO WATANABE / OmniMove

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

OmniMove.cpp@10:f902cac49aaf, 2021-11-11 (annotated)

Committer:
DarkFlame
Date:
Thu Nov 11 09:07:04 2021 +0000
Revision:
10:f902cac49aaf
Parent:
9:91ce72a587ad 
a little fix.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
DarkFlame 0:44476ac6ab91 1 #include "mbed.h"
DarkFlame 0:44476ac6ab91 2 #include "math.h"
DarkFlame 0:44476ac6ab91 3 #include "OmniMove.h"
DarkFlame 0:44476ac6ab91 4
DarkFlame 6:fac3dcaebe83 5 void OmniMove::setup(int nWheel,float fstWheelAng){
DarkFlame 3:e08a0ee65f69 6 this->nWheel = limit(3,8,nWheel);
DarkFlame 0:44476ac6ab91 7
DarkFlame 0:44476ac6ab91 8 for(i = 0;i < nWheel; i++){
DarkFlame 0:44476ac6ab91 9 Vx_wheel[i] = -1 * sin( conv_rad( fstWheelAng + (360 / nWheel) * i ) );
DarkFlame 0:44476ac6ab91 10 Vy_wheel[i] = cos( conv_rad( fstWheelAng + (360 / nWheel) * i ) );
DarkFlame 0:44476ac6ab91 11 }
DarkFlame 0:44476ac6ab91 12 }
DarkFlame 0:44476ac6ab91 13
DarkFlame 7:9b9d488ebcfd 14 void OmniMove::input_polar(float r,float theta,float Vroll){
DarkFlame 3:e08a0ee65f69 15 r = limit(0,1,r);
DarkFlame 8:fb082466cfec 16 this->Vroll = limit(-1,1,Vroll);
DarkFlame 8:fb082466cfec 17 Vx = r * cos( conv_rad( theta ) ) * ( 1 - fabs(this->Vroll) );
DarkFlame 8:fb082466cfec 18 Vy = r * sin( conv_rad( theta ) ) * ( 1 - fabs(this->Vroll) );
DarkFlame 0:44476ac6ab91 19 }
DarkFlame 0:44476ac6ab91 20
DarkFlame 7:9b9d488ebcfd 21 void OmniMove::input_polar(float r,float theta,float Vroll,float MachineAng){
DarkFlame 3:e08a0ee65f69 22 r = limit(0,1,r);
DarkFlame 8:fb082466cfec 23 this->Vroll = limit(-1,1,Vroll);
DarkFlame 8:fb082466cfec 24 Vx = r * cos( conv_rad( theta - MachineAng ) ) * ( 1 - fabs(this->Vroll) );
DarkFlame 8:fb082466cfec 25 Vy = r * sin( conv_rad( theta - MachineAng ) ) * ( 1 - fabs(this->Vroll) );
DarkFlame 3:e08a0ee65f69 26 }
DarkFlame 3:e08a0ee65f69 27
DarkFlame 7:9b9d488ebcfd 28 void OmniMove::input_cartesian(float x,float y,float Vroll){
DarkFlame 3:e08a0ee65f69 29 x = limit(-1,1,x);
DarkFlame 3:e08a0ee65f69 30 y = limit(-1,1,y);
DarkFlame 8:fb082466cfec 31 this->Vroll = limit(-1,1,Vroll);
DarkFlame 9:91ce72a587ad 32 this->r = limit(0,1,hypotf(x,y));
DarkFlame 9:91ce72a587ad 33 this->theta = conv_deg(atan2(y,x));
DarkFlame 9:91ce72a587ad 34 this->Vx = r * cos( conv_rad( this->theta ) ) * ( 1 - fabs(this->Vroll) );
DarkFlame 9:91ce72a587ad 35 this->Vy = r * sin( conv_rad( this->theta ) ) * ( 1 - fabs(this->Vroll) );
DarkFlame 3:e08a0ee65f69 36 }
DarkFlame 3:e08a0ee65f69 37
DarkFlame 7:9b9d488ebcfd 38 void OmniMove::input_cartesian(float x,float y,float Vroll,float MachineAng){
DarkFlame 3:e08a0ee65f69 39 x = limit(-1,1,x);
DarkFlame 3:e08a0ee65f69 40 y = limit(-1,1,y);
DarkFlame 8:fb082466cfec 41 this->Vroll = limit(-1,1,Vroll);
DarkFlame 3:e08a0ee65f69 42 r = limit(0,1,hypotf(x,y));
DarkFlame 3:e08a0ee65f69 43 theta = conv_deg(atan2(y,x));
DarkFlame 8:fb082466cfec 44 Vx = r * cos( conv_rad( theta - MachineAng ) ) * ( 1 - fabs(this->Vroll) );
DarkFlame 8:fb082466cfec 45 Vy = r * sin( conv_rad( theta - MachineAng ) ) * ( 1 - fabs(this->Vroll) );
DarkFlame 0:44476ac6ab91 46 }
DarkFlame 0:44476ac6ab91 47
DarkFlame 6:fac3dcaebe83 48 float OmniMove::output_(int n){
DarkFlame 9:91ce72a587ad 49 if( this->nWheel < 8 ){
DarkFlame 9:91ce72a587ad 50 return (float)(this->Vx*this->Vx_wheel[n] + this->Vy*this->Vy_wheel[n] + this->Vroll);
DarkFlame 0:44476ac6ab91 51 }else{
DarkFlame 0:44476ac6ab91 52 return 0;
DarkFlame 0:44476ac6ab91 53 }
DarkFlame 0:44476ac6ab91 54 }
DarkFlame 0:44476ac6ab91 55
DarkFlame 6:fac3dcaebe83 56 void OmniMove::output(float *V){
DarkFlame 9:91ce72a587ad 57 for(i = 0;i < this->nWheel;i++){
DarkFlame 6:fac3dcaebe83 58 V[i] = (float)(Vx*Vx_wheel[i] + Vy*Vy_wheel[i] + Vroll);
DarkFlame 0:44476ac6ab91 59 }
DarkFlame 0:44476ac6ab91 60 }
DarkFlame 0:44476ac6ab91 61
DarkFlame 6:fac3dcaebe83 62 float OmniMove::limit(float min,float max,float _value){
DarkFlame 3:e08a0ee65f69 63 if(_value > max){
DarkFlame 3:e08a0ee65f69 64 return max;
DarkFlame 3:e08a0ee65f69 65 }else if(_value < min){
DarkFlame 3:e08a0ee65f69 66 return min;
DarkFlame 3:e08a0ee65f69 67 }else{
DarkFlame 3:e08a0ee65f69 68 return _value;
DarkFlame 3:e08a0ee65f69 69 }
DarkFlame 3:e08a0ee65f69 70 }
DarkFlame 3:e08a0ee65f69 71
DarkFlame 6:fac3dcaebe83 72 float OmniMove::conv_deg(float _rad){
DarkFlame 4:4d94b6148a0a 73 return _rad * 180 / 3.14159265;
DarkFlame 0:44476ac6ab91 74 }
DarkFlame 0:44476ac6ab91 75
DarkFlame 6:fac3dcaebe83 76 float OmniMove::conv_rad(float _deg){
DarkFlame 4:4d94b6148a0a 77 return 3.14159265 * _deg / 180;
DarkFlame 0:44476ac6ab91 78 }