Mekatronics Team G
Dependencies: BNO055_fusion PowerControl mbed BMP280
Fork of DEMO3 by
LOCOMOTION.cpp@24:fb1f083ebd62, 2016-04-03 (annotated)
- Committer:
- 12104404
- Date:
- Sun Apr 03 07:25:07 2016 +0000
- Revision:
- 24:fb1f083ebd62
- Parent:
- 23:455f7da3dd7a
- Child:
- 25:f3bf8351bbd4
Added LED Error Indicators
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 | 24:fb1f083ebd62 | 3 | LOCOMOTION::LOCOMOTION (PinName en, PinName motor1F, PinName motor1B, PinName motor2F, PinName motor2B, PinName forward1, PinName forward2, DigitalOut& led1, DigitalOut& led2, DigitalOut& led3, DigitalOut& led4): |
12104404 | 24:fb1f083ebd62 | 4 | _en(en), _m1f(motor1F), _m1b(motor1B), _m2f(motor2F), _m2b(motor2B), _m1dir(forward1), _m2dir(forward2), _led1(led1), _led2(led2), _led3(led3), _led4(led4) |
12104404 | 23:455f7da3dd7a | 5 | { |
12104404 | 23:455f7da3dd7a | 6 | s=0; |
12104404 | 23:455f7da3dd7a | 7 | stopMotors(); |
12104404 | 23:455f7da3dd7a | 8 | _m1dir=0; |
12104404 | 23:455f7da3dd7a | 9 | _m2dir=0; |
12104404 | 23:455f7da3dd7a | 10 | _en=1; |
12104404 | 24:fb1f083ebd62 | 11 | _led1=1; |
12104404 | 24:fb1f083ebd62 | 12 | wait(0.01); |
12104404 | 24:fb1f083ebd62 | 13 | _led1=0; |
12104404 | 23:455f7da3dd7a | 14 | } |
12104404 | 23:455f7da3dd7a | 15 | |
12104404 | 23:455f7da3dd7a | 16 | inline void LOCOMOTION::stopMotors(void) |
12104404 | 15:7729da55873a | 17 | { |
12104404 | 15:7729da55873a | 18 | _m1f=0; |
12104404 | 15:7729da55873a | 19 | _m1b=0; |
12104404 | 15:7729da55873a | 20 | _m2f=0; |
12104404 | 15:7729da55873a | 21 | _m2b=0; |
12104404 | 15:7729da55873a | 22 | } |
12104404 | 15:7729da55873a | 23 | |
12104404 | 23:455f7da3dd7a | 24 | |
12104404 | 23:455f7da3dd7a | 25 | inline int LOCOMOTION::wrap(int num) |
12104404 | 19:2dd81b864e14 | 26 | { |
12104404 | 23:455f7da3dd7a | 27 | return num%360; |
12104404 | 23:455f7da3dd7a | 28 | } |
12104404 | 23:455f7da3dd7a | 29 | |
12104404 | 23:455f7da3dd7a | 30 | void LOCOMOTION::eStop(void) |
12104404 | 23:455f7da3dd7a | 31 | { |
12104404 | 23:455f7da3dd7a | 32 | //Stop Motors |
12104404 | 23:455f7da3dd7a | 33 | stopMotors(); |
12104404 | 23:455f7da3dd7a | 34 | //Disable Motor Drivers |
12104404 | 23:455f7da3dd7a | 35 | _en=0; |
12104404 | 19:2dd81b864e14 | 36 | } |
12104404 | 19:2dd81b864e14 | 37 | |
12104404 | 18:f9012e93edb8 | 38 | bool LOCOMOTION::setXPos(int target, int current, int error, int angle) |
12104404 | 17:2f89826b5679 | 39 | { |
12104404 | 17:2f89826b5679 | 40 | //s = 0; |
12104404 | 17:2f89826b5679 | 41 | if(abs(current-target)<=error) |
12104404 | 17:2f89826b5679 | 42 | s=0.07; |
12104404 | 17:2f89826b5679 | 43 | else |
12104404 | 17:2f89826b5679 | 44 | s=((0.17-0.07)*abs(current-target)/FRAME_W)+0.07; |
12104404 | 17:2f89826b5679 | 45 | if(current>target+error) { |
12104404 | 18:f9012e93edb8 | 46 | if(angle==0) { |
12104404 | 18:f9012e93edb8 | 47 | _m1dir=1; |
12104404 | 18:f9012e93edb8 | 48 | _m2dir=1; |
12104404 | 18:f9012e93edb8 | 49 | } else { |
12104404 | 18:f9012e93edb8 | 50 | _m1dir=0; |
12104404 | 18:f9012e93edb8 | 51 | _m2dir=0; |
12104404 | 18:f9012e93edb8 | 52 | } |
12104404 | 17:2f89826b5679 | 53 | _m1f=s; |
12104404 | 17:2f89826b5679 | 54 | _m1b=s; |
12104404 | 17:2f89826b5679 | 55 | _m2f=s; |
12104404 | 17:2f89826b5679 | 56 | _m2b=s; |
12104404 | 17:2f89826b5679 | 57 | } else if(current<target-error) { |
12104404 | 18:f9012e93edb8 | 58 | if(angle==0) { |
12104404 | 18:f9012e93edb8 | 59 | _m1dir=0; |
12104404 | 18:f9012e93edb8 | 60 | _m2dir=0; |
12104404 | 18:f9012e93edb8 | 61 | } else { |
12104404 | 18:f9012e93edb8 | 62 | _m1dir=1; |
12104404 | 18:f9012e93edb8 | 63 | _m2dir=1; |
12104404 | 18:f9012e93edb8 | 64 | } |
12104404 | 17:2f89826b5679 | 65 | _m1f=s; |
12104404 | 17:2f89826b5679 | 66 | _m1b=s; |
12104404 | 17:2f89826b5679 | 67 | _m2f=s; |
12104404 | 17:2f89826b5679 | 68 | _m2b=s; |
12104404 | 17:2f89826b5679 | 69 | } else { |
12104404 | 19:2dd81b864e14 | 70 | stopMotors(); |
12104404 | 17:2f89826b5679 | 71 | return true; |
12104404 | 17:2f89826b5679 | 72 | } |
12104404 | 17:2f89826b5679 | 73 | return false; |
12104404 | 17:2f89826b5679 | 74 | } |
12104404 | 17:2f89826b5679 | 75 | |
12104404 | 18:f9012e93edb8 | 76 | bool LOCOMOTION::setYPos(int target, int current, int error, int angle) |
12104404 | 17:2f89826b5679 | 77 | { |
12104404 | 17:2f89826b5679 | 78 | //float s = 0; |
12104404 | 17:2f89826b5679 | 79 | if(abs(current-target)<=error) |
12104404 | 17:2f89826b5679 | 80 | s=0.50; |
12104404 | 17:2f89826b5679 | 81 | else |
12104404 | 17:2f89826b5679 | 82 | s=((1-0.50)*abs(current-target)/FRAME_H)+0.50; |
12104404 | 17:2f89826b5679 | 83 | if(current>target+error) { |
12104404 | 17:2f89826b5679 | 84 | //_m1dir=1; |
12104404 | 17:2f89826b5679 | 85 | //_m2dir=1; |
12104404 | 18:f9012e93edb8 | 86 | if(angle==0) { |
12104404 | 18:f9012e93edb8 | 87 | _m1f=_m1f*(1+s); |
12104404 | 18:f9012e93edb8 | 88 | _m1b=_m1b*(1+s); |
12104404 | 18:f9012e93edb8 | 89 | } else { |
12104404 | 18:f9012e93edb8 | 90 | _m2f=_m2f*(1+s); |
12104404 | 18:f9012e93edb8 | 91 | _m2b=_m2b*(1+s); |
12104404 | 18:f9012e93edb8 | 92 | } |
12104404 | 17:2f89826b5679 | 93 | } else if(current<target-error) { |
12104404 | 17:2f89826b5679 | 94 | //_m1dir=0; |
12104404 | 17:2f89826b5679 | 95 | //_m2dir=0; |
12104404 | 18:f9012e93edb8 | 96 | if(angle==0) { |
12104404 | 18:f9012e93edb8 | 97 | _m2f=_m2f*(1+s); |
12104404 | 18:f9012e93edb8 | 98 | _m2b=_m2b*(1+s); |
12104404 | 18:f9012e93edb8 | 99 | } else { |
12104404 | 18:f9012e93edb8 | 100 | _m1f=_m1f*(1+s); |
12104404 | 18:f9012e93edb8 | 101 | _m1b=_m1b*(1+s); |
12104404 | 18:f9012e93edb8 | 102 | } |
12104404 | 17:2f89826b5679 | 103 | } else { |
12104404 | 17:2f89826b5679 | 104 | s=0; |
12104404 | 18:f9012e93edb8 | 105 | |
12104404 | 17:2f89826b5679 | 106 | return true; |
12104404 | 17:2f89826b5679 | 107 | } |
12104404 | 17:2f89826b5679 | 108 | return false; |
12104404 | 17:2f89826b5679 | 109 | } |
12104404 | 17:2f89826b5679 | 110 | |
12104404 | 15:7729da55873a | 111 | bool LOCOMOTION::setAngle(int target, int current, int error, int mode) |
12104404 | 15:7729da55873a | 112 | { |
12104404 | 17:2f89826b5679 | 113 | s = 0; |
12104404 | 15:7729da55873a | 114 | int diff = 0; |
12104404 | 15:7729da55873a | 115 | diff = 180-wrap(target); |
12104404 | 15:7729da55873a | 116 | if(abs(wrap(current+diff)-180)<=error) |
12104404 | 15:7729da55873a | 117 | s=SPEED_TURN_MIN; |
12104404 | 15:7729da55873a | 118 | else |
12104404 | 15:7729da55873a | 119 | s=((SPEED_TURN_MAX-SPEED_TURN_MIN)*abs(wrap(current+diff)-180)/180)+SPEED_TURN_MIN; |
12104404 | 15:7729da55873a | 120 | switch(mode) { |
12104404 | 15:7729da55873a | 121 | case ANGLE_TURN: |
12104404 | 16:d6f15a13c3aa | 122 | if(wrap(current+diff)>180+error) { |
12104404 | 16:d6f15a13c3aa | 123 | _m1dir=1; |
12104404 | 16:d6f15a13c3aa | 124 | _m2dir=0; |
12104404 | 16:d6f15a13c3aa | 125 | _m1f=s; |
12104404 | 16:d6f15a13c3aa | 126 | _m1b=s; |
12104404 | 16:d6f15a13c3aa | 127 | _m2f=s; |
12104404 | 16:d6f15a13c3aa | 128 | _m2b=s; |
12104404 | 16:d6f15a13c3aa | 129 | } else if(wrap(current+diff)<180-error) { |
12104404 | 16:d6f15a13c3aa | 130 | _m1dir=0; |
12104404 | 16:d6f15a13c3aa | 131 | _m2dir=1; |
12104404 | 16:d6f15a13c3aa | 132 | _m1f=s; |
12104404 | 16:d6f15a13c3aa | 133 | _m1b=s; |
12104404 | 16:d6f15a13c3aa | 134 | _m2f=s; |
12104404 | 16:d6f15a13c3aa | 135 | _m2b=s; |
12104404 | 16:d6f15a13c3aa | 136 | } else { |
12104404 | 19:2dd81b864e14 | 137 | stopMotors(); |
12104404 | 16:d6f15a13c3aa | 138 | return true; |
12104404 | 16:d6f15a13c3aa | 139 | } |
12104404 | 15:7729da55873a | 140 | break; |
12104404 | 15:7729da55873a | 141 | case ANGLE_BIAS: |
12104404 | 15:7729da55873a | 142 | |
12104404 | 15:7729da55873a | 143 | break; |
12104404 | 15:7729da55873a | 144 | } |
12104404 | 16:d6f15a13c3aa | 145 | return false; |
12104404 | 23:455f7da3dd7a | 146 | } |