2022_gorobo_Ateam / 2022_Ateam_MOTORprogram

足回り用プログラム(修正版)

Dependencies:   SBDBT arrc_mbed BNO055

Odmetry.cpp@5:e189d55ef292, 2022-01-28 (annotated)

Committer:
kazumayamanaka
Date:
Fri Jan 28 15:12:04 2022 +0000
Revision:
5:e189d55ef292
program  of wheel;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kazumayamanaka 5:e189d55ef292 1 #include "Odmetry.hpp"
kazumayamanaka 5:e189d55ef292 2
kazumayamanaka 5:e189d55ef292 3 Odmetry::Odmetry(){
kazumayamanaka 5:e189d55ef292 4 dt = 0.05;
kazumayamanaka 5:e189d55ef292 5 Radius = 395;
kazumayamanaka 5:e189d55ef292 6 Wradius = 25;
kazumayamanaka 5:e189d55ef292 7 XPos = 0;
kazumayamanaka 5:e189d55ef292 8 YPos = 0;
kazumayamanaka 5:e189d55ef292 9 }
kazumayamanaka 5:e189d55ef292 10
kazumayamanaka 5:e189d55ef292 11 void Odmetry::pass_pulse(double PULSE1,double PULSE2,double PULSE3,double PULSE4){
kazumayamanaka 5:e189d55ef292 12 pulse_a[0] = PULSE1;
kazumayamanaka 5:e189d55ef292 13 pulse_a[1] = PULSE2;
kazumayamanaka 5:e189d55ef292 14 pulse_a[2] = PULSE3;
kazumayamanaka 5:e189d55ef292 15 pulse_a[3] = PULSE4;
kazumayamanaka 5:e189d55ef292 16
kazumayamanaka 5:e189d55ef292 17 if(PULSE1 == PULSE2 == PULSE3 == PULSE4){
kazumayamanaka 5:e189d55ef292 18 return;
kazumayamanaka 5:e189d55ef292 19 }
kazumayamanaka 5:e189d55ef292 20 else{
kazumayamanaka 5:e189d55ef292 21 CalcSpd();
kazumayamanaka 5:e189d55ef292 22 Angular();
kazumayamanaka 5:e189d55ef292 23 transform();
kazumayamanaka 5:e189d55ef292 24 CalcPosition();
kazumayamanaka 5:e189d55ef292 25 }
kazumayamanaka 5:e189d55ef292 26 }
kazumayamanaka 5:e189d55ef292 27
kazumayamanaka 5:e189d55ef292 28 void Odmetry::pass_angle(double THETA){
kazumayamanaka 5:e189d55ef292 29 Theta = THETA - PI / 4;
kazumayamanaka 5:e189d55ef292 30 }
kazumayamanaka 5:e189d55ef292 31
kazumayamanaka 5:e189d55ef292 32
kazumayamanaka 5:e189d55ef292 33 void Odmetry::CalcSpd(){
kazumayamanaka 5:e189d55ef292 34 for(int i = 0;i < 4;i ++){
kazumayamanaka 5:e189d55ef292 35 diff[i] = pulse_a[i] - pulse_b[i];
kazumayamanaka 5:e189d55ef292 36 spd[i] = Co * diff[i] / dt;
kazumayamanaka 5:e189d55ef292 37 pulse_b[i] = pulse_a[i];
kazumayamanaka 5:e189d55ef292 38 }
kazumayamanaka 5:e189d55ef292 39 }
kazumayamanaka 5:e189d55ef292 40
kazumayamanaka 5:e189d55ef292 41 void Odmetry::Angular(){
kazumayamanaka 5:e189d55ef292 42 for(int i = 0;i < 4;i ++){
kazumayamanaka 5:e189d55ef292 43 Omega[i] = spd[i] / Radius;
kazumayamanaka 5:e189d55ef292 44 }
kazumayamanaka 5:e189d55ef292 45 }
kazumayamanaka 5:e189d55ef292 46
kazumayamanaka 5:e189d55ef292 47 void Odmetry::transform(){
kazumayamanaka 5:e189d55ef292 48 Xpower = Wradius * (Omega[1] - Omega[3]);
kazumayamanaka 5:e189d55ef292 49 Ypower = Wradius * (Omega[0] - Omega[2]);
kazumayamanaka 5:e189d55ef292 50 }
kazumayamanaka 5:e189d55ef292 51
kazumayamanaka 5:e189d55ef292 52 void Odmetry::CalcPosition(){
kazumayamanaka 5:e189d55ef292 53 Xspd = cos(Theta) * Xpower - sin(Theta) * Ypower;
kazumayamanaka 5:e189d55ef292 54 Yspd = sin(Theta) * Xpower + cos(Theta) * Ypower;
kazumayamanaka 5:e189d55ef292 55
kazumayamanaka 5:e189d55ef292 56 XPos += Xspd * dt;
kazumayamanaka 5:e189d55ef292 57 YPos += Yspd * dt;
kazumayamanaka 5:e189d55ef292 58 }
kazumayamanaka 5:e189d55ef292 59
kazumayamanaka 5:e189d55ef292 60 void Odmetry::print_Pos(){ printf("XPos:%lf YPos:%lf theta:%lf\n",XPos,YPos,Theta); }