100% akira
夏ロボ＠手動機
syudouki.cpp@1:797913662b1f, 2018-07-15 (annotated)
- Committer:
- AK1412
- Date:
- Sun Jul 15 09:12:49 2018 +0000
- Revision:
- 1:797913662b1f
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| AK1412 | 1:797913662b1f | 1 | #include "mbed.h" |
| AK1412 | 1:797913662b1f | 2 | #include "kbt.h" |
| AK1412 | 1:797913662b1f | 3 | |
| AK1412 | 1:797913662b1f | 4 | KBT kbt(PA_9,PA_10); |
| AK1412 | 1:797913662b1f | 5 | Serial pc(USBTX,USBRX); |
| AK1412 | 1:797913662b1f | 6 | |
| AK1412 | 1:797913662b1f | 7 | PwmOut rifrpl (PC_12); |
| AK1412 | 1:797913662b1f | 8 | PwmOut rifrmi (PA_13); |
| AK1412 | 1:797913662b1f | 9 | PwmOut ribapl (PA_14); |
| AK1412 | 1:797913662b1f | 10 | PwmOut ribami (PA_15); |
| AK1412 | 1:797913662b1f | 11 | PwmOut lefrpl (PB_7); |
| AK1412 | 1:797913662b1f | 12 | PwmOut lefrmi (PC_13); |
| AK1412 | 1:797913662b1f | 13 | PwmOut lebapl (PF_0); |
| AK1412 | 1:797913662b1f | 14 | PwmOut lebami (PC_2); |
| AK1412 | 1:797913662b1f | 15 | PwmOut servo (PC_3); |
| AK1412 | 1:797913662b1f | 16 | DigitalOut air (PB_10); |
| AK1412 | 1:797913662b1f | 17 | |
| AK1412 | 1:797913662b1f | 18 | double map(double x, double in_min, double in_max, double out_min, double out_max) |
| AK1412 | 1:797913662b1f | 19 | { |
| AK1412 | 1:797913662b1f | 20 | return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; |
| AK1412 | 1:797913662b1f | 21 | } |
| AK1412 | 1:797913662b1f | 22 | |
| AK1412 | 1:797913662b1f | 23 | double Abs ( double value ){ |
| AK1412 | 1:797913662b1f | 24 | if ( value < 0 ) |
| AK1412 | 1:797913662b1f | 25 | value = value*(-1); |
| AK1412 | 1:797913662b1f | 26 | return value; |
| AK1412 | 1:797913662b1f | 27 | } |
| AK1412 | 1:797913662b1f | 28 | |
| AK1412 | 1:797913662b1f | 29 | int main() { |
| AK1412 | 1:797913662b1f | 30 | |
| AK1412 | 1:797913662b1f | 31 | kbt.init(2400); |
| AK1412 | 1:797913662b1f | 32 | pc.baud(9600); |
| AK1412 | 1:797913662b1f | 33 | |
| AK1412 | 1:797913662b1f | 34 | while(1) { |
| AK1412 | 1:797913662b1f | 35 | double Larpow = kbt.Stick[L_around]; |
| AK1412 | 1:797913662b1f | 36 | double Luppow = kbt.Stick[L_updown]; |
| AK1412 | 1:797913662b1f | 37 | |
| AK1412 | 1:797913662b1f | 38 | int dir; |
| AK1412 | 1:797913662b1f | 39 | |
| AK1412 | 1:797913662b1f | 40 | double pow = 0; |
| AK1412 | 1:797913662b1f | 41 | |
| AK1412 | 1:797913662b1f | 42 | double Larpowabs = Abs(Larpow); |
| AK1412 | 1:797913662b1f | 43 | double Luppowabs = Abs(Luppow); |
| AK1412 | 1:797913662b1f | 44 | |
| AK1412 | 1:797913662b1f | 45 | if (Larpowabs > Luppowabs){ |
| AK1412 | 1:797913662b1f | 46 | if (Larpow < 0){ |
| AK1412 | 1:797913662b1f | 47 | dir = 0; |
| AK1412 | 1:797913662b1f | 48 | pow = Larpow*(-1); |
| AK1412 | 1:797913662b1f | 49 | } |
| AK1412 | 1:797913662b1f | 50 | else if (Larpow > 0){ |
| AK1412 | 1:797913662b1f | 51 | dir = 1; |
| AK1412 | 1:797913662b1f | 52 | pow = Larpow; |
| AK1412 | 1:797913662b1f | 53 | } |
| AK1412 | 1:797913662b1f | 54 | } |
| AK1412 | 1:797913662b1f | 55 | else { |
| AK1412 | 1:797913662b1f | 56 | if (Luppow < 0){ |
| AK1412 | 1:797913662b1f | 57 | dir = 2; |
| AK1412 | 1:797913662b1f | 58 | pow = Luppow*(-1); |
| AK1412 | 1:797913662b1f | 59 | } |
| AK1412 | 1:797913662b1f | 60 | else if (Luppow > 0){ |
| AK1412 | 1:797913662b1f | 61 | dir = 3; |
| AK1412 | 1:797913662b1f | 62 | pow = Luppow; |
| AK1412 | 1:797913662b1f | 63 | } |
| AK1412 | 1:797913662b1f | 64 | } |
| AK1412 | 1:797913662b1f | 65 | |
| AK1412 | 1:797913662b1f | 66 | pow = map(pow,0.0,255.0,0.0,1.0); |
| AK1412 | 1:797913662b1f | 67 | |
| AK1412 | 1:797913662b1f | 68 | switch (dir){ |
| AK1412 | 1:797913662b1f | 69 | case 0 : |
| AK1412 | 1:797913662b1f | 70 | rifrpl = 0; |
| AK1412 | 1:797913662b1f | 71 | rifrmi = pow; |
| AK1412 | 1:797913662b1f | 72 | ribapl = pow; |
| AK1412 | 1:797913662b1f | 73 | ribami = 0; |
| AK1412 | 1:797913662b1f | 74 | lefrpl = pow; |
| AK1412 | 1:797913662b1f | 75 | lefrmi = 0; |
| AK1412 | 1:797913662b1f | 76 | lebapl = 0; |
| AK1412 | 1:797913662b1f | 77 | lebami = pow; |
| AK1412 | 1:797913662b1f | 78 | break; |
| AK1412 | 1:797913662b1f | 79 | case 1 : |
| AK1412 | 1:797913662b1f | 80 | rifrpl = pow; |
| AK1412 | 1:797913662b1f | 81 | rifrmi = 0; |
| AK1412 | 1:797913662b1f | 82 | ribapl = 0; |
| AK1412 | 1:797913662b1f | 83 | ribami = pow; |
| AK1412 | 1:797913662b1f | 84 | lefrpl = 0; |
| AK1412 | 1:797913662b1f | 85 | lefrmi = pow; |
| AK1412 | 1:797913662b1f | 86 | lebapl = pow; |
| AK1412 | 1:797913662b1f | 87 | lebami = 0; |
| AK1412 | 1:797913662b1f | 88 | break; |
| AK1412 | 1:797913662b1f | 89 | case 2 : |
| AK1412 | 1:797913662b1f | 90 | rifrpl = 0; |
| AK1412 | 1:797913662b1f | 91 | rifrmi = pow; |
| AK1412 | 1:797913662b1f | 92 | ribapl = 0; |
| AK1412 | 1:797913662b1f | 93 | ribami = pow; |
| AK1412 | 1:797913662b1f | 94 | lefrpl = 0; |
| AK1412 | 1:797913662b1f | 95 | lefrmi = pow; |
| AK1412 | 1:797913662b1f | 96 | lebapl = 0; |
| AK1412 | 1:797913662b1f | 97 | lebami = pow; |
| AK1412 | 1:797913662b1f | 98 | break; |
| AK1412 | 1:797913662b1f | 99 | case 3 : |
| AK1412 | 1:797913662b1f | 100 | rifrpl = pow; |
| AK1412 | 1:797913662b1f | 101 | rifrmi = 0; |
| AK1412 | 1:797913662b1f | 102 | ribapl = pow; |
| AK1412 | 1:797913662b1f | 103 | ribami = 0; |
| AK1412 | 1:797913662b1f | 104 | lefrpl = pow; |
| AK1412 | 1:797913662b1f | 105 | lefrmi = 0; |
| AK1412 | 1:797913662b1f | 106 | lebapl = pow; |
| AK1412 | 1:797913662b1f | 107 | lebami = 0; |
| AK1412 | 1:797913662b1f | 108 | break; |
| AK1412 | 1:797913662b1f | 109 | } |
| AK1412 | 1:797913662b1f | 110 | |
| AK1412 | 1:797913662b1f | 111 | bool ser = kbt.Button[circle]; |
| AK1412 | 1:797913662b1f | 112 | servo.period_ms(20); |
| AK1412 | 1:797913662b1f | 113 | if (ser == 1){ |
| AK1412 | 1:797913662b1f | 114 | servo.pulsewidth_us(700); |
| AK1412 | 1:797913662b1f | 115 | wait(8); |
| AK1412 | 1:797913662b1f | 116 | servo.pulsewidth_us(1500); |
| AK1412 | 1:797913662b1f | 117 | wait(8); |
| AK1412 | 1:797913662b1f | 118 | } |
| AK1412 | 1:797913662b1f | 119 | |
| AK1412 | 1:797913662b1f | 120 | bool ai = kbt.Button[triangle]; |
| AK1412 | 1:797913662b1f | 121 | if (ai == 1) |
| AK1412 | 1:797913662b1f | 122 | air = 1; |
| AK1412 | 1:797913662b1f | 123 | else |
| AK1412 | 1:797913662b1f | 124 | air = 0; |
| AK1412 | 1:797913662b1f | 125 | } |
| AK1412 | 1:797913662b1f | 126 | } |