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@0:44476ac6ab91, 2021-03-24 (annotated)

Committer:
DarkFlame
Date:
Wed Mar 24 16:33:58 2021 +0000
Revision:
0:44476ac6ab91
Child:
1:e515f6a4da2e
first commit.

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 0:44476ac6ab91 5 void OmuniMove::setup(int nWheel,float fstWheelAng){
DarkFlame 0:44476ac6ab91 6 this->nWheel = nWheel;
DarkFlame 0:44476ac6ab91 7 m_pi = 3.1415;
DarkFlame 0:44476ac6ab91 8
DarkFlame 0:44476ac6ab91 9 for(i = 0;i < nWheel; i++){
DarkFlame 0:44476ac6ab91 10 Vx_wheel[i] = -1 * sin( conv_rad( fstWheelAng + (360 / nWheel) * i ) );
DarkFlame 0:44476ac6ab91 11 Vy_wheel[i] = cos( conv_rad( fstWheelAng + (360 / nWheel) * i ) );
DarkFlame 0:44476ac6ab91 12 }
DarkFlame 0:44476ac6ab91 13 }
DarkFlame 0:44476ac6ab91 14
DarkFlame 0:44476ac6ab91 15 void OmuniMove::input_polar(float r,float theta,float Vroll,float roll_ratio){
DarkFlame 0:44476ac6ab91 16 Vx = r * cos( conv_rad( theta ) ) * ( 1 - roll_ratio );
DarkFlame 0:44476ac6ab91 17 Vy = r * sin( conv_rad( theta ) ) * ( 1 - roll_ratio );
DarkFlame 0:44476ac6ab91 18 this->Vroll = Vroll * roll_ratio;
DarkFlame 0:44476ac6ab91 19 }
DarkFlame 0:44476ac6ab91 20
DarkFlame 0:44476ac6ab91 21 void OmuniMove::input_polar(float r,float theta,float Vroll,float roll_ratio,float MachineAng){
DarkFlame 0:44476ac6ab91 22 Vx = r * cos( conv_rad( theta - MachineAng ) ) * ( 1 - roll_ratio );
DarkFlame 0:44476ac6ab91 23 Vy = r * sin( conv_rad( theta - MachineAng ) ) * ( 1 - roll_ratio );
DarkFlame 0:44476ac6ab91 24 this->Vroll = Vroll * roll_ratio;
DarkFlame 0:44476ac6ab91 25 }
DarkFlame 0:44476ac6ab91 26
DarkFlame 0:44476ac6ab91 27 float OmuniMove::output(int nWheel){
DarkFlame 0:44476ac6ab91 28 if( nWheel < 8 ){
DarkFlame 0:44476ac6ab91 29 return Vx*Vx_wheel[nWheel] + Vy*Vy_wheel[nWheel] + Vroll;
DarkFlame 0:44476ac6ab91 30 }else{
DarkFlame 0:44476ac6ab91 31 return 0;
DarkFlame 0:44476ac6ab91 32 }
DarkFlame 0:44476ac6ab91 33 }
DarkFlame 0:44476ac6ab91 34
DarkFlame 0:44476ac6ab91 35 void OmuniMove::output(float *V){
DarkFlame 0:44476ac6ab91 36 for(i = 0;i < nWheel;i++){
DarkFlame 0:44476ac6ab91 37 V[i] = Vx*Vx_wheel[i] + Vy*Vy_wheel[i] + Vroll;
DarkFlame 0:44476ac6ab91 38 }
DarkFlame 0:44476ac6ab91 39 }
DarkFlame 0:44476ac6ab91 40
DarkFlame 0:44476ac6ab91 41 float OmuniMove::conv_deg(float _rad){
DarkFlame 0:44476ac6ab91 42 return _rad * 180 / m_pi;
DarkFlame 0:44476ac6ab91 43 }
DarkFlame 0:44476ac6ab91 44
DarkFlame 0:44476ac6ab91 45 float OmuniMove::conv_rad(float _deg){
DarkFlame 0:44476ac6ab91 46 return m_pi * _deg / 180;
DarkFlame 0:44476ac6ab91 47 }