2022_Ateam_MOTORprogramをscrp_slaveでメインマイコンからコントローラー状況を読み取れるように改良。 また、モータに0以外のpwmが送られている場合に基盤付属のledが点灯するようにした。
Dependencies: SBDBT arrc_mbed BNO055
TARGETco.cpp@13:369f4abc1f36, 2022-04-08 (annotated)
- Committer:
- guesta
- Date:
- Fri Apr 08 09:14:20 2022 +0000
- Revision:
- 13:369f4abc1f36
- Parent:
- 7:a0375e6dc8ca
2022/04/08
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kazumayamanaka | 0:a1238c4cd105 | 1 | #include "TARGETco.hpp" |
kazumayamanaka | 0:a1238c4cd105 | 2 | |
kazumayamanaka | 0:a1238c4cd105 | 3 | void TARGETco::pass_val(double XPOWER,double YPOWER,double THETA){ |
kazumayamanaka | 0:a1238c4cd105 | 4 | Xpower = XPOWER; |
kazumayamanaka | 0:a1238c4cd105 | 5 | Ypower = YPOWER; |
kazumayamanaka | 0:a1238c4cd105 | 6 | theta = THETA; |
kazumayamanaka | 0:a1238c4cd105 | 7 | |
kazumayamanaka | 0:a1238c4cd105 | 8 | cal_angular(); |
kazumayamanaka | 0:a1238c4cd105 | 9 | } |
kazumayamanaka | 0:a1238c4cd105 | 10 | |
kazumayamanaka | 0:a1238c4cd105 | 11 | void TARGETco::pass_target(double TARGET){ |
kazumayamanaka | 0:a1238c4cd105 | 12 | for(int i = 0;i < 4;i ++){ |
kazumayamanaka | 0:a1238c4cd105 | 13 | target[i] = TARGET; |
kazumayamanaka | 0:a1238c4cd105 | 14 | } |
kazumayamanaka | 0:a1238c4cd105 | 15 | } |
kazumayamanaka | 0:a1238c4cd105 | 16 | |
kazumayamanaka | 0:a1238c4cd105 | 17 | void TARGETco::cal_angular(){ |
kazumayamanaka | 0:a1238c4cd105 | 18 | Xvelocity = Xpower * cos(theta) - Ypower * sin(theta); |
kazumayamanaka | 1:ea880e226e5a | 19 | Yvelocity = -Xpower * sin(theta) - Ypower * cos(theta); |
kazumayamanaka | 0:a1238c4cd105 | 20 | target[0] = -Xvelocity; |
kazumayamanaka | 0:a1238c4cd105 | 21 | target[1] = -Yvelocity; |
kazumayamanaka | 0:a1238c4cd105 | 22 | target[2] = Xvelocity; |
kazumayamanaka | 0:a1238c4cd105 | 23 | target[3] = Yvelocity; |
guesta | 7:a0375e6dc8ca | 24 | //printf("%lf\n",target[1]); |
kazumayamanaka | 0:a1238c4cd105 | 25 | } |
kazumayamanaka | 0:a1238c4cd105 | 26 | |
kazumayamanaka | 0:a1238c4cd105 | 27 | double TARGETco::obt_target1(){ return target[0]; } |
kazumayamanaka | 0:a1238c4cd105 | 28 | |
kazumayamanaka | 0:a1238c4cd105 | 29 | double TARGETco::obt_target2(){ return target[1]; } |
kazumayamanaka | 0:a1238c4cd105 | 30 | |
kazumayamanaka | 0:a1238c4cd105 | 31 | double TARGETco::obt_target3(){ return target[2]; } |
kazumayamanaka | 0:a1238c4cd105 | 32 | |
kazumayamanaka | 0:a1238c4cd105 | 33 | double TARGETco::obt_target4(){ return target[3]; } |