
アームのスライド移動 まだ片方のアームしか実装されてないけど コピーすれば終わる。
Dependencies: mbed EC_speedcontroller_syuuseiban
main.cpp@0:d4deb703d536, 2019-01-16 (annotated)
- Committer:
- ryota1999
- Date:
- Wed Jan 16 11:17:34 2019 +0000
- Revision:
- 0:d4deb703d536
aa
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ryota1999 | 0:d4deb703d536 | 1 | #include "mbed.h" |
ryota1999 | 0:d4deb703d536 | 2 | #include "EC.h" |
ryota1999 | 0:d4deb703d536 | 3 | #include "math.h" |
ryota1999 | 0:d4deb703d536 | 4 | |
ryota1999 | 0:d4deb703d536 | 5 | PwmOut motor1F(PC_9); |
ryota1999 | 0:d4deb703d536 | 6 | PwmOut motor1B(PC_8); |
ryota1999 | 0:d4deb703d536 | 7 | PwmOut motor2F(PC_7); |
ryota1999 | 0:d4deb703d536 | 8 | PwmOut motor2B(PC_6); |
ryota1999 | 0:d4deb703d536 | 9 | Ec ec1(PA_12, PA_11, NC, 500, 0.05); |
ryota1999 | 0:d4deb703d536 | 10 | Ec ec2(PC_4, PB_14, NC, 500, 0.05); |
ryota1999 | 0:d4deb703d536 | 11 | Ticker ticker; |
ryota1999 | 0:d4deb703d536 | 12 | int tag1 = -100; |
ryota1999 | 0:d4deb703d536 | 13 | int tag2 = 100; |
ryota1999 | 0:d4deb703d536 | 14 | int gap1; |
ryota1999 | 0:d4deb703d536 | 15 | int gap2; |
ryota1999 | 0:d4deb703d536 | 16 | int a=0; |
ryota1999 | 0:d4deb703d536 | 17 | int i=1; |
ryota1999 | 0:d4deb703d536 | 18 | |
ryota1999 | 0:d4deb703d536 | 19 | void calomega() { |
ryota1999 | 0:d4deb703d536 | 20 | ec1.CalOmega(); |
ryota1999 | 0:d4deb703d536 | 21 | ec2.CalOmega(); |
ryota1999 | 0:d4deb703d536 | 22 | a=a+1; |
ryota1999 | 0:d4deb703d536 | 23 | if(a>=30&&i==1){ |
ryota1999 | 0:d4deb703d536 | 24 | tag1=-400; |
ryota1999 | 0:d4deb703d536 | 25 | a=0; |
ryota1999 | 0:d4deb703d536 | 26 | i=0; |
ryota1999 | 0:d4deb703d536 | 27 | } |
ryota1999 | 0:d4deb703d536 | 28 | if(a>=30&&i==0){ |
ryota1999 | 0:d4deb703d536 | 29 | tag1=-100; |
ryota1999 | 0:d4deb703d536 | 30 | a=0; |
ryota1999 | 0:d4deb703d536 | 31 | i=1; |
ryota1999 | 0:d4deb703d536 | 32 | }} |
ryota1999 | 0:d4deb703d536 | 33 | |
ryota1999 | 0:d4deb703d536 | 34 | int main() { |
ryota1999 | 0:d4deb703d536 | 35 | ticker.attach(&calomega, 0.05); |
ryota1999 | 0:d4deb703d536 | 36 | motor1F.period_us(50); |
ryota1999 | 0:d4deb703d536 | 37 | motor1B.period_us(50); |
ryota1999 | 0:d4deb703d536 | 38 | motor2F.period_us(50); |
ryota1999 | 0:d4deb703d536 | 39 | motor2B.period_us(50); |
ryota1999 | 0:d4deb703d536 | 40 | while (1) { |
ryota1999 | 0:d4deb703d536 | 41 | printf("count=%d\r\n", ec1.getCount()); |
ryota1999 | 0:d4deb703d536 | 42 | gap1 = tag1 - ec1.getCount(); |
ryota1999 | 0:d4deb703d536 | 43 | gap2 = tag2 - ec2.getCount(); |
ryota1999 | 0:d4deb703d536 | 44 | if (gap1 > 0) { |
ryota1999 | 0:d4deb703d536 | 45 | if (gap1 > 50) { |
ryota1999 | 0:d4deb703d536 | 46 | motor1F = 0; |
ryota1999 | 0:d4deb703d536 | 47 | motor1B = 0.3; |
ryota1999 | 0:d4deb703d536 | 48 | } |
ryota1999 | 0:d4deb703d536 | 49 | if (gap1 < 50) { |
ryota1999 | 0:d4deb703d536 | 50 | motor1F = 0; |
ryota1999 | 0:d4deb703d536 | 51 | motor1B = gap1 * 0.005; |
ryota1999 | 0:d4deb703d536 | 52 | } |
ryota1999 | 0:d4deb703d536 | 53 | } |
ryota1999 | 0:d4deb703d536 | 54 | if (gap1 < 0) { |
ryota1999 | 0:d4deb703d536 | 55 | if (gap1 < -50) { |
ryota1999 | 0:d4deb703d536 | 56 | motor1F = 0.3; |
ryota1999 | 0:d4deb703d536 | 57 | motor1B = 0; |
ryota1999 | 0:d4deb703d536 | 58 | } |
ryota1999 | 0:d4deb703d536 | 59 | if (gap1 > -50) { |
ryota1999 | 0:d4deb703d536 | 60 | motor1F = -gap1 * 0.005; |
ryota1999 | 0:d4deb703d536 | 61 | motor1B = 0; |
ryota1999 | 0:d4deb703d536 | 62 | } |
ryota1999 | 0:d4deb703d536 | 63 | } |
ryota1999 | 0:d4deb703d536 | 64 | } |
ryota1999 | 0:d4deb703d536 | 65 | } |
ryota1999 | 0:d4deb703d536 | 66 |