Mekatronics Team G
Dependencies: BNO055_fusion PowerControl mbed BMP280
Fork of DEMO3 by
LOCOMOTION.cpp@16:d6f15a13c3aa, 2016-03-27 (annotated)
- Committer:
- 12104404
- Date:
- Sun Mar 27 02:39:07 2016 +0000
- Revision:
- 16:d6f15a13c3aa
- Parent:
- 15:7729da55873a
- Child:
- 17:2f89826b5679
ANGLE PORTED and tested
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
12104404 | 6:0602a9e8118b | 1 | #include "LOCOMOTION.h" |
12104404 | 15:7729da55873a | 2 | |
12104404 | 15:7729da55873a | 3 | LOCOMOTION::LOCOMOTION (PinName motor1F, PinName motor1B, PinName motor2F, PinName motor2B, PinName forward1, PinName forward2): |
12104404 | 16:d6f15a13c3aa | 4 | _m1f(motor1F), _m1b(motor1B), _m2f(motor2F), _m2b(motor2B), _m1dir(forward1), _m2dir(forward2) |
12104404 | 15:7729da55873a | 5 | { |
12104404 | 15:7729da55873a | 6 | _m1f=0; |
12104404 | 15:7729da55873a | 7 | _m1b=0; |
12104404 | 15:7729da55873a | 8 | _m2f=0; |
12104404 | 15:7729da55873a | 9 | _m2b=0; |
12104404 | 15:7729da55873a | 10 | _m1dir=0; |
12104404 | 15:7729da55873a | 11 | _m2dir=0; |
12104404 | 15:7729da55873a | 12 | } |
12104404 | 15:7729da55873a | 13 | |
12104404 | 15:7729da55873a | 14 | bool LOCOMOTION::setAngle(int target, int current, int error, int mode) |
12104404 | 15:7729da55873a | 15 | { |
12104404 | 15:7729da55873a | 16 | float s = 0; |
12104404 | 15:7729da55873a | 17 | int diff = 0; |
12104404 | 15:7729da55873a | 18 | diff = 180-wrap(target); |
12104404 | 15:7729da55873a | 19 | if(abs(wrap(current+diff)-180)<=error) |
12104404 | 15:7729da55873a | 20 | s=SPEED_TURN_MIN; |
12104404 | 15:7729da55873a | 21 | else |
12104404 | 15:7729da55873a | 22 | s=((SPEED_TURN_MAX-SPEED_TURN_MIN)*abs(wrap(current+diff)-180)/180)+SPEED_TURN_MIN; |
12104404 | 15:7729da55873a | 23 | switch(mode) { |
12104404 | 15:7729da55873a | 24 | case ANGLE_TURN: |
12104404 | 16:d6f15a13c3aa | 25 | if(wrap(current+diff)>180+error) { |
12104404 | 16:d6f15a13c3aa | 26 | _m1dir=1; |
12104404 | 16:d6f15a13c3aa | 27 | _m2dir=0; |
12104404 | 16:d6f15a13c3aa | 28 | _m1f=s; |
12104404 | 16:d6f15a13c3aa | 29 | _m1b=s; |
12104404 | 16:d6f15a13c3aa | 30 | _m2f=s; |
12104404 | 16:d6f15a13c3aa | 31 | _m2b=s; |
12104404 | 16:d6f15a13c3aa | 32 | } else if(wrap(current+diff)<180-error) { |
12104404 | 16:d6f15a13c3aa | 33 | _m1dir=0; |
12104404 | 16:d6f15a13c3aa | 34 | _m2dir=1; |
12104404 | 16:d6f15a13c3aa | 35 | _m1f=s; |
12104404 | 16:d6f15a13c3aa | 36 | _m1b=s; |
12104404 | 16:d6f15a13c3aa | 37 | _m2f=s; |
12104404 | 16:d6f15a13c3aa | 38 | _m2b=s; |
12104404 | 16:d6f15a13c3aa | 39 | } else { |
12104404 | 16:d6f15a13c3aa | 40 | s=0; |
12104404 | 16:d6f15a13c3aa | 41 | _m1f=s; |
12104404 | 16:d6f15a13c3aa | 42 | _m1b=s; |
12104404 | 16:d6f15a13c3aa | 43 | _m2f=s; |
12104404 | 16:d6f15a13c3aa | 44 | _m2b=s; |
12104404 | 16:d6f15a13c3aa | 45 | return true; |
12104404 | 16:d6f15a13c3aa | 46 | } |
12104404 | 15:7729da55873a | 47 | break; |
12104404 | 15:7729da55873a | 48 | case ANGLE_BIAS: |
12104404 | 15:7729da55873a | 49 | |
12104404 | 15:7729da55873a | 50 | break; |
12104404 | 15:7729da55873a | 51 | } |
12104404 | 16:d6f15a13c3aa | 52 | return false; |
12104404 | 15:7729da55873a | 53 | } |
12104404 | 15:7729da55873a | 54 | |
12104404 | 15:7729da55873a | 55 | int LOCOMOTION::wrap(int num) |
12104404 | 15:7729da55873a | 56 | { |
12104404 | 15:7729da55873a | 57 | return num%360; |
12104404 | 15:7729da55873a | 58 | } |
12104404 | 6:0602a9e8118b | 59 | /* |
12104404 | 15:7729da55873a | 60 | void setAngle(int angle) |
12104404 | 15:7729da55873a | 61 | { |
12104404 | 15:7729da55873a | 62 | float s = 0; |
12104404 | 15:7729da55873a | 63 | int diff = 0; |
12104404 | 15:7729da55873a | 64 | diff = 180-wrap(angle); |
12104404 | 15:7729da55873a | 65 | if(abs(wrap(xya.a+diff)-180)<=5) |
12104404 | 15:7729da55873a | 66 | s=SPEED_TURN_MIN; |
12104404 | 15:7729da55873a | 67 | else |
12104404 | 15:7729da55873a | 68 | s=((SPEED_TURN_MAX-SPEED_TURN_MIN)*abs(wrap(xya.a+diff)-180)/180)+SPEED_TURN_MIN; |
12104404 | 15:7729da55873a | 69 | motor1F=s; |
12104404 | 15:7729da55873a | 70 | motor1B=s; |
12104404 | 15:7729da55873a | 71 | motor2F=s; |
12104404 | 15:7729da55873a | 72 | motor2B=s; |
12104404 | 15:7729da55873a | 73 | if(wrap(xya.a+diff)>180+2) { |
12104404 | 15:7729da55873a | 74 | dir1=1; |
12104404 | 15:7729da55873a | 75 | dir2=0; |
12104404 | 15:7729da55873a | 76 | } else if(wrap(xya.a+diff)<180-2) { |
12104404 | 15:7729da55873a | 77 | dir1=0; |
12104404 | 15:7729da55873a | 78 | dir2=1; |
12104404 | 15:7729da55873a | 79 | } else { |
12104404 | 15:7729da55873a | 80 | motor1F=0; |
12104404 | 15:7729da55873a | 81 | motor1B=0; |
12104404 | 15:7729da55873a | 82 | motor2F=0; |
12104404 | 15:7729da55873a | 83 | motor2B=0; |
12104404 | 15:7729da55873a | 84 | }*/ |