Takeuchi Issei / Mbed 2 deprecated 4Omunisleeve3

メカナムのコード

Dependencies:   mbed Eigen

shared/Mecanum_4/Mecanum_4.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 "Mecanum_4.h"
e2011220 0:ee7e9405e1c7 2
e2011220 0:ee7e9405e1c7 3 Mecanum_4::Mecanum_4(MD *md[NUM])
e2011220 0:ee7e9405e1c7 4 {
e2011220 0:ee7e9405e1c7 5 wheel[LF] = new Wheel( md[LF], 1.0 * M_PI / 4.0, 3.0 * M_PI / 4.0 );
e2011220 0:ee7e9405e1c7 6 wheel[LB] = new Wheel( md[LB], 3.0 * M_PI / 4.0, -3.0 * M_PI / 4.0 );
e2011220 0:ee7e9405e1c7 7 wheel[RB] = new Wheel( md[RB], -3.0 * M_PI / 4.0, -1.0 * M_PI / 4.0 );
e2011220 0:ee7e9405e1c7 8 wheel[RF] = new Wheel( md[RF], -1.0 * M_PI / 4.0, 1.0 * M_PI / 4.0 );
e2011220 0:ee7e9405e1c7 9 }
e2011220 0:ee7e9405e1c7 10
e2011220 0:ee7e9405e1c7 11 Mecanum_4::Mecanum_4(MD *md_LF, MD *md_LB, MD *md_RB, MD *md_RF)
e2011220 0:ee7e9405e1c7 12 {
e2011220 0:ee7e9405e1c7 13 wheel[LF] = new Wheel( md_LF, 1.0 * M_PI / 4.0, 3.0 * M_PI / 4.0 );
e2011220 0:ee7e9405e1c7 14 wheel[LB] = new Wheel( md_LB, 3.0 * M_PI / 4.0, -3.0 * M_PI / 4.0 );
e2011220 0:ee7e9405e1c7 15 wheel[RB] = new Wheel( md_RB, -3.0 * M_PI / 4.0, -1.0 * M_PI / 4.0 );
e2011220 0:ee7e9405e1c7 16 wheel[RF] = new Wheel( md_RF, -1.0 * M_PI / 4.0, 1.0 * M_PI / 4.0 );
e2011220 0:ee7e9405e1c7 17 }
e2011220 0:ee7e9405e1c7 18
e2011220 0:ee7e9405e1c7 19 Mecanum_4::Mecanum_4(Wheel *wheel[NUM])
e2011220 0:ee7e9405e1c7 20 {
e2011220 0:ee7e9405e1c7 21 for(int i = 0; i < NUM; i++){
e2011220 0:ee7e9405e1c7 22 this->wheel[i] = wheel[i];
e2011220 0:ee7e9405e1c7 23 }
e2011220 0:ee7e9405e1c7 24 }
e2011220 0:ee7e9405e1c7 25
e2011220 0:ee7e9405e1c7 26 void Mecanum_4::move(double x, double y, double yaw)
e2011220 0:ee7e9405e1c7 27 {
e2011220 0:ee7e9405e1c7 28 double move_angle, move_radius;
e2011220 0:ee7e9405e1c7 29 double duty[NUM];
e2011220 0:ee7e9405e1c7 30 double max_duty, limit_duty = 1.0;
e2011220 0:ee7e9405e1c7 31
e2011220 0:ee7e9405e1c7 32 move_angle = atan2(y, x);
e2011220 0:ee7e9405e1c7 33 move_radius = sqrt( y * y + x * x );
e2011220 0:ee7e9405e1c7 34
e2011220 0:ee7e9405e1c7 35 for(int i = 0; i < NUM; i++)
e2011220 0:ee7e9405e1c7 36 duty[i] = wheel[i]->wheel_speed(move_angle, move_radius, yaw);
e2011220 0:ee7e9405e1c7 37
e2011220 0:ee7e9405e1c7 38 // 平行移動と回転のうち、大きい方をmax_dutyに格納
e2011220 0:ee7e9405e1c7 39 if(move_radius > fabs(yaw))
e2011220 0:ee7e9405e1c7 40 max_duty = move_radius;
e2011220 0:ee7e9405e1c7 41 else
e2011220 0:ee7e9405e1c7 42 max_duty = fabs(yaw);
e2011220 0:ee7e9405e1c7 43
e2011220 0:ee7e9405e1c7 44 up_limit_balance(duty, NUM, max_duty);
e2011220 0:ee7e9405e1c7 45 down_limit_balance(duty, NUM, limit_duty);
e2011220 0:ee7e9405e1c7 46
e2011220 0:ee7e9405e1c7 47 for(int i = 0; i < NUM; i++){
e2011220 0:ee7e9405e1c7 48 wheel[i]->drive(duty[i]);
e2011220 0:ee7e9405e1c7 49 }
e2011220 0:ee7e9405e1c7 50 }