uhbduhbv
Dependencies: BNO055_fusion mbed
Fork of DEMO3 by
LOCOMOTION.cpp@17:2f89826b5679, 2016-03-29 (annotated)
- Committer:
- 12104404
- Date:
- Tue Mar 29 01:11:09 2016 +0000
- Revision:
- 17:2f89826b5679
- Parent:
- 16:d6f15a13c3aa
- Child:
- 18:f9012e93edb8
working
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 | 17:2f89826b5679 | 14 | bool LOCOMOTION::setXPos(int target, int current, int error) |
12104404 | 17:2f89826b5679 | 15 | { |
12104404 | 17:2f89826b5679 | 16 | //s = 0; |
12104404 | 17:2f89826b5679 | 17 | if(abs(current-target)<=error) |
12104404 | 17:2f89826b5679 | 18 | s=0.07; |
12104404 | 17:2f89826b5679 | 19 | else |
12104404 | 17:2f89826b5679 | 20 | s=((0.17-0.07)*abs(current-target)/FRAME_W)+0.07; |
12104404 | 17:2f89826b5679 | 21 | if(current>target+error) { |
12104404 | 17:2f89826b5679 | 22 | _m1dir=1; |
12104404 | 17:2f89826b5679 | 23 | _m2dir=1; |
12104404 | 17:2f89826b5679 | 24 | _m1f=s; |
12104404 | 17:2f89826b5679 | 25 | _m1b=s; |
12104404 | 17:2f89826b5679 | 26 | _m2f=s; |
12104404 | 17:2f89826b5679 | 27 | _m2b=s; |
12104404 | 17:2f89826b5679 | 28 | } else if(current<target-error) { |
12104404 | 17:2f89826b5679 | 29 | _m1dir=0; |
12104404 | 17:2f89826b5679 | 30 | _m2dir=0; |
12104404 | 17:2f89826b5679 | 31 | _m1f=s; |
12104404 | 17:2f89826b5679 | 32 | _m1b=s; |
12104404 | 17:2f89826b5679 | 33 | _m2f=s; |
12104404 | 17:2f89826b5679 | 34 | _m2b=s; |
12104404 | 17:2f89826b5679 | 35 | } else { |
12104404 | 17:2f89826b5679 | 36 | s=0; |
12104404 | 17:2f89826b5679 | 37 | _m1f=s; |
12104404 | 17:2f89826b5679 | 38 | _m1b=s; |
12104404 | 17:2f89826b5679 | 39 | _m2f=s; |
12104404 | 17:2f89826b5679 | 40 | _m2b=s; |
12104404 | 17:2f89826b5679 | 41 | return true; |
12104404 | 17:2f89826b5679 | 42 | } |
12104404 | 17:2f89826b5679 | 43 | return false; |
12104404 | 17:2f89826b5679 | 44 | } |
12104404 | 17:2f89826b5679 | 45 | |
12104404 | 17:2f89826b5679 | 46 | bool LOCOMOTION::setYPos(int target, int current, int error) |
12104404 | 17:2f89826b5679 | 47 | { |
12104404 | 17:2f89826b5679 | 48 | //float s = 0; |
12104404 | 17:2f89826b5679 | 49 | if(abs(current-target)<=error) |
12104404 | 17:2f89826b5679 | 50 | s=0.50; |
12104404 | 17:2f89826b5679 | 51 | else |
12104404 | 17:2f89826b5679 | 52 | s=((1-0.50)*abs(current-target)/FRAME_H)+0.50; |
12104404 | 17:2f89826b5679 | 53 | if(current>target+error) { |
12104404 | 17:2f89826b5679 | 54 | //_m1dir=1; |
12104404 | 17:2f89826b5679 | 55 | //_m2dir=1; |
12104404 | 17:2f89826b5679 | 56 | _m1f=_m1f*(1+s); |
12104404 | 17:2f89826b5679 | 57 | _m1b=_m1b*(1+s); |
12104404 | 17:2f89826b5679 | 58 | } else if(current<target-error) { |
12104404 | 17:2f89826b5679 | 59 | //_m1dir=0; |
12104404 | 17:2f89826b5679 | 60 | //_m2dir=0; |
12104404 | 17:2f89826b5679 | 61 | _m2f=_m2f*(1+s); |
12104404 | 17:2f89826b5679 | 62 | _m2b=_m2b*(1+s); |
12104404 | 17:2f89826b5679 | 63 | } else { |
12104404 | 17:2f89826b5679 | 64 | s=0; |
12104404 | 17:2f89826b5679 | 65 | |
12104404 | 17:2f89826b5679 | 66 | return true; |
12104404 | 17:2f89826b5679 | 67 | } |
12104404 | 17:2f89826b5679 | 68 | return false; |
12104404 | 17:2f89826b5679 | 69 | } |
12104404 | 17:2f89826b5679 | 70 | |
12104404 | 15:7729da55873a | 71 | bool LOCOMOTION::setAngle(int target, int current, int error, int mode) |
12104404 | 15:7729da55873a | 72 | { |
12104404 | 17:2f89826b5679 | 73 | s = 0; |
12104404 | 15:7729da55873a | 74 | int diff = 0; |
12104404 | 15:7729da55873a | 75 | diff = 180-wrap(target); |
12104404 | 15:7729da55873a | 76 | if(abs(wrap(current+diff)-180)<=error) |
12104404 | 15:7729da55873a | 77 | s=SPEED_TURN_MIN; |
12104404 | 15:7729da55873a | 78 | else |
12104404 | 15:7729da55873a | 79 | s=((SPEED_TURN_MAX-SPEED_TURN_MIN)*abs(wrap(current+diff)-180)/180)+SPEED_TURN_MIN; |
12104404 | 15:7729da55873a | 80 | switch(mode) { |
12104404 | 15:7729da55873a | 81 | case ANGLE_TURN: |
12104404 | 16:d6f15a13c3aa | 82 | if(wrap(current+diff)>180+error) { |
12104404 | 16:d6f15a13c3aa | 83 | _m1dir=1; |
12104404 | 16:d6f15a13c3aa | 84 | _m2dir=0; |
12104404 | 16:d6f15a13c3aa | 85 | _m1f=s; |
12104404 | 16:d6f15a13c3aa | 86 | _m1b=s; |
12104404 | 16:d6f15a13c3aa | 87 | _m2f=s; |
12104404 | 16:d6f15a13c3aa | 88 | _m2b=s; |
12104404 | 16:d6f15a13c3aa | 89 | } else if(wrap(current+diff)<180-error) { |
12104404 | 16:d6f15a13c3aa | 90 | _m1dir=0; |
12104404 | 16:d6f15a13c3aa | 91 | _m2dir=1; |
12104404 | 16:d6f15a13c3aa | 92 | _m1f=s; |
12104404 | 16:d6f15a13c3aa | 93 | _m1b=s; |
12104404 | 16:d6f15a13c3aa | 94 | _m2f=s; |
12104404 | 16:d6f15a13c3aa | 95 | _m2b=s; |
12104404 | 16:d6f15a13c3aa | 96 | } else { |
12104404 | 16:d6f15a13c3aa | 97 | s=0; |
12104404 | 16:d6f15a13c3aa | 98 | _m1f=s; |
12104404 | 16:d6f15a13c3aa | 99 | _m1b=s; |
12104404 | 16:d6f15a13c3aa | 100 | _m2f=s; |
12104404 | 16:d6f15a13c3aa | 101 | _m2b=s; |
12104404 | 16:d6f15a13c3aa | 102 | return true; |
12104404 | 16:d6f15a13c3aa | 103 | } |
12104404 | 15:7729da55873a | 104 | break; |
12104404 | 15:7729da55873a | 105 | case ANGLE_BIAS: |
12104404 | 15:7729da55873a | 106 | |
12104404 | 15:7729da55873a | 107 | break; |
12104404 | 15:7729da55873a | 108 | } |
12104404 | 16:d6f15a13c3aa | 109 | return false; |
12104404 | 15:7729da55873a | 110 | } |
12104404 | 15:7729da55873a | 111 | |
12104404 | 15:7729da55873a | 112 | int LOCOMOTION::wrap(int num) |
12104404 | 15:7729da55873a | 113 | { |
12104404 | 15:7729da55873a | 114 | return num%360; |
12104404 | 17:2f89826b5679 | 115 | } |