BE@R lab
/
motion_control
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: motor_relay
Dependents: dog_V3_3_testmotor
motion_control.cpp@8:371686666f4d, 2015-10-06 (annotated)
- Committer:
- soulx
- Date:
- Tue Oct 06 12:30:56 2015 +0000
- Revision:
- 8:371686666f4d
- Parent:
- 7:94518aee126b
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| soulx | 0:77ab14788110 | 1 | #include "mbed.h" |
| soulx | 0:77ab14788110 | 2 | #include "motion_control.h" |
| soulx | 0:77ab14788110 | 3 | #include "motor_relay.h" |
| soulx | 0:77ab14788110 | 4 | |
| soulx | 0:77ab14788110 | 5 | |
| soulx | 4:6509fec6a6fb | 6 | MOTION_CONTROL::MOTION_CONTROL(PinName dirA, PinName dirB, PinName limitUp, PinName limitDown, PinName vr): motor(dirA,dirB), _limit_up(limitUp), _limit_down(limitDown), _position(vr) |
| soulx | 0:77ab14788110 | 7 | { |
| soulx | 4:6509fec6a6fb | 8 | //MOTOR_RELAY *motor = new MOTOR_RELAY(dirA,dirB); |
| soulx | 0:77ab14788110 | 9 | error=0; |
| soulx | 4:6509fec6a6fb | 10 | MARGIN=1; |
| soulx | 7:94518aee126b | 11 | mode_limit=0; |
| soulx | 7:94518aee126b | 12 | OFFSET_MAX_POSITION=0; |
| soulx | 4:6509fec6a6fb | 13 | //scale =1000; |
| soulx | 0:77ab14788110 | 14 | } |
| soulx | 0:77ab14788110 | 15 | |
| soulx | 0:77ab14788110 | 16 | |
| soulx | 4:6509fec6a6fb | 17 | int MOTION_CONTROL::limit_motor(uint8_t dirction) |
| soulx | 0:77ab14788110 | 18 | { |
| soulx | 4:6509fec6a6fb | 19 | |
| soulx | 0:77ab14788110 | 20 | if(_limit_up) { |
| soulx | 7:94518aee126b | 21 | |
| soulx | 7:94518aee126b | 22 | if(mode_limit == 0) { |
| soulx | 7:94518aee126b | 23 | motor.SetMotor(2); |
| soulx | 7:94518aee126b | 24 | return 1; |
| soulx | 7:94518aee126b | 25 | } else if(mode_limit == 1) { |
| soulx | 7:94518aee126b | 26 | |
| soulx | 7:94518aee126b | 27 | if(Scale(_position.read_u16()) > 1) { |
| soulx | 7:94518aee126b | 28 | motor.SetMotor(dirction); |
| soulx | 7:94518aee126b | 29 | } else { |
| soulx | 7:94518aee126b | 30 | motor.StopMotor(); |
| soulx | 7:94518aee126b | 31 | } |
| soulx | 7:94518aee126b | 32 | } else { |
| soulx | 7:94518aee126b | 33 | motor.SetMotor(dirction); |
| soulx | 7:94518aee126b | 34 | } |
| soulx | 7:94518aee126b | 35 | return 0;//Normally |
| soulx | 4:6509fec6a6fb | 36 | } else if(_limit_down) { |
| soulx | 4:6509fec6a6fb | 37 | motor.SetMotor(1); |
| soulx | 4:6509fec6a6fb | 38 | // } |
| soulx | 4:6509fec6a6fb | 39 | return -1; |
| soulx | 0:77ab14788110 | 40 | } else { |
| soulx | 4:6509fec6a6fb | 41 | motor.SetMotor(dirction); |
| soulx | 4:6509fec6a6fb | 42 | return 0;//Normally |
| soulx | 4:6509fec6a6fb | 43 | |
| soulx | 0:77ab14788110 | 44 | } |
| soulx | 4:6509fec6a6fb | 45 | |
| soulx | 4:6509fec6a6fb | 46 | //motor.SetMotor(dirction); |
| soulx | 4:6509fec6a6fb | 47 | //return 0; |
| soulx | 0:77ab14788110 | 48 | } |
| soulx | 0:77ab14788110 | 49 | |
| soulx | 4:6509fec6a6fb | 50 | int8_t MOTION_CONTROL::position_control(uint16_t target) |
| soulx | 0:77ab14788110 | 51 | { |
| soulx | 4:6509fec6a6fb | 52 | uint16_t current =Scale(_position.read_u16()); |
| soulx | 4:6509fec6a6fb | 53 | //target = Scale(target); |
| soulx | 6:41d7cf11fdb1 | 54 | if(target > scale) { |
| soulx | 4:6509fec6a6fb | 55 | return -3; |
| soulx | 4:6509fec6a6fb | 56 | } |
| soulx | 5:91d905f8bef7 | 57 | |
| soulx | 0:77ab14788110 | 58 | error = target-current; |
| soulx | 1:5b313fd2ca6f | 59 | if(error >scale || error < -scale) { |
| soulx | 4:6509fec6a6fb | 60 | // pc.printf("data error\n"); |
| soulx | 4:6509fec6a6fb | 61 | return -2; |
| soulx | 1:5b313fd2ca6f | 62 | } else { |
| soulx | 1:5b313fd2ca6f | 63 | if(error > MARGIN) { |
| soulx | 1:5b313fd2ca6f | 64 | if(limit_motor(1)==0 ) { //limit sens |
| soulx | 1:5b313fd2ca6f | 65 | //pc.printf("motor[%d]=limit error\n",id); |
| soulx | 1:5b313fd2ca6f | 66 | return limit_motor(1); |
| soulx | 1:5b313fd2ca6f | 67 | } |
| soulx | 1:5b313fd2ca6f | 68 | } else if(error < -MARGIN) { |
| soulx | 1:5b313fd2ca6f | 69 | if(limit_motor(2)!=0 ) { //limit sens |
| soulx | 1:5b313fd2ca6f | 70 | //pc.printf("motor[%d]=limit error\n",id); |
| soulx | 1:5b313fd2ca6f | 71 | return limit_motor(2); |
| soulx | 1:5b313fd2ca6f | 72 | } |
| soulx | 1:5b313fd2ca6f | 73 | } else { //in zone |
| soulx | 4:6509fec6a6fb | 74 | motor.StopMotor(); |
| soulx | 1:5b313fd2ca6f | 75 | //pc.printf("motor[%d]=complete\n",id); |
| soulx | 1:5b313fd2ca6f | 76 | return 2; //in zone complete |
| soulx | 0:77ab14788110 | 77 | } |
| soulx | 0:77ab14788110 | 78 | } |
| soulx | 4:6509fec6a6fb | 79 | //pc.printf("motor[%d]=in process\n",id); |
| soulx | 4:6509fec6a6fb | 80 | return 0; //in process |
| soulx | 0:77ab14788110 | 81 | } |
| soulx | 0:77ab14788110 | 82 | |
| soulx | 0:77ab14788110 | 83 | void MOTION_CONTROL::calibration() |
| soulx | 0:77ab14788110 | 84 | { |
| soulx | 7:94518aee126b | 85 | uint8_t buff =GetMode(); |
| soulx | 7:94518aee126b | 86 | SetMode(0); |
| soulx | 4:6509fec6a6fb | 87 | int state=0; |
| soulx | 5:91d905f8bef7 | 88 | uint32_t sum=0; |
| soulx | 8:371686666f4d | 89 | uint16_t min=65535,max=0; |
| soulx | 0:77ab14788110 | 90 | |
| soulx | 4:6509fec6a6fb | 91 | do { |
| soulx | 4:6509fec6a6fb | 92 | state = limit_motor(1); |
| soulx | 4:6509fec6a6fb | 93 | } while(state==0); |
| soulx | 5:91d905f8bef7 | 94 | |
| soulx | 4:6509fec6a6fb | 95 | do { |
| soulx | 4:6509fec6a6fb | 96 | state = limit_motor(2); |
| soulx | 4:6509fec6a6fb | 97 | } while(state!=1); |
| soulx | 4:6509fec6a6fb | 98 | wait_ms(200); |
| soulx | 4:6509fec6a6fb | 99 | motor.StopMotor(); |
| soulx | 4:6509fec6a6fb | 100 | wait_ms(500); |
| soulx | 5:91d905f8bef7 | 101 | |
| soulx | 5:91d905f8bef7 | 102 | sum=0; |
| soulx | 5:91d905f8bef7 | 103 | for(int a=0; a<LOOP; a++) { |
| soulx | 8:371686666f4d | 104 | sum = _position.read_u16(); |
| soulx | 8:371686666f4d | 105 | if( sum < min) { |
| soulx | 8:371686666f4d | 106 | min =sum; |
| soulx | 8:371686666f4d | 107 | } |
| soulx | 5:91d905f8bef7 | 108 | } |
| soulx | 8:371686666f4d | 109 | MAX_POSITION = min; |
| soulx | 4:6509fec6a6fb | 110 | |
| soulx | 4:6509fec6a6fb | 111 | do { |
| soulx | 4:6509fec6a6fb | 112 | state = limit_motor(2); |
| soulx | 4:6509fec6a6fb | 113 | } while(state==0); |
| soulx | 4:6509fec6a6fb | 114 | //motor.StopMotor(); |
| soulx | 4:6509fec6a6fb | 115 | do { |
| soulx | 4:6509fec6a6fb | 116 | state = limit_motor(1); |
| soulx | 4:6509fec6a6fb | 117 | } while(state!=0); |
| soulx | 4:6509fec6a6fb | 118 | wait_ms(100); |
| soulx | 4:6509fec6a6fb | 119 | motor.StopMotor(); |
| soulx | 5:91d905f8bef7 | 120 | |
| soulx | 5:91d905f8bef7 | 121 | sum=0; |
| soulx | 5:91d905f8bef7 | 122 | for(int a=0; a<LOOP; a++) { |
| soulx | 8:371686666f4d | 123 | sum = _position.read_u16(); |
| soulx | 8:371686666f4d | 124 | if( sum > max) { |
| soulx | 8:371686666f4d | 125 | max =sum; |
| soulx | 8:371686666f4d | 126 | } |
| soulx | 5:91d905f8bef7 | 127 | } |
| soulx | 8:371686666f4d | 128 | MIN_POSITION = max; |
| soulx | 8:371686666f4d | 129 | |
| soulx | 7:94518aee126b | 130 | SetMode(buff); |
| soulx | 3:4fa191f2194d | 131 | } |
| soulx | 3:4fa191f2194d | 132 | |
| soulx | 3:4fa191f2194d | 133 | int MOTION_CONTROL::GetLimitUp() |
| soulx | 3:4fa191f2194d | 134 | { |
| soulx | 3:4fa191f2194d | 135 | return _limit_up; |
| soulx | 3:4fa191f2194d | 136 | } |
| soulx | 3:4fa191f2194d | 137 | |
| soulx | 3:4fa191f2194d | 138 | int MOTION_CONTROL::GetLimitDown() |
| soulx | 3:4fa191f2194d | 139 | { |
| soulx | 3:4fa191f2194d | 140 | return _limit_down; |
| soulx | 3:4fa191f2194d | 141 | } |
| soulx | 3:4fa191f2194d | 142 | |
| soulx | 3:4fa191f2194d | 143 | void MOTION_CONTROL::SetMargin(int16_t data) |
| soulx | 3:4fa191f2194d | 144 | { |
| soulx | 3:4fa191f2194d | 145 | MARGIN = data; |
| soulx | 3:4fa191f2194d | 146 | } |
| soulx | 3:4fa191f2194d | 147 | int16_t MOTION_CONTROL::GetMargin() |
| soulx | 3:4fa191f2194d | 148 | { |
| soulx | 3:4fa191f2194d | 149 | return MARGIN; |
| soulx | 3:4fa191f2194d | 150 | } |
| soulx | 3:4fa191f2194d | 151 | |
| soulx | 3:4fa191f2194d | 152 | uint16_t MOTION_CONTROL::GetMaxPosition() |
| soulx | 3:4fa191f2194d | 153 | { |
| soulx | 3:4fa191f2194d | 154 | return MAX_POSITION; |
| soulx | 3:4fa191f2194d | 155 | } |
| soulx | 3:4fa191f2194d | 156 | |
| soulx | 3:4fa191f2194d | 157 | uint16_t MOTION_CONTROL::GetMinPosition() |
| soulx | 3:4fa191f2194d | 158 | { |
| soulx | 3:4fa191f2194d | 159 | return MIN_POSITION; |
| soulx | 3:4fa191f2194d | 160 | } |
| soulx | 4:6509fec6a6fb | 161 | |
| soulx | 5:91d905f8bef7 | 162 | void MOTION_CONTROL::SetMaxPosition(uint16_t value) |
| soulx | 5:91d905f8bef7 | 163 | { |
| soulx | 5:91d905f8bef7 | 164 | MAX_POSITION = value; |
| soulx | 5:91d905f8bef7 | 165 | } |
| soulx | 5:91d905f8bef7 | 166 | void MOTION_CONTROL::SetMinPosition(uint16_t value) |
| soulx | 5:91d905f8bef7 | 167 | { |
| soulx | 5:91d905f8bef7 | 168 | MIN_POSITION = value; |
| soulx | 5:91d905f8bef7 | 169 | } |
| soulx | 5:91d905f8bef7 | 170 | |
| soulx | 4:6509fec6a6fb | 171 | uint16_t MOTION_CONTROL::Scale(uint16_t data) |
| soulx | 4:6509fec6a6fb | 172 | { |
| soulx | 7:94518aee126b | 173 | return ((float)(data-MIN_POSITION)/((MAX_POSITION+OFFSET_MAX_POSITION) - MIN_POSITION))*scale; |
| soulx | 4:6509fec6a6fb | 174 | } |
| soulx | 4:6509fec6a6fb | 175 | |
| soulx | 4:6509fec6a6fb | 176 | uint16_t MOTION_CONTROL::GetAnalog() |
| soulx | 4:6509fec6a6fb | 177 | { |
| soulx | 4:6509fec6a6fb | 178 | return _position.read_u16(); |
| soulx | 4:6509fec6a6fb | 179 | } |
| soulx | 4:6509fec6a6fb | 180 | |
| soulx | 4:6509fec6a6fb | 181 | uint16_t MOTION_CONTROL::GetPosition() |
| soulx | 4:6509fec6a6fb | 182 | { |
| soulx | 4:6509fec6a6fb | 183 | return Scale(_position.read_u16()); |
| soulx | 4:6509fec6a6fb | 184 | } |
| soulx | 4:6509fec6a6fb | 185 | |
| soulx | 4:6509fec6a6fb | 186 | void MOTION_CONTROL::stop() |
| soulx | 4:6509fec6a6fb | 187 | { |
| soulx | 4:6509fec6a6fb | 188 | motor.StopMotor(); |
| soulx | 7:94518aee126b | 189 | } |
| soulx | 7:94518aee126b | 190 | |
| soulx | 7:94518aee126b | 191 | void MOTION_CONTROL::SetMode(uint8_t mode) |
| soulx | 7:94518aee126b | 192 | { |
| soulx | 7:94518aee126b | 193 | mode_limit = mode; |
| soulx | 7:94518aee126b | 194 | } |
| soulx | 7:94518aee126b | 195 | uint8_t MOTION_CONTROL::GetMode() |
| soulx | 7:94518aee126b | 196 | { |
| soulx | 7:94518aee126b | 197 | return mode_limit; |
| soulx | 7:94518aee126b | 198 | } |
| soulx | 7:94518aee126b | 199 | |
| soulx | 7:94518aee126b | 200 | void MOTION_CONTROL::SetOffset(uint16_t value) |
| soulx | 7:94518aee126b | 201 | { |
| soulx | 7:94518aee126b | 202 | OFFSET_MAX_POSITION = value; |
| soulx | 7:94518aee126b | 203 | } |
| soulx | 7:94518aee126b | 204 | uint16_t MOTION_CONTROL::GetOffset() |
| soulx | 7:94518aee126b | 205 | { |
| soulx | 7:94518aee126b | 206 | return OFFSET_MAX_POSITION; |
| soulx | 4:6509fec6a6fb | 207 | } |