mo mo
memo
main.cpp@0:5feeeb404a6a, 2021-08-09 (annotated)
- Committer:
- imoha
- Date:
- Mon Aug 09 13:37:22 2021 +0000
- Revision:
- 0:5feeeb404a6a
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| imoha | 0:5feeeb404a6a | 1 | #include "mbed.h" |
| imoha | 0:5feeeb404a6a | 2 | #include "PS_PAD.h" |
| imoha | 0:5feeeb404a6a | 3 | |
| imoha | 0:5feeeb404a6a | 4 | |
| imoha | 0:5feeeb404a6a | 5 | //入力pin設定 |
| imoha | 0:5feeeb404a6a | 6 | PS_PAD vsc3(PA_7,PA_6,PA_5,PB_6); |
| imoha | 0:5feeeb404a6a | 7 | |
| imoha | 0:5feeeb404a6a | 8 | |
| imoha | 0:5feeeb404a6a | 9 | //出力pin設定 |
| imoha | 0:5feeeb404a6a | 10 | //RIGHT |
| imoha | 0:5feeeb404a6a | 11 | PwmOut MRA (PA_11); |
| imoha | 0:5feeeb404a6a | 12 | PwmOut MRB (PB_13); |
| imoha | 0:5feeeb404a6a | 13 | //LEFT |
| imoha | 0:5feeeb404a6a | 14 | PwmOut MLA (PB_14); |
| imoha | 0:5feeeb404a6a | 15 | PwmOut MLB (PB_15); |
| imoha | 0:5feeeb404a6a | 16 | |
| imoha | 0:5feeeb404a6a | 17 | |
| imoha | 0:5feeeb404a6a | 18 | //SERVO |
| imoha | 0:5feeeb404a6a | 19 | PwmOut s1 (PC_9);//raise |
| imoha | 0:5feeeb404a6a | 20 | PwmOut s2 (PB_8);//grab |
| imoha | 0:5feeeb404a6a | 21 | PwmOut s3 (PB_9);//table |
| imoha | 0:5feeeb404a6a | 22 | |
| imoha | 0:5feeeb404a6a | 23 | |
| imoha | 0:5feeeb404a6a | 24 | //joystick |
| imoha | 0:5feeeb404a6a | 25 | int RY; |
| imoha | 0:5feeeb404a6a | 26 | int LY; |
| imoha | 0:5feeeb404a6a | 27 | |
| imoha | 0:5feeeb404a6a | 28 | double du; |
| imoha | 0:5feeeb404a6a | 29 | |
| imoha | 0:5feeeb404a6a | 30 | int main() |
| imoha | 0:5feeeb404a6a | 31 | { |
| imoha | 0:5feeeb404a6a | 32 | |
| imoha | 0:5feeeb404a6a | 33 | vsc3.init(); |
| imoha | 0:5feeeb404a6a | 34 | |
| imoha | 0:5feeeb404a6a | 35 | MRA.period(0.002); |
| imoha | 0:5feeeb404a6a | 36 | MRB.period(0.002); |
| imoha | 0:5feeeb404a6a | 37 | MLA.period(0.002); |
| imoha | 0:5feeeb404a6a | 38 | MLB.period(0.002); |
| imoha | 0:5feeeb404a6a | 39 | |
| imoha | 0:5feeeb404a6a | 40 | s1.period_us(20000); |
| imoha | 0:5feeeb404a6a | 41 | s2.period_us(20000); |
| imoha | 0:5feeeb404a6a | 42 | s3.period_us(20000); |
| imoha | 0:5feeeb404a6a | 43 | |
| imoha | 0:5feeeb404a6a | 44 | |
| imoha | 0:5feeeb404a6a | 45 | while(1) { |
| imoha | 0:5feeeb404a6a | 46 | |
| imoha | 0:5feeeb404a6a | 47 | vsc3.poll(); |
| imoha | 0:5feeeb404a6a | 48 | |
| imoha | 0:5feeeb404a6a | 49 | RY = vsc3.read(PS_PAD::ANALOG_RY); |
| imoha | 0:5feeeb404a6a | 50 | LY = vsc3.read(PS_PAD::ANALOG_LY); |
| imoha | 0:5feeeb404a6a | 51 | |
| imoha | 0:5feeeb404a6a | 52 | |
| imoha | 0:5feeeb404a6a | 53 | //サーボ操作 |
| imoha | 0:5feeeb404a6a | 54 | if(vsc3.read(PS_PAD::PAD_TOP)) { |
| imoha | 0:5feeeb404a6a | 55 | s1.pulsewidth_us(400); |
| imoha | 0:5feeeb404a6a | 56 | } else if(vsc3.read(PS_PAD::PAD_BOTTOM)) { |
| imoha | 0:5feeeb404a6a | 57 | s1.pulsewidth_us(1550); |
| imoha | 0:5feeeb404a6a | 58 | } else if(vsc3.read(PS_PAD::PAD_RIGHT)) { |
| imoha | 0:5feeeb404a6a | 59 | s2.pulsewidth_us(1700); |
| imoha | 0:5feeeb404a6a | 60 | } else if(vsc3.read(PS_PAD::PAD_LEFT)) { |
| imoha | 0:5feeeb404a6a | 61 | s2.pulsewidth_us(1350); |
| imoha | 0:5feeeb404a6a | 62 | } else if(vsc3.read(PS_PAD::PAD_TRIANGLE)) { |
| imoha | 0:5feeeb404a6a | 63 | s3.pulsewidth_us(1550); |
| imoha | 0:5feeeb404a6a | 64 | } else if(vsc3.read(PS_PAD::PAD_X)) { |
| imoha | 0:5feeeb404a6a | 65 | s3.pulsewidth_us(1900); |
| imoha | 0:5feeeb404a6a | 66 | } else; |
| imoha | 0:5feeeb404a6a | 67 | |
| imoha | 0:5feeeb404a6a | 68 | //速度設定 |
| imoha | 0:5feeeb404a6a | 69 | if(vsc3.read(PS_PAD::PAD_L1)) { |
| imoha | 0:5feeeb404a6a | 70 | du = 0.2; |
| imoha | 0:5feeeb404a6a | 71 | } else if(vsc3.read(PS_PAD::PAD_R1)) { |
| imoha | 0:5feeeb404a6a | 72 | du = 0.99; |
| imoha | 0:5feeeb404a6a | 73 | } else { |
| imoha | 0:5feeeb404a6a | 74 | du = 0.4; |
| imoha | 0:5feeeb404a6a | 75 | } |
| imoha | 0:5feeeb404a6a | 76 | |
| imoha | 0:5feeeb404a6a | 77 | |
| imoha | 0:5feeeb404a6a | 78 | wait(0.05); |
| imoha | 0:5feeeb404a6a | 79 | |
| imoha | 0:5feeeb404a6a | 80 | |
| imoha | 0:5feeeb404a6a | 81 | //右車輪 |
| imoha | 0:5feeeb404a6a | 82 | if (-25 < RY < 25) { |
| imoha | 0:5feeeb404a6a | 83 | MRA = 0; |
| imoha | 0:5feeeb404a6a | 84 | MRB = 0; |
| imoha | 0:5feeeb404a6a | 85 | } |
| imoha | 0:5feeeb404a6a | 86 | if(RY < -25) { |
| imoha | 0:5feeeb404a6a | 87 | MRA = du; |
| imoha | 0:5feeeb404a6a | 88 | MRB = 0; |
| imoha | 0:5feeeb404a6a | 89 | } |
| imoha | 0:5feeeb404a6a | 90 | if(25 < RY) { |
| imoha | 0:5feeeb404a6a | 91 | MRA = 0; |
| imoha | 0:5feeeb404a6a | 92 | MRB = du; |
| imoha | 0:5feeeb404a6a | 93 | } |
| imoha | 0:5feeeb404a6a | 94 | |
| imoha | 0:5feeeb404a6a | 95 | |
| imoha | 0:5feeeb404a6a | 96 | //左車輪 |
| imoha | 0:5feeeb404a6a | 97 | if (-25 < LY < 25) { |
| imoha | 0:5feeeb404a6a | 98 | MLA = 0; |
| imoha | 0:5feeeb404a6a | 99 | MLB = 0; |
| imoha | 0:5feeeb404a6a | 100 | } |
| imoha | 0:5feeeb404a6a | 101 | if(LY < -25) { |
| imoha | 0:5feeeb404a6a | 102 | MLA = du; |
| imoha | 0:5feeeb404a6a | 103 | MLB = 0; |
| imoha | 0:5feeeb404a6a | 104 | } |
| imoha | 0:5feeeb404a6a | 105 | if(25 < LY) { |
| imoha | 0:5feeeb404a6a | 106 | MLA = 0; |
| imoha | 0:5feeeb404a6a | 107 | MLB = du; |
| imoha | 0:5feeeb404a6a | 108 | } |
| imoha | 0:5feeeb404a6a | 109 | |
| imoha | 0:5feeeb404a6a | 110 | } |
| imoha | 0:5feeeb404a6a | 111 | |
| imoha | 0:5feeeb404a6a | 112 | } |