Takeuchi Issei / Mbed 2 deprecated 4Omunisleeve3

メカナムのコード

Dependencies:   mbed Eigen

shared/Wheel/Wheel.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 "Wheel.h"
e2011220 0:ee7e9405e1c7 2
e2011220 0:ee7e9405e1c7 3 Wheel::Wheel(MD *md, double wheel_angle, double position_angle)
e2011220 0:ee7e9405e1c7 4 {
e2011220 0:ee7e9405e1c7 5 this->md = md;
e2011220 0:ee7e9405e1c7 6 this->wheel_angle = wheel_angle;
e2011220 0:ee7e9405e1c7 7 this->wheel_radius = 0;
e2011220 0:ee7e9405e1c7 8
e2011220 0:ee7e9405e1c7 9 this->position_angle = position_angle;
e2011220 0:ee7e9405e1c7 10 this->position_radius = 1;
e2011220 0:ee7e9405e1c7 11 }
e2011220 0:ee7e9405e1c7 12
e2011220 0:ee7e9405e1c7 13 Wheel::Wheel(MD *md, double wheel_angle, double position_x, double position_y, double wheel_radius)
e2011220 0:ee7e9405e1c7 14 {
e2011220 0:ee7e9405e1c7 15 this->md = md;
e2011220 0:ee7e9405e1c7 16 this->wheel_angle = wheel_angle;
e2011220 0:ee7e9405e1c7 17 this->wheel_radius = wheel_radius;
e2011220 0:ee7e9405e1c7 18
e2011220 0:ee7e9405e1c7 19 set_position(position_x, position_y);
e2011220 0:ee7e9405e1c7 20 }
e2011220 0:ee7e9405e1c7 21
e2011220 0:ee7e9405e1c7 22 void Wheel::set_position(double position_x, double position_y)
e2011220 0:ee7e9405e1c7 23 {
e2011220 0:ee7e9405e1c7 24 position_angle = atan2(position_y, position_x);
e2011220 0:ee7e9405e1c7 25 position_radius = sqrt( position_y * position_y + position_x * position_x );
e2011220 0:ee7e9405e1c7 26 }
e2011220 0:ee7e9405e1c7 27
e2011220 0:ee7e9405e1c7 28 double Wheel::wheel_speed(double move_angle, double move_radius, double yaw)
e2011220 0:ee7e9405e1c7 29 {
e2011220 0:ee7e9405e1c7 30 double speed;
e2011220 0:ee7e9405e1c7 31 double moving, rotating;
e2011220 0:ee7e9405e1c7 32
e2011220 0:ee7e9405e1c7 33 while(move_angle > M_PI)
e2011220 0:ee7e9405e1c7 34 move_angle -= 2.0 * M_PI;
e2011220 0:ee7e9405e1c7 35
e2011220 0:ee7e9405e1c7 36 moving = cos(wheel_angle - move_angle) * move_radius;
e2011220 0:ee7e9405e1c7 37
e2011220 0:ee7e9405e1c7 38 rotating = sin(wheel_angle - position_angle) * yaw * position_radius;
e2011220 0:ee7e9405e1c7 39
e2011220 0:ee7e9405e1c7 40 speed = moving + rotating;
e2011220 0:ee7e9405e1c7 41
e2011220 0:ee7e9405e1c7 42 return speed;
e2011220 0:ee7e9405e1c7 43 }
e2011220 0:ee7e9405e1c7 44
e2011220 0:ee7e9405e1c7 45 void Wheel::drive(double duty)
e2011220 0:ee7e9405e1c7 46 {
e2011220 0:ee7e9405e1c7 47 md->drive(duty);
e2011220 0:ee7e9405e1c7 48 }