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@3:e08a0ee65f69, 2021-03-25 (annotated)

Committer:
DarkFlame
Date:
Thu Mar 25 11:49:16 2021 +0000
Revision:
3:e08a0ee65f69
Parent:
2:4c4ff6bf6282
Child:
4:4d94b6148a0a
add cartesian input

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 1:e515f6a4da2e 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 1:e515f6a4da2e 14 void OmniMove::input_polar(float r,float theta,float Vroll,float roll_ratio){
DarkFlame 3:e08a0ee65f69 15 r = limit(0,1,r);
DarkFlame 3:e08a0ee65f69 16 Vroll = limit(-1,1,Vroll);
DarkFlame 3:e08a0ee65f69 17 roll_ratio = limit(0,1,roll_ratio);
DarkFlame 0:44476ac6ab91 18 Vx = r * cos( conv_rad( theta ) ) * ( 1 - roll_ratio );
DarkFlame 0:44476ac6ab91 19 Vy = r * sin( conv_rad( theta ) ) * ( 1 - roll_ratio );
DarkFlame 3:e08a0ee65f69 20 Vroll = Vroll * roll_ratio;
DarkFlame 0:44476ac6ab91 21 }
DarkFlame 0:44476ac6ab91 22
DarkFlame 1:e515f6a4da2e 23 void OmniMove::input_polar(float r,float theta,float Vroll,float roll_ratio,float MachineAng){
DarkFlame 3:e08a0ee65f69 24 r = limit(0,1,r);
DarkFlame 3:e08a0ee65f69 25 Vroll = limit(-1,1,Vroll);
DarkFlame 3:e08a0ee65f69 26 roll_ratio = limit(0,1,roll_ratio);
DarkFlame 0:44476ac6ab91 27 Vx = r * cos( conv_rad( theta - MachineAng ) ) * ( 1 - roll_ratio );
DarkFlame 0:44476ac6ab91 28 Vy = r * sin( conv_rad( theta - MachineAng ) ) * ( 1 - roll_ratio );
DarkFlame 3:e08a0ee65f69 29 Vroll = Vroll * roll_ratio;
DarkFlame 3:e08a0ee65f69 30 }
DarkFlame 3:e08a0ee65f69 31
DarkFlame 3:e08a0ee65f69 32 void OmniMove::input_cartesian(float x,float y,float Vroll,float roll_ratio){
DarkFlame 3:e08a0ee65f69 33 x = limit(-1,1,x);
DarkFlame 3:e08a0ee65f69 34 y = limit(-1,1,y);
DarkFlame 3:e08a0ee65f69 35 Vroll = limit(-1,1,Vroll);
DarkFlame 3:e08a0ee65f69 36 roll_ratio = limit(0,1,roll_ratio);
DarkFlame 3:e08a0ee65f69 37 r = limit(0,1,hypotf(x,y));
DarkFlame 3:e08a0ee65f69 38 theta = conv_deg(atan2(y,x));
DarkFlame 3:e08a0ee65f69 39 Vx = r * cos( conv_rad( theta ) ) * ( 1 - roll_ratio );
DarkFlame 3:e08a0ee65f69 40 Vy = r * sin( conv_rad( theta ) ) * ( 1 - roll_ratio );
DarkFlame 3:e08a0ee65f69 41 Vroll = Vroll * roll_ratio;
DarkFlame 3:e08a0ee65f69 42 }
DarkFlame 3:e08a0ee65f69 43
DarkFlame 3:e08a0ee65f69 44 void OmniMove::input_cartesian(float x,float y,float Vroll,float roll_ratio,float MachineAng){
DarkFlame 3:e08a0ee65f69 45 x = limit(-1,1,x);
DarkFlame 3:e08a0ee65f69 46 y = limit(-1,1,y);
DarkFlame 3:e08a0ee65f69 47 Vroll = limit(-1,1,Vroll);
DarkFlame 3:e08a0ee65f69 48 roll_ratio = limit(0,1,roll_ratio);
DarkFlame 3:e08a0ee65f69 49 r = limit(0,1,hypotf(x,y));
DarkFlame 3:e08a0ee65f69 50 theta = conv_deg(atan2(y,x));
DarkFlame 3:e08a0ee65f69 51 Vx = r * cos( conv_rad( theta - MachineAng ) ) * ( 1 - roll_ratio );
DarkFlame 3:e08a0ee65f69 52 Vy = r * sin( conv_rad( theta - MachineAng ) ) * ( 1 - roll_ratio );
DarkFlame 3:e08a0ee65f69 53 Vroll = Vroll * roll_ratio;
DarkFlame 0:44476ac6ab91 54 }
DarkFlame 0:44476ac6ab91 55
DarkFlame 2:4c4ff6bf6282 56 float OmniMove::output_(int nWheel){
DarkFlame 0:44476ac6ab91 57 if( nWheel < 8 ){
DarkFlame 0:44476ac6ab91 58 return Vx*Vx_wheel[nWheel] + Vy*Vy_wheel[nWheel] + Vroll;
DarkFlame 0:44476ac6ab91 59 }else{
DarkFlame 0:44476ac6ab91 60 return 0;
DarkFlame 0:44476ac6ab91 61 }
DarkFlame 0:44476ac6ab91 62 }
DarkFlame 0:44476ac6ab91 63
DarkFlame 1:e515f6a4da2e 64 void OmniMove::output(float *V){
DarkFlame 0:44476ac6ab91 65 for(i = 0;i < nWheel;i++){
DarkFlame 0:44476ac6ab91 66 V[i] = Vx*Vx_wheel[i] + Vy*Vy_wheel[i] + Vroll;
DarkFlame 0:44476ac6ab91 67 }
DarkFlame 0:44476ac6ab91 68 }
DarkFlame 0:44476ac6ab91 69
DarkFlame 3:e08a0ee65f69 70 float OmniMove::limit(float min,float max,float _value){
DarkFlame 3:e08a0ee65f69 71 if(_value > max){
DarkFlame 3:e08a0ee65f69 72 return max;
DarkFlame 3:e08a0ee65f69 73 }else if(_value < min){
DarkFlame 3:e08a0ee65f69 74 return min;
DarkFlame 3:e08a0ee65f69 75 }else{
DarkFlame 3:e08a0ee65f69 76 return _value;
DarkFlame 3:e08a0ee65f69 77 }
DarkFlame 3:e08a0ee65f69 78 }
DarkFlame 3:e08a0ee65f69 79
DarkFlame 1:e515f6a4da2e 80 float OmniMove::conv_deg(float _rad){
DarkFlame 3:e08a0ee65f69 81 return _rad * 180 / 3.141592;
DarkFlame 0:44476ac6ab91 82 }
DarkFlame 0:44476ac6ab91 83
DarkFlame 1:e515f6a4da2e 84 float OmniMove::conv_rad(float _deg){
DarkFlame 3:e08a0ee65f69 85 return 3.141592 * _deg / 180;
DarkFlame 0:44476ac6ab91 86 }