自己位置推定機能を追加
Dependencies: SBDBT arrc_mbed BNO055
DUALSHOCKco.cpp@0:a1238c4cd105, 2022-01-19 (annotated)
- Committer:
- kazumayamanaka
- Date:
- Wed Jan 19 12:43:26 2022 +0000
- Revision:
- 0:a1238c4cd105
2022robotcontest
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kazumayamanaka | 0:a1238c4cd105 | 1 | #include "DUALSHOCKco.hpp" |
kazumayamanaka | 0:a1238c4cd105 | 2 | |
kazumayamanaka | 0:a1238c4cd105 | 3 | void DUALSHOCKco::pass_val(double RSX,double RSY,double R2AN,double L2AN){ |
kazumayamanaka | 0:a1238c4cd105 | 4 | rsx = RSX; |
kazumayamanaka | 0:a1238c4cd105 | 5 | rsy = RSY; |
kazumayamanaka | 0:a1238c4cd105 | 6 | r2an = R2AN; |
kazumayamanaka | 0:a1238c4cd105 | 7 | l2an = L2AN; |
kazumayamanaka | 0:a1238c4cd105 | 8 | } |
kazumayamanaka | 0:a1238c4cd105 | 9 | |
kazumayamanaka | 0:a1238c4cd105 | 10 | bool DUALSHOCKco::cal_input(){ |
kazumayamanaka | 0:a1238c4cd105 | 11 | if(r2an == 0&&l2an == 0){ |
kazumayamanaka | 0:a1238c4cd105 | 12 | Xval = (rsx - RS_normal) * 400 / RS_normal; |
kazumayamanaka | 0:a1238c4cd105 | 13 | Yval = ((RS_MAX - rsy) - RS_normal + 1) * 400 / RS_normal; |
kazumayamanaka | 0:a1238c4cd105 | 14 | return true; |
kazumayamanaka | 0:a1238c4cd105 | 15 | } |
kazumayamanaka | 0:a1238c4cd105 | 16 | else{ |
kazumayamanaka | 0:a1238c4cd105 | 17 | if(r2an != 0){ |
kazumayamanaka | 0:a1238c4cd105 | 18 | Xval = r2an * 400 / RL_MAX; |
kazumayamanaka | 0:a1238c4cd105 | 19 | } |
kazumayamanaka | 0:a1238c4cd105 | 20 | else{ |
kazumayamanaka | 0:a1238c4cd105 | 21 | Xval = -l2an * 400 / RL_MAX; |
kazumayamanaka | 0:a1238c4cd105 | 22 | } |
kazumayamanaka | 0:a1238c4cd105 | 23 | return false; |
kazumayamanaka | 0:a1238c4cd105 | 24 | } |
kazumayamanaka | 0:a1238c4cd105 | 25 | } |
kazumayamanaka | 0:a1238c4cd105 | 26 | |
kazumayamanaka | 0:a1238c4cd105 | 27 | double DUALSHOCKco::obt_X(){ return Xval; } |
kazumayamanaka | 0:a1238c4cd105 | 28 | |
kazumayamanaka | 0:a1238c4cd105 | 29 | double DUALSHOCKco::obt_Y(){ return Yval; } |