Akashi_2021_Bteam / odometry

odometry

Dependencies:   arrc_mbed BNO055

main.cpp@1:4062af6224fa, 2021-07-03 (annotated)

Committer:
robokenken
Date:
Sat Jul 03 00:15:03 2021 +0000
Revision:
1:4062af6224fa
Parent:
0:dea3c70f4911 
nakasima

Who changed what in which revision?

UserRevisionLine numberNew contents of line
robokenken 0:dea3c70f4911 1 #include "mbed.h"
robokenken 0:dea3c70f4911 2 #include "rotary_inc.hpp"
robokenken 0:dea3c70f4911 3 #include "matrix.h"
robokenken 0:dea3c70f4911 4 #include "BNO055.h"
robokenken 0:dea3c70f4911 5 #include <cmath>
robokenken 0:dea3c70f4911 6
robokenken 0:dea3c70f4911 7 //Serial pc(USBTX, USBRX);
robokenken 0:dea3c70f4911 8
robokenken 0:dea3c70f4911 9 RotaryInc enc1(PC_3,PC_2,2*M_PI,256,4);
robokenken 0:dea3c70f4911 10 RotaryInc enc2(PC_5,PA_12,2*M_PI,256,4);
robokenken 0:dea3c70f4911 11 RotaryInc enc3(PC_1,PC_0,2*M_PI,256,4);
robokenken 0:dea3c70f4911 12 RotaryInc enc4(PA_13,PC_4,2*M_PI,256,4);
robokenken 0:dea3c70f4911 13
robokenken 0:dea3c70f4911 14 BNO055 bno(PB_3, PB_10);
robokenken 0:dea3c70f4911 15
robokenken 0:dea3c70f4911 16 Timer t;
robokenken 0:dea3c70f4911 17
robokenken 0:dea3c70f4911 18 Matrix<double> Vr(3, 1);
robokenken 0:dea3c70f4911 19 Matrix<double> J(3, 4);
robokenken 0:dea3c70f4911 20 Matrix<double> W(4, 1);
robokenken 0:dea3c70f4911 21
robokenken 0:dea3c70f4911 22 int main(){
robokenken 0:dea3c70f4911 23 double r = 50.6;
robokenken 0:dea3c70f4911 24 double R = 2.0*r;
robokenken 0:dea3c70f4911 25 double theta = 0.0;
robokenken 0:dea3c70f4911 26 double gyro = 0.0;
robokenken 0:dea3c70f4911 27 double fgyro = 0.0;
robokenken 0:dea3c70f4911 28 double x = 0.0;
robokenken 0:dea3c70f4911 29 double y = 0.0;
robokenken 0:dea3c70f4911 30 double vx = 0.0;
robokenken 0:dea3c70f4911 31 double vy = 0.0;
robokenken 0:dea3c70f4911 32 double old1 = 0.0, old2 = 0.0, old3 = 0.0, old4 = 0.0;
robokenken 0:dea3c70f4911 33 double cor1 = 0.0, cor2 = 0.0, cor3 = 0.0, cor4 = 0.0;
robokenken 0:dea3c70f4911 34 bno.setmode(OPERATION_MODE_IMUPLUS);
robokenken 0:dea3c70f4911 35 bno.get_angles();/*
robokenken 0:dea3c70f4911 36 while(fgyro == 0){
robokenken 0:dea3c70f4911 37 bno.get_angles();
robokenken 0:dea3c70f4911 38 fgyro = bno.euler.yaw * (M_PI / 180.0);
robokenken 0:dea3c70f4911 39 }
robokenken 0:dea3c70f4911 40 fgyro = 0.0;*/
robokenken 0:dea3c70f4911 41 while(fgyro - (bno.euler.yaw * (M_PI / 180.0)) != 0.0){
robokenken 0:dea3c70f4911 42 fgyro = bno.euler.yaw * (M_PI / 180.0);
robokenken 0:dea3c70f4911 43 bno.get_angles();
robokenken 0:dea3c70f4911 44 }
robokenken 0:dea3c70f4911 45 t.start();
robokenken 0:dea3c70f4911 46 double start;
robokenken 0:dea3c70f4911 47 while(1){
robokenken 0:dea3c70f4911 48 start = t.read();
robokenken 0:dea3c70f4911 49
robokenken 0:dea3c70f4911 50 cor1 = 2*M_PI*enc1.get()/1024.0;
robokenken 0:dea3c70f4911 51 cor2 = 2*M_PI*enc2.get()/1024.0;
robokenken 0:dea3c70f4911 52 cor3 = 2*M_PI*enc3.get()/1024.0;
robokenken 0:dea3c70f4911 53 cor4 = 2*M_PI*enc4.get()/1024.0;
robokenken 0:dea3c70f4911 54
robokenken 0:dea3c70f4911 55 W.set( 4,
robokenken 0:dea3c70f4911 56 (old1-cor1)/0.01,//enc1.getSpeed(),
robokenken 0:dea3c70f4911 57 (old2-cor2)/0.01,//enc2.getSpeed(),
robokenken 0:dea3c70f4911 58 (old3-cor3)/0.01,//enc3.getSpeed(),
robokenken 0:dea3c70f4911 59 (old4-cor4)/0.01//enc4.getSpeed()
robokenken 0:dea3c70f4911 60 );
robokenken 0:dea3c70f4911 61
robokenken 0:dea3c70f4911 62 old1 = cor1;
robokenken 0:dea3c70f4911 63 old2 = cor2;
robokenken 0:dea3c70f4911 64 old3 = cor3;
robokenken 0:dea3c70f4911 65 old4 = cor4;
robokenken 0:dea3c70f4911 66
robokenken 0:dea3c70f4911 67 //W.show();
robokenken 0:dea3c70f4911 68 bno.get_angles();
robokenken 0:dea3c70f4911 69 gyro = bno.euler.yaw * (M_PI / 180.0);
robokenken 0:dea3c70f4911 70 theta = abs(fgyro - gyro);
robokenken 0:dea3c70f4911 71 J.set( 12,
robokenken 1:4062af6224fa 72 -sin(theta), -sin(theta + M_PI/2.0), -sin(theta + M_PI), -sin(theta + 3.0*M_PI/2.0),
robokenken 1:4062af6224fa 73 cos(theta), cos(theta + M_PI/2.0), cos(theta + M_PI), cos(theta + 3.0*M_PI/2.0),
robokenken 0:dea3c70f4911 74 1.0/(2.0*R), 1.0/(2.0*R), 1.0/(2.0*R), 1.0/(2.0*R)
robokenken 0:dea3c70f4911 75 );
robokenken 0:dea3c70f4911 76 //J.show();
robokenken 0:dea3c70f4911 77
robokenken 0:dea3c70f4911 78 Vr = J*W*(r/2);
robokenken 0:dea3c70f4911 79 //Vr.show();
robokenken 0:dea3c70f4911 80
robokenken 0:dea3c70f4911 81 x += ((vx + Vr[0][0]) * 0.01) / 2.0;
robokenken 0:dea3c70f4911 82 y += ((vy + Vr[1][0]) * 0.01) / 2.0;
robokenken 0:dea3c70f4911 83 vx = Vr[0][0];
robokenken 0:dea3c70f4911 84 vy = Vr[1][0];
robokenken 0:dea3c70f4911 85
robokenken 0:dea3c70f4911 86 pc.printf("x : %lf y : %lf : %lf theta : %lf\r\n", x, y, Vr[2][0], theta);
robokenken 0:dea3c70f4911 87 //pc.printf("vx : %lf vy : %lf\r\n",Vr[0][0], Vr[1][0]);
robokenken 0:dea3c70f4911 88
robokenken 0:dea3c70f4911 89 while(t.read() < start + 0.01);
robokenken 0:dea3c70f4911 90 }
robokenken 0:dea3c70f4911 91 }