Ryoga Sato / newMecanumLibrary

newMecanum.cpp@0:ae2c570afbe8, 2018-05-28 (annotated)

Committer:
surpace0924
Date:
Mon May 28 23:15:55 2018 +0000
Revision:
0:ae2c570afbe8
Initial commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
surpace0924 0:ae2c570afbe8 1 #include "newMecanum.hpp"
surpace0924 0:ae2c570afbe8 2
surpace0924 0:ae2c570afbe8 3 void newMecanum::init(float length, float width, float _maxVal)
surpace0924 0:ae2c570afbe8 4 {
surpace0924 0:ae2c570afbe8 5 float vectorAngle = atan2(length, width);
surpace0924 0:ae2c570afbe8 6
surpace0924 0:ae2c570afbe8 7 // 理想的なメカナムホイールの配置（45deg）からのズレの度合い
surpace0924 0:ae2c570afbe8 8 rotationCorrection = cos(vectorAngle - PI / 4);
surpace0924 0:ae2c570afbe8 9
surpace0924 0:ae2c570afbe8 10 maxSpeed = _maxSpeed;
surpace0924 0:ae2c570afbe8 11 }
surpace0924 0:ae2c570afbe8 12
surpace0924 0:ae2c570afbe8 13 void newMecanum::calculate(float vx, float vy, float angularVelocity)
surpace0924 0:ae2c570afbe8 14 {
surpace0924 0:ae2c570afbe8 15 // 計算（ヤコビ行列の中身を展開した状態で記述）
surpace0924 0:ae2c570afbe8 16 v[0] = vx + vy + rotationCorrection * angularVelocity;
surpace0924 0:ae2c570afbe8 17 v[1] = vx - vy + rotationCorrection * angularVelocity;
surpace0924 0:ae2c570afbe8 18 v[2] = -vx + vy + rotationCorrection * angularVelocity;
surpace0924 0:ae2c570afbe8 19 v[3] = -vx - vy + rotationCorrection * angularVelocity;
surpace0924 0:ae2c570afbe8 20
surpace0924 0:ae2c570afbe8 21 // ガード処理
surpace0924 0:ae2c570afbe8 22 float maxVal = 0;
surpace0924 0:ae2c570afbe8 23 for (int i = 0; i < 4; i++)
surpace0924 0:ae2c570afbe8 24 {
surpace0924 0:ae2c570afbe8 25 if (abs(v[i]) > maxVal)
surpace0924 0:ae2c570afbe8 26 {
surpace0924 0:ae2c570afbe8 27 maxVal = v[i];
surpace0924 0:ae2c570afbe8 28 }
surpace0924 0:ae2c570afbe8 29 }
surpace0924 0:ae2c570afbe8 30
surpace0924 0:ae2c570afbe8 31 if (maxVal > maxSpeed)
surpace0924 0:ae2c570afbe8 32 {
surpace0924 0:ae2c570afbe8 33 for (int i = 0; i < 4; i++)
surpace0924 0:ae2c570afbe8 34 {
surpace0924 0:ae2c570afbe8 35 v[i] = map(v[i], -maxVal, maxVal, -maxSpeed, maxSpeed);
surpace0924 0:ae2c570afbe8 36 }
surpace0924 0:ae2c570afbe8 37 }
surpace0924 0:ae2c570afbe8 38 }
surpace0924 0:ae2c570afbe8 39
surpace0924 0:ae2c570afbe8 40 float newMecanum::getWheelVelocity(short num)
surpace0924 0:ae2c570afbe8 41 {
surpace0924 0:ae2c570afbe8 42 return v[num];
surpace0924 0:ae2c570afbe8 43 }