Jose Eduardo Laruta Espejo
/
maple_multiservos
secuencia para control de 2 patas roboticas de 2 grados de libertad
main.cpp@0:3fdd6444be32, 2016-04-24 (annotated)
- Committer:
- tabris2015
- Date:
- Sun Apr 24 14:33:19 2016 +0000
- Revision:
- 0:3fdd6444be32
secuencia inicial
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
tabris2015 | 0:3fdd6444be32 | 1 | #include "mbed.h" |
tabris2015 | 0:3fdd6444be32 | 2 | #define pulse(theta) (700 + 10 * theta) |
tabris2015 | 0:3fdd6444be32 | 3 | /* aaa */ |
tabris2015 | 0:3fdd6444be32 | 4 | PwmOut caderaRight(PA_3); |
tabris2015 | 0:3fdd6444be32 | 5 | PwmOut piernaRight(PA_2); |
tabris2015 | 0:3fdd6444be32 | 6 | PwmOut caderaLeft(PA_1); |
tabris2015 | 0:3fdd6444be32 | 7 | PwmOut piernaLeft(PB_0); |
tabris2015 | 0:3fdd6444be32 | 8 | |
tabris2015 | 0:3fdd6444be32 | 9 | |
tabris2015 | 0:3fdd6444be32 | 10 | DigitalOut myled(PB_1); |
tabris2015 | 0:3fdd6444be32 | 11 | |
tabris2015 | 0:3fdd6444be32 | 12 | int main() { |
tabris2015 | 0:3fdd6444be32 | 13 | /* */ |
tabris2015 | 0:3fdd6444be32 | 14 | caderaRight.period(0.020); |
tabris2015 | 0:3fdd6444be32 | 15 | piernaRight.period(0.020); |
tabris2015 | 0:3fdd6444be32 | 16 | caderaLeft.pulsewidth(pulse(0)); |
tabris2015 | 0:3fdd6444be32 | 17 | piernaLeft.pulsewidth(pulse(90)); |
tabris2015 | 0:3fdd6444be32 | 18 | wait(2); |
tabris2015 | 0:3fdd6444be32 | 19 | int init = pulse(45); |
tabris2015 | 0:3fdd6444be32 | 20 | int backRight = pulse(45); |
tabris2015 | 0:3fdd6444be32 | 21 | int frontRight = pulse(135); |
tabris2015 | 0:3fdd6444be32 | 22 | int backLeft = pulse(135); |
tabris2015 | 0:3fdd6444be32 | 23 | int frontLeft = pulse(45); |
tabris2015 | 0:3fdd6444be32 | 24 | int upRight = pulse(90); |
tabris2015 | 0:3fdd6444be32 | 25 | int upLeft = pulse(90); |
tabris2015 | 0:3fdd6444be32 | 26 | int downRight = pulse(20); |
tabris2015 | 0:3fdd6444be32 | 27 | int downLeft = pulse(160); |
tabris2015 | 0:3fdd6444be32 | 28 | |
tabris2015 | 0:3fdd6444be32 | 29 | int fin = pulse(135); |
tabris2015 | 0:3fdd6444be32 | 30 | float tDelay = 0.2; |
tabris2015 | 0:3fdd6444be32 | 31 | |
tabris2015 | 0:3fdd6444be32 | 32 | while(1) { |
tabris2015 | 0:3fdd6444be32 | 33 | |
tabris2015 | 0:3fdd6444be32 | 34 | // (1) Left: D - B ; Right: U - F |
tabris2015 | 0:3fdd6444be32 | 35 | piernaLeft.pulsewidth_us(downLeft); |
tabris2015 | 0:3fdd6444be32 | 36 | caderaLeft.pulsewidth_us(backLeft); |
tabris2015 | 0:3fdd6444be32 | 37 | piernaRight.pulsewidth_us(upRight); |
tabris2015 | 0:3fdd6444be32 | 38 | caderaRight.pulsewidth_us(frontRight); |
tabris2015 | 0:3fdd6444be32 | 39 | wait(tDelay); |
tabris2015 | 0:3fdd6444be32 | 40 | // (2) Left: D - B ; Right: D - F |
tabris2015 | 0:3fdd6444be32 | 41 | piernaLeft.pulsewidth_us(downLeft); |
tabris2015 | 0:3fdd6444be32 | 42 | caderaLeft.pulsewidth_us(backLeft); |
tabris2015 | 0:3fdd6444be32 | 43 | piernaRight.pulsewidth_us(downRight); |
tabris2015 | 0:3fdd6444be32 | 44 | caderaRight.pulsewidth_us(frontRight); |
tabris2015 | 0:3fdd6444be32 | 45 | wait(tDelay); |
tabris2015 | 0:3fdd6444be32 | 46 | // (3) Left: U - B ; Right: D - F |
tabris2015 | 0:3fdd6444be32 | 47 | piernaLeft.pulsewidth_us(upLeft); |
tabris2015 | 0:3fdd6444be32 | 48 | caderaLeft.pulsewidth_us(backLeft); |
tabris2015 | 0:3fdd6444be32 | 49 | piernaRight.pulsewidth_us(downRight); |
tabris2015 | 0:3fdd6444be32 | 50 | caderaRight.pulsewidth_us(frontRight); |
tabris2015 | 0:3fdd6444be32 | 51 | wait(tDelay); |
tabris2015 | 0:3fdd6444be32 | 52 | // (4) Left: U - F ; Right: D - B |
tabris2015 | 0:3fdd6444be32 | 53 | piernaLeft.pulsewidth_us(upLeft); |
tabris2015 | 0:3fdd6444be32 | 54 | caderaLeft.pulsewidth_us(frontLeft); |
tabris2015 | 0:3fdd6444be32 | 55 | piernaRight.pulsewidth_us(downRight); |
tabris2015 | 0:3fdd6444be32 | 56 | caderaRight.pulsewidth_us(backRight); |
tabris2015 | 0:3fdd6444be32 | 57 | wait(tDelay); |
tabris2015 | 0:3fdd6444be32 | 58 | |
tabris2015 | 0:3fdd6444be32 | 59 | // (5) Left: D - F ; Right: D - B |
tabris2015 | 0:3fdd6444be32 | 60 | piernaLeft.pulsewidth_us(downLeft); |
tabris2015 | 0:3fdd6444be32 | 61 | caderaLeft.pulsewidth_us(frontLeft); |
tabris2015 | 0:3fdd6444be32 | 62 | piernaRight.pulsewidth_us(downRight); |
tabris2015 | 0:3fdd6444be32 | 63 | caderaRight.pulsewidth_us(backRight); |
tabris2015 | 0:3fdd6444be32 | 64 | wait(tDelay); |
tabris2015 | 0:3fdd6444be32 | 65 | |
tabris2015 | 0:3fdd6444be32 | 66 | // (6) Left: D - F ; Right: U - B |
tabris2015 | 0:3fdd6444be32 | 67 | piernaLeft.pulsewidth_us(downLeft); |
tabris2015 | 0:3fdd6444be32 | 68 | caderaLeft.pulsewidth_us(frontLeft); |
tabris2015 | 0:3fdd6444be32 | 69 | piernaRight.pulsewidth_us(upRight); |
tabris2015 | 0:3fdd6444be32 | 70 | caderaRight.pulsewidth_us(backRight); |
tabris2015 | 0:3fdd6444be32 | 71 | wait(tDelay); |
tabris2015 | 0:3fdd6444be32 | 72 | |
tabris2015 | 0:3fdd6444be32 | 73 | myled = 1; |
tabris2015 | 0:3fdd6444be32 | 74 | } |
tabris2015 | 0:3fdd6444be32 | 75 | } |