Takeuchi Issei / Mbed 2 deprecated 4Omunisleeve3

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.

Dependencies:   mbed Eigen

shared/Movement/Movement.cpp@0:ee7e9405e1c7, 2021-04-14 (annotated)

Committer:
e2011220
Date:
Wed Apr 14 07:26:19 2021 +0000
Revision:
0:ee7e9405e1c7
first

Who changed what in which revision?

UserRevisionLine numberNew contents of line
e2011220 0:ee7e9405e1c7 1 #include "Movement.h"
e2011220 0:ee7e9405e1c7 2
e2011220 0:ee7e9405e1c7 3 Movement::Movement()
e2011220 0:ee7e9405e1c7 4 {}
e2011220 0:ee7e9405e1c7 5
e2011220 0:ee7e9405e1c7 6 void Movement::up_limit_balance(double duty[], double units_num, double limit)
e2011220 0:ee7e9405e1c7 7 {
e2011220 0:ee7e9405e1c7 8 double max_duty = 0;
e2011220 0:ee7e9405e1c7 9 // max_dutyにduty[i]の最大値を格納
e2011220 0:ee7e9405e1c7 10 for(int i = 0; i < units_num; i++){
e2011220 0:ee7e9405e1c7 11 if(fabs(duty[i]) > max_duty)
e2011220 0:ee7e9405e1c7 12 max_duty = fabs(duty[i]);
e2011220 0:ee7e9405e1c7 13 }
e2011220 0:ee7e9405e1c7 14
e2011220 0:ee7e9405e1c7 15 // 各車輪の比率を保ちながらduty[i]の最大値をlimitに引き上げ
e2011220 0:ee7e9405e1c7 16 if( max_duty != 0 && ( max_duty < limit ) ){
e2011220 0:ee7e9405e1c7 17 for(int i = 0; i < units_num; i++)
e2011220 0:ee7e9405e1c7 18 duty[i] *= limit / max_duty;
e2011220 0:ee7e9405e1c7 19 }
e2011220 0:ee7e9405e1c7 20 }
e2011220 0:ee7e9405e1c7 21
e2011220 0:ee7e9405e1c7 22 void Movement::down_limit_balance(double duty[], double units_num, double limit)
e2011220 0:ee7e9405e1c7 23 {
e2011220 0:ee7e9405e1c7 24 // 各車輪の比率を保ちながらduty[i]を上限以下に引き下げ
e2011220 0:ee7e9405e1c7 25 for(int i = 0; i < units_num; i ++){
e2011220 0:ee7e9405e1c7 26 if(fabs(duty[i]) > limit){
e2011220 0:ee7e9405e1c7 27 double inv = fabs(duty[i]);
e2011220 0:ee7e9405e1c7 28 for(int j = 0; j < units_num; j ++)
e2011220 0:ee7e9405e1c7 29 duty[j] *= limit / inv;
e2011220 0:ee7e9405e1c7 30 }
e2011220 0:ee7e9405e1c7 31 }
e2011220 0:ee7e9405e1c7 32 }
e2011220 0:ee7e9405e1c7 33
e2011220 0:ee7e9405e1c7 34 void Movement::move(Eigen::Vector3d move_vector)
e2011220 0:ee7e9405e1c7 35 {
e2011220 0:ee7e9405e1c7 36 move(move_vector(X), move_vector(Y), move_vector(Yaw));
e2011220 0:ee7e9405e1c7 37 }