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