odometry
Dependencies: arrc_mbed BNO055
main.cpp@0:dea3c70f4911, 2021-06-21 (annotated)
- Committer:
- robokenken
- Date:
- Mon Jun 21 07:35:27 2021 +0000
- Revision:
- 0:dea3c70f4911
- Child:
- 1:4062af6224fa
omuni wheel odometry
Who changed what in which revision?
User | Revision | Line number | New 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 | 0:dea3c70f4911 | 72 | -sin(theta + M_PI/4.0), -sin(theta + 3.0*M_PI/2.0), -sin(theta + 5.0*M_PI/4.0), -sin(theta + 7.0*M_PI/4.0), |
robokenken | 0:dea3c70f4911 | 73 | cos(theta + M_PI/4.0), cos(theta + 3.0*M_PI/2.0), cos(theta + 5.0*M_PI/4.0), cos(theta + 7.0*M_PI/4.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 | } |