勇紀 有岡 / move4wheel

あ

move4wheel.cpp@2:a07bc3e733ff, 2018-11-17 (annotated)

Committer:
yuki0701
Date:
Sat Nov 17 06:56:30 2018 +0000
Revision:
2:a07bc3e733ff
Parent:
1:6a2b95e78d25 
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
la00noix 0:ac911845484d 1 #include <move4wheel.h>
la00noix 0:ac911845484d 2 #include <math.h>
la00noix 0:ac911845484d 3
la00noix 0:ac911845484d 4 namespace
la00noix 0:ac911845484d 5 {
la00noix 0:ac911845484d 6 const int kMotorNum = 4;
la00noix 0:ac911845484d 7 double motorOut[kMotorNum] = {};
la00noix 0:ac911845484d 8 }//namespace
la00noix 0:ac911845484d 9
la00noix 0:ac911845484d 10 //x,y,回転rの出力を4輪に変換。x,y,rの取り方は数学と同じ
la00noix 0:ac911845484d 11 void CalMotorOut(double x_out, double y_out, double r_out)
la00noix 0:ac911845484d 12 {
la00noix 0:ac911845484d 13 const double Pi = 3.1415926535897;
la00noix 0:ac911845484d 14 //motorのついている角度
yuki0701 2:a07bc3e733ff 15 const double kMotorRad[kMotorNum] = {(5.0/4.0)*Pi, (7.0/4.0)*Pi, (1.0/4.0)*Pi, (3.0/4.0)*Pi};
la00noix 0:ac911845484d 16
la00noix 0:ac911845484d 17 //motorの回転方向。機体が反時計回りに回る向きなら1,逆なら-1
la00noix 0:ac911845484d 18 const int rotation[kMotorNum] = {1, 1, 1, 1};
la00noix 0:ac911845484d 19 for (int i = 0; i < kMotorNum; i++) {
la00noix 0:ac911845484d 20 motorOut[i] = cos(kMotorRad[i]) * x_out + sin(kMotorRad[i]) * y_out + rotation[i] * r_out;
la00noix 0:ac911845484d 21
la00noix 0:ac911845484d 22 // → x軸, ↑ Y軸, 反時計回り を正とする
la00noix 0:ac911845484d 23
la00noix 0:ac911845484d 24
la00noix 0:ac911845484d 25 }
la00noix 0:ac911845484d 26 }
la00noix 0:ac911845484d 27
la00noix 0:ac911845484d 28 double GetMotorOut(int motor_num)
la00noix 0:ac911845484d 29 {
la00noix 0:ac911845484d 30 return motorOut[motor_num];
la00noix 0:ac911845484d 31 }