BE@R lab / Mbed 2 deprecated dog_V3_3_testmotor

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:   Communication_Robot QEI iSerial mbed motion_control

Fork of dog_V3_2_testmotor by BE@R lab

main.cpp@0:2433ddae2772, 2015-07-15 (annotated)

Committer:
soulx
Date:
Wed Jul 15 05:07:26 2015 +0000
Revision:
0:2433ddae2772
Child:
1:768d359e9d96
test1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
soulx 0:2433ddae2772 1 #include "mbed.h"
soulx 0:2433ddae2772 2 #include "pin_config.h"
soulx 0:2433ddae2772 3
soulx 0:2433ddae2772 4 //define pin class
soulx 0:2433ddae2772 5 DigitalOut dirA_LU(INA_L_U);
soulx 0:2433ddae2772 6 DigitalOut dirB_LU(INB_L_U);
soulx 0:2433ddae2772 7
soulx 0:2433ddae2772 8 DigitalOut dirA_LL(INA_L_L);
soulx 0:2433ddae2772 9 DigitalOut dirB_LL(INB_L_L);
soulx 0:2433ddae2772 10
soulx 0:2433ddae2772 11 DigitalOut dirA_RU(INA_R_U);
soulx 0:2433ddae2772 12 DigitalOut dirB_RU(INB_R_U);
soulx 0:2433ddae2772 13
soulx 0:2433ddae2772 14 DigitalOut dirA_RL(INA_R_L);
soulx 0:2433ddae2772 15 DigitalOut dirB_RL(INB_R_L);
soulx 0:2433ddae2772 16
soulx 0:2433ddae2772 17 DigitalIn sw_LU_U(LIMIT_LU_U,PullUp);
soulx 0:2433ddae2772 18 DigitalIn sw_LU_D(LIMIT_LU_D,PullUp);
soulx 0:2433ddae2772 19
soulx 0:2433ddae2772 20 DigitalIn sw_LL_U(LIMIT_LL_U,PullUp);
soulx 0:2433ddae2772 21 DigitalIn sw_LL_D(LIMIT_LL_D,PullUp);
soulx 0:2433ddae2772 22
soulx 0:2433ddae2772 23 DigitalIn sw_RU_U(LIMIT_RU_U,PullUp);
soulx 0:2433ddae2772 24 DigitalIn sw_RU_D(LIMIT_RU_D,PullUp);
soulx 0:2433ddae2772 25
soulx 0:2433ddae2772 26 DigitalIn sw_RL_U(LIMIT_RL_U,PullUp);
soulx 0:2433ddae2772 27 DigitalIn sw_RL_D(LIMIT_RL_D,PullUp);
soulx 0:2433ddae2772 28
soulx 0:2433ddae2772 29 AnalogIn position_LU(VR_LU);
soulx 0:2433ddae2772 30 AnalogIn position_LL(VR_LL);
soulx 0:2433ddae2772 31 AnalogIn position_RU(VR_RU);
soulx 0:2433ddae2772 32 AnalogIn position_RL(VR_RL);
soulx 0:2433ddae2772 33
soulx 0:2433ddae2772 34 DigitalOut myled(LED1);
soulx 0:2433ddae2772 35 DigitalIn mybutton(USER_BUTTON);
soulx 0:2433ddae2772 36
soulx 0:2433ddae2772 37 Serial pc(USBTX, USBRX);
soulx 0:2433ddae2772 38
soulx 0:2433ddae2772 39
soulx 0:2433ddae2772 40 //Function Prototype
soulx 0:2433ddae2772 41 void motor_set(uint8_t id, uint8_t direct);
soulx 0:2433ddae2772 42 void motor_stop(uint8_t id);
soulx 0:2433ddae2772 43
soulx 0:2433ddae2772 44 uint8_t limit_motor(uint8_t id, uint8_t dirction);
soulx 0:2433ddae2772 45 uint8_t position_control(uint8_t id, uint16_t current, uint16_t target);
soulx 0:2433ddae2772 46
soulx 0:2433ddae2772 47 void calibration(uint8_t id);
soulx 0:2433ddae2772 48
soulx 0:2433ddae2772 49
soulx 0:2433ddae2772 50 //Globle Variable
soulx 0:2433ddae2772 51 uint16_t max_pos_LU= 10000;
soulx 0:2433ddae2772 52 uint16_t min_pos_LU= 6000;
soulx 0:2433ddae2772 53 uint16_t max_pos_LL= 50000;
soulx 0:2433ddae2772 54 uint16_t min_pos_LL= 37000;
soulx 0:2433ddae2772 55
soulx 0:2433ddae2772 56 uint16_t max_pos_RU= 17800;
soulx 0:2433ddae2772 57 uint16_t min_pos_RU= 9000;
soulx 0:2433ddae2772 58 uint16_t max_pos_RL= 51000;
soulx 0:2433ddae2772 59 uint16_t min_pos_RL= 11000;
soulx 0:2433ddae2772 60
soulx 0:2433ddae2772 61 int16_t MARGIN = 500;
soulx 0:2433ddae2772 62
soulx 0:2433ddae2772 63 //Main function
soulx 0:2433ddae2772 64 int main()
soulx 0:2433ddae2772 65 {
soulx 0:2433ddae2772 66 uint16_t vr_lu,vr_ru;
soulx 0:2433ddae2772 67 uint16_t vr_ll,vr_rl;
soulx 0:2433ddae2772 68 pc.printf("wait\n");
soulx 0:2433ddae2772 69 motor_stop(0);
soulx 0:2433ddae2772 70 // wait(10);
soulx 0:2433ddae2772 71 /*
soulx 0:2433ddae2772 72 while(1) {
soulx 0:2433ddae2772 73 motor_set(1,1);
soulx 0:2433ddae2772 74 motor_set(2,1);
soulx 0:2433ddae2772 75 motor_set(3,1);
soulx 0:2433ddae2772 76 motor_set(4,1);
soulx 0:2433ddae2772 77 wait(1);
soulx 0:2433ddae2772 78 motor_set(1,2);
soulx 0:2433ddae2772 79 motor_set(2,2);
soulx 0:2433ddae2772 80 motor_set(3,2);
soulx 0:2433ddae2772 81 motor_set(4,2);
soulx 0:2433ddae2772 82 wait(1);
soulx 0:2433ddae2772 83 }
soulx 0:2433ddae2772 84 */
soulx 0:2433ddae2772 85
soulx 0:2433ddae2772 86 /*
soulx 0:2433ddae2772 87 while(1) {
soulx 0:2433ddae2772 88 //Read position
soulx 0:2433ddae2772 89 vr_ll = position_LL.read_u16();
soulx 0:2433ddae2772 90 vr_lu = position_LU.read_u16();
soulx 0:2433ddae2772 91 vr_rl = position_RL.read_u16();
soulx 0:2433ddae2772 92 vr_ru = position_RU.read_u16();
soulx 0:2433ddae2772 93 pc.printf("vr_LL = %d\t",vr_ll);
soulx 0:2433ddae2772 94 pc.printf("vr_LU = %d\t",vr_lu);
soulx 0:2433ddae2772 95 pc.printf("vr_RL = %d\t",vr_rl);
soulx 0:2433ddae2772 96 pc.printf("vr_RU = %d\n",vr_ru);
soulx 0:2433ddae2772 97 }
soulx 0:2433ddae2772 98 */
soulx 0:2433ddae2772 99
soulx 0:2433ddae2772 100 /*
soulx 0:2433ddae2772 101 while(1) {
soulx 0:2433ddae2772 102 myled =1;
soulx 0:2433ddae2772 103 wait_ms(200);
soulx 0:2433ddae2772 104
soulx 0:2433ddae2772 105 if(mybutton == 0) {
soulx 0:2433ddae2772 106 myled =0;
soulx 0:2433ddae2772 107 wait_ms(200);
soulx 0:2433ddae2772 108 }
soulx 0:2433ddae2772 109
soulx 0:2433ddae2772 110 if(sw_LU_U == 0) {
soulx 0:2433ddae2772 111 myled =0;
soulx 0:2433ddae2772 112 wait_ms(200);
soulx 0:2433ddae2772 113 }
soulx 0:2433ddae2772 114
soulx 0:2433ddae2772 115 if(sw_LU_D == 0) {
soulx 0:2433ddae2772 116 myled =0;
soulx 0:2433ddae2772 117 wait_ms(200);
soulx 0:2433ddae2772 118 }
soulx 0:2433ddae2772 119
soulx 0:2433ddae2772 120 if(sw_LL_U == 0) {
soulx 0:2433ddae2772 121 myled =0;
soulx 0:2433ddae2772 122 wait_ms(200);
soulx 0:2433ddae2772 123 }
soulx 0:2433ddae2772 124
soulx 0:2433ddae2772 125 if(sw_LL_D == 0) {
soulx 0:2433ddae2772 126 myled =0;
soulx 0:2433ddae2772 127 wait_ms(200);
soulx 0:2433ddae2772 128 }
soulx 0:2433ddae2772 129
soulx 0:2433ddae2772 130 if(sw_RU_U == 0) {
soulx 0:2433ddae2772 131 myled =0;
soulx 0:2433ddae2772 132 wait_ms(200);
soulx 0:2433ddae2772 133 }
soulx 0:2433ddae2772 134
soulx 0:2433ddae2772 135 if(sw_RU_D == 0) {
soulx 0:2433ddae2772 136 myled =0;
soulx 0:2433ddae2772 137 wait_ms(200);
soulx 0:2433ddae2772 138 }
soulx 0:2433ddae2772 139
soulx 0:2433ddae2772 140 if(sw_RL_U == 0) {
soulx 0:2433ddae2772 141 myled =0;
soulx 0:2433ddae2772 142 wait_ms(200);
soulx 0:2433ddae2772 143 }
soulx 0:2433ddae2772 144
soulx 0:2433ddae2772 145 if(sw_RL_D == 0) {
soulx 0:2433ddae2772 146 myled =0;
soulx 0:2433ddae2772 147 wait_ms(200);
soulx 0:2433ddae2772 148 }
soulx 0:2433ddae2772 149
soulx 0:2433ddae2772 150 }
soulx 0:2433ddae2772 151 */
soulx 0:2433ddae2772 152
soulx 0:2433ddae2772 153 // while(1) {
soulx 0:2433ddae2772 154 //calibration
soulx 0:2433ddae2772 155 pc.printf("Welcome to DOGWHEELSCHAIR\n");
soulx 0:2433ddae2772 156 pc.printf("Calibration [START]\n");
soulx 0:2433ddae2772 157 calibration(1);
soulx 0:2433ddae2772 158 calibration(2);
soulx 0:2433ddae2772 159 calibration(3);
soulx 0:2433ddae2772 160 calibration(4);
soulx 0:2433ddae2772 161 pc.printf("Calibration [FINISH]\n");
soulx 0:2433ddae2772 162 pc.printf("RUN mode [START]\n");
soulx 0:2433ddae2772 163 // }
soulx 0:2433ddae2772 164
soulx 0:2433ddae2772 165
soulx 0:2433ddae2772 166 uint8_t count=0;
soulx 0:2433ddae2772 167
soulx 0:2433ddae2772 168 while(1) {
soulx 0:2433ddae2772 169 uint8_t state=0;
soulx 0:2433ddae2772 170 pc.printf("Count %d",count);
soulx 0:2433ddae2772 171 do {
soulx 0:2433ddae2772 172 state=0;
soulx 0:2433ddae2772 173
soulx 0:2433ddae2772 174 if(position_control(1,position_LU.read_u16(),max_pos_LU) == 1) {
soulx 0:2433ddae2772 175 state++;
soulx 0:2433ddae2772 176 }
soulx 0:2433ddae2772 177
soulx 0:2433ddae2772 178 if(position_control(2,position_LL.read_u16(),max_pos_LL) == 1) {
soulx 0:2433ddae2772 179 state++;
soulx 0:2433ddae2772 180 }
soulx 0:2433ddae2772 181
soulx 0:2433ddae2772 182 if(position_control(3,position_RU.read_u16(),max_pos_RU) == 1) {
soulx 0:2433ddae2772 183 state++;
soulx 0:2433ddae2772 184 }
soulx 0:2433ddae2772 185
soulx 0:2433ddae2772 186 if(position_control(4,position_RL.read_u16(),max_pos_RL) == 1) {
soulx 0:2433ddae2772 187 state++;
soulx 0:2433ddae2772 188 }
soulx 0:2433ddae2772 189
soulx 0:2433ddae2772 190 pc.printf("state = %d",state);
soulx 0:2433ddae2772 191 } while(state <= 4 );
soulx 0:2433ddae2772 192
soulx 0:2433ddae2772 193 do {
soulx 0:2433ddae2772 194 state=0;
soulx 0:2433ddae2772 195
soulx 0:2433ddae2772 196 if(position_control(1,position_LU.read_u16(),min_pos_LU) == 1) {
soulx 0:2433ddae2772 197 state++;
soulx 0:2433ddae2772 198 }
soulx 0:2433ddae2772 199
soulx 0:2433ddae2772 200 if(position_control(2,position_LL.read_u16(),min_pos_LL) == 1) {
soulx 0:2433ddae2772 201 state++;
soulx 0:2433ddae2772 202 }
soulx 0:2433ddae2772 203
soulx 0:2433ddae2772 204 if(position_control(3,position_RU.read_u16(),min_pos_RU) == 1) {
soulx 0:2433ddae2772 205 state++;
soulx 0:2433ddae2772 206 }
soulx 0:2433ddae2772 207
soulx 0:2433ddae2772 208 if(position_control(4,position_RL.read_u16(),min_pos_RL) == 1) {
soulx 0:2433ddae2772 209 state++;
soulx 0:2433ddae2772 210 }
soulx 0:2433ddae2772 211
soulx 0:2433ddae2772 212 pc.printf("state = %d",state);
soulx 0:2433ddae2772 213 } while(state <= 4 );
soulx 0:2433ddae2772 214 count++;
soulx 0:2433ddae2772 215 }
soulx 0:2433ddae2772 216 }
soulx 0:2433ddae2772 217
soulx 0:2433ddae2772 218
soulx 0:2433ddae2772 219
soulx 0:2433ddae2772 220 void motor_set(uint8_t id, uint8_t direct)
soulx 0:2433ddae2772 221 {
soulx 0:2433ddae2772 222 //direct: Should be between 0 and 3, with the following result
soulx 0:2433ddae2772 223 //0: Brake to VCC
soulx 0:2433ddae2772 224 //1: Clockwise
soulx 0:2433ddae2772 225 //2: CounterClockwise
soulx 0:2433ddae2772 226 //3: Brake to GND
soulx 0:2433ddae2772 227
soulx 0:2433ddae2772 228 if(direct <=4) {
soulx 0:2433ddae2772 229 switch(id) {
soulx 0:2433ddae2772 230 case 1:
soulx 0:2433ddae2772 231 // Set inA[motor]
soulx 0:2433ddae2772 232 if (direct <=1)
soulx 0:2433ddae2772 233 dirA_LU=0;
soulx 0:2433ddae2772 234 else
soulx 0:2433ddae2772 235 dirA_LU=1;
soulx 0:2433ddae2772 236
soulx 0:2433ddae2772 237 // Set inB[motor]
soulx 0:2433ddae2772 238 if ((direct==0)||(direct==2))
soulx 0:2433ddae2772 239 dirB_LU=0;
soulx 0:2433ddae2772 240 else
soulx 0:2433ddae2772 241 dirB_LU=1;
soulx 0:2433ddae2772 242 break;
soulx 0:2433ddae2772 243
soulx 0:2433ddae2772 244 case 2:
soulx 0:2433ddae2772 245 // Set inA[motor]
soulx 0:2433ddae2772 246 if (direct <=1)
soulx 0:2433ddae2772 247 dirA_LL=0;
soulx 0:2433ddae2772 248 else
soulx 0:2433ddae2772 249 dirA_LL=1;
soulx 0:2433ddae2772 250
soulx 0:2433ddae2772 251 // Set inB[motor]
soulx 0:2433ddae2772 252 if ((direct==0)||(direct==2))
soulx 0:2433ddae2772 253 dirB_LL=0;
soulx 0:2433ddae2772 254 else
soulx 0:2433ddae2772 255 dirB_LL=1;
soulx 0:2433ddae2772 256 break;
soulx 0:2433ddae2772 257
soulx 0:2433ddae2772 258 case 3:
soulx 0:2433ddae2772 259 // Set inA[motor]
soulx 0:2433ddae2772 260 if (direct <=1)
soulx 0:2433ddae2772 261 dirA_RU=0;
soulx 0:2433ddae2772 262 else
soulx 0:2433ddae2772 263 dirA_RU=1;
soulx 0:2433ddae2772 264
soulx 0:2433ddae2772 265 // Set inB[motor]
soulx 0:2433ddae2772 266 if ((direct==0)||(direct==2))
soulx 0:2433ddae2772 267 dirB_RU=0;
soulx 0:2433ddae2772 268 else
soulx 0:2433ddae2772 269 dirB_RU=1;
soulx 0:2433ddae2772 270 break;
soulx 0:2433ddae2772 271
soulx 0:2433ddae2772 272 case 4:
soulx 0:2433ddae2772 273 // Set inA[motor]
soulx 0:2433ddae2772 274 if (direct <=1)
soulx 0:2433ddae2772 275 dirA_RL=0;
soulx 0:2433ddae2772 276 else
soulx 0:2433ddae2772 277 dirA_RL=1;
soulx 0:2433ddae2772 278 // Set inB[motor]
soulx 0:2433ddae2772 279 if ((direct==0)||(direct==2))
soulx 0:2433ddae2772 280 dirB_RL=0;
soulx 0:2433ddae2772 281 else
soulx 0:2433ddae2772 282 dirB_RL=1;
soulx 0:2433ddae2772 283 break;
soulx 0:2433ddae2772 284 }
soulx 0:2433ddae2772 285 }
soulx 0:2433ddae2772 286 }
soulx 0:2433ddae2772 287
soulx 0:2433ddae2772 288 void motor_stop(uint8_t id)
soulx 0:2433ddae2772 289 {
soulx 0:2433ddae2772 290 switch(id) {
soulx 0:2433ddae2772 291 case 1:
soulx 0:2433ddae2772 292 dirA_LU=1;
soulx 0:2433ddae2772 293 dirB_LU=1;
soulx 0:2433ddae2772 294 break;
soulx 0:2433ddae2772 295
soulx 0:2433ddae2772 296 case 2:
soulx 0:2433ddae2772 297 dirA_LL=1;
soulx 0:2433ddae2772 298 dirB_LL=1;
soulx 0:2433ddae2772 299 break;
soulx 0:2433ddae2772 300
soulx 0:2433ddae2772 301 case 3:
soulx 0:2433ddae2772 302 dirA_RU=1;
soulx 0:2433ddae2772 303 dirB_RU=1;
soulx 0:2433ddae2772 304 break;
soulx 0:2433ddae2772 305
soulx 0:2433ddae2772 306 case 4:
soulx 0:2433ddae2772 307 dirA_RL=1;
soulx 0:2433ddae2772 308 dirB_RL=1;
soulx 0:2433ddae2772 309 break;
soulx 0:2433ddae2772 310
soulx 0:2433ddae2772 311 case 0:
soulx 0:2433ddae2772 312 dirA_LU=1;
soulx 0:2433ddae2772 313 dirB_LU=1;
soulx 0:2433ddae2772 314
soulx 0:2433ddae2772 315 dirA_LL=1;
soulx 0:2433ddae2772 316 dirB_LL=1;
soulx 0:2433ddae2772 317
soulx 0:2433ddae2772 318 dirA_RU=1;
soulx 0:2433ddae2772 319 dirB_RU=1;
soulx 0:2433ddae2772 320
soulx 0:2433ddae2772 321 dirA_RL=1;
soulx 0:2433ddae2772 322 dirB_RL=1;
soulx 0:2433ddae2772 323 break;
soulx 0:2433ddae2772 324 }
soulx 0:2433ddae2772 325 }
soulx 0:2433ddae2772 326
soulx 0:2433ddae2772 327
soulx 0:2433ddae2772 328
soulx 0:2433ddae2772 329 uint8_t limit_motor(uint8_t id, uint8_t dirction)
soulx 0:2433ddae2772 330 {
soulx 0:2433ddae2772 331 switch(id) {
soulx 0:2433ddae2772 332 case 1://Left Upper
soulx 0:2433ddae2772 333 if(sw_LU_U && sw_LU_D) {
soulx 0:2433ddae2772 334 motor_set(id,dirction);
soulx 0:2433ddae2772 335 } else {
soulx 0:2433ddae2772 336 motor_stop(id);
soulx 0:2433ddae2772 337 return 0;
soulx 0:2433ddae2772 338 }
soulx 0:2433ddae2772 339 break;
soulx 0:2433ddae2772 340
soulx 0:2433ddae2772 341 case 2://Left Lowwer
soulx 0:2433ddae2772 342 if(sw_LL_U && sw_LL_D) {
soulx 0:2433ddae2772 343 motor_set(id,dirction);
soulx 0:2433ddae2772 344 } else {
soulx 0:2433ddae2772 345 motor_stop(id);
soulx 0:2433ddae2772 346 return 0;
soulx 0:2433ddae2772 347 }
soulx 0:2433ddae2772 348 break;
soulx 0:2433ddae2772 349
soulx 0:2433ddae2772 350 case 3://Right Upper
soulx 0:2433ddae2772 351 if(sw_RU_U && sw_RU_D) {
soulx 0:2433ddae2772 352 motor_set(id,dirction);
soulx 0:2433ddae2772 353 } else {
soulx 0:2433ddae2772 354 motor_stop(id);
soulx 0:2433ddae2772 355 return 0;
soulx 0:2433ddae2772 356 }
soulx 0:2433ddae2772 357 break;
soulx 0:2433ddae2772 358
soulx 0:2433ddae2772 359 case 4://Right Lowwer
soulx 0:2433ddae2772 360 if(sw_RL_U && sw_RL_D) {
soulx 0:2433ddae2772 361 motor_set(id,dirction);
soulx 0:2433ddae2772 362 } else {
soulx 0:2433ddae2772 363 motor_stop(id);
soulx 0:2433ddae2772 364 return 0;
soulx 0:2433ddae2772 365 }
soulx 0:2433ddae2772 366 break;
soulx 0:2433ddae2772 367 }
soulx 0:2433ddae2772 368 return 1;//normally
soulx 0:2433ddae2772 369 }
soulx 0:2433ddae2772 370
soulx 0:2433ddae2772 371
soulx 0:2433ddae2772 372 uint8_t position_control(uint8_t id, uint16_t current, uint16_t target)
soulx 0:2433ddae2772 373 {
soulx 0:2433ddae2772 374 //uint8_t state=0;
soulx 0:2433ddae2772 375
soulx 0:2433ddae2772 376
soulx 0:2433ddae2772 377 int16_t error = target-current;
soulx 0:2433ddae2772 378
soulx 0:2433ddae2772 379 pc.printf("error[%d]=%d\n",id,error);
soulx 0:2433ddae2772 380 if(error > MARGIN) {
soulx 0:2433ddae2772 381 if(limit_motor(id,1)==0 ) { //limit sens
soulx 0:2433ddae2772 382
soulx 0:2433ddae2772 383 pc.printf("motor[%d]=limit error\n",id);
soulx 0:2433ddae2772 384
soulx 0:2433ddae2772 385 return 2;
soulx 0:2433ddae2772 386 }
soulx 0:2433ddae2772 387
soulx 0:2433ddae2772 388
soulx 0:2433ddae2772 389 } else if(error < -MARGIN) {
soulx 0:2433ddae2772 390 if(limit_motor(id,2)==0 ) { //limit sens
soulx 0:2433ddae2772 391
soulx 0:2433ddae2772 392 pc.printf("motor[%d]=limit error\n",id);
soulx 0:2433ddae2772 393
soulx 0:2433ddae2772 394 return 2;
soulx 0:2433ddae2772 395 }
soulx 0:2433ddae2772 396
soulx 0:2433ddae2772 397 } else { //in zone
soulx 0:2433ddae2772 398 motor_stop(2);
soulx 0:2433ddae2772 399
soulx 0:2433ddae2772 400 pc.printf("motor[%d]=complete\n",id);
soulx 0:2433ddae2772 401
soulx 0:2433ddae2772 402 return 1; //in zone complete
soulx 0:2433ddae2772 403 }
soulx 0:2433ddae2772 404
soulx 0:2433ddae2772 405 pc.printf("motor[%d]=in process\n",id);
soulx 0:2433ddae2772 406 return 0; //in process
soulx 0:2433ddae2772 407 }
soulx 0:2433ddae2772 408
soulx 0:2433ddae2772 409 void calibration(uint8_t id)
soulx 0:2433ddae2772 410 {
soulx 0:2433ddae2772 411 switch(id) {
soulx 0:2433ddae2772 412 case 1:
soulx 0:2433ddae2772 413 pc.printf("motor[1] run up\n");
soulx 0:2433ddae2772 414 while(sw_LU_U) {
soulx 0:2433ddae2772 415 motor_set(id,1);
soulx 0:2433ddae2772 416 }
soulx 0:2433ddae2772 417 pc.printf("motor[1] stop up\n");
soulx 0:2433ddae2772 418 //wait_ms(500);
soulx 0:2433ddae2772 419 do {
soulx 0:2433ddae2772 420 motor_set(id,2);
soulx 0:2433ddae2772 421 } while(sw_LU_U ==0);
soulx 0:2433ddae2772 422 motor_stop(id);
soulx 0:2433ddae2772 423 wait_ms(500);
soulx 0:2433ddae2772 424 pc.printf("motor[1] read position\n");
soulx 0:2433ddae2772 425 max_pos_LU = position_LU.read_u16();
soulx 0:2433ddae2772 426 pc.printf("max_pos_LU= %d\n",max_pos_LU);
soulx 0:2433ddae2772 427
soulx 0:2433ddae2772 428 pc.printf("motor[1] run down\n");
soulx 0:2433ddae2772 429 while(sw_LU_D) {
soulx 0:2433ddae2772 430 motor_set(id,2);
soulx 0:2433ddae2772 431 }
soulx 0:2433ddae2772 432 pc.printf("motor[1] stop down\n");
soulx 0:2433ddae2772 433 do {
soulx 0:2433ddae2772 434 motor_set(id,1);
soulx 0:2433ddae2772 435 } while(sw_LU_D ==0);
soulx 0:2433ddae2772 436 motor_stop(id);
soulx 0:2433ddae2772 437 wait_ms(500);
soulx 0:2433ddae2772 438 pc.printf("motor[1] read position\n");
soulx 0:2433ddae2772 439 min_pos_LU = position_LU.read_u16();
soulx 0:2433ddae2772 440 pc.printf("min_pos_LU= %d\n",min_pos_LU);
soulx 0:2433ddae2772 441 break;
soulx 0:2433ddae2772 442
soulx 0:2433ddae2772 443 case 2:
soulx 0:2433ddae2772 444
soulx 0:2433ddae2772 445 pc.printf("motor[2] run up\n");
soulx 0:2433ddae2772 446 while(sw_LL_U) {
soulx 0:2433ddae2772 447 motor_set(id,1);
soulx 0:2433ddae2772 448 }
soulx 0:2433ddae2772 449 motor_stop(id);
soulx 0:2433ddae2772 450 wait_ms(500);
soulx 0:2433ddae2772 451 pc.printf("motor[2] stop up\n");
soulx 0:2433ddae2772 452 do {
soulx 0:2433ddae2772 453 motor_set(id,2);
soulx 0:2433ddae2772 454 } while(sw_LL_U == 0);
soulx 0:2433ddae2772 455 motor_stop(id);
soulx 0:2433ddae2772 456 wait_ms(500);
soulx 0:2433ddae2772 457 max_pos_LL = position_LL.read_u16();
soulx 0:2433ddae2772 458 pc.printf("max_pos_LL= %d\n",max_pos_LL);
soulx 0:2433ddae2772 459 pc.printf("motor[2] run down\n");
soulx 0:2433ddae2772 460 while(sw_LL_D) {
soulx 0:2433ddae2772 461 motor_set(id,2);
soulx 0:2433ddae2772 462 }
soulx 0:2433ddae2772 463 motor_stop(id);
soulx 0:2433ddae2772 464 wait_ms(500);
soulx 0:2433ddae2772 465 pc.printf("motor[2] stop down\n");
soulx 0:2433ddae2772 466 do {
soulx 0:2433ddae2772 467 motor_set(id,1);
soulx 0:2433ddae2772 468 } while(sw_LL_D == 0);
soulx 0:2433ddae2772 469 motor_stop(id);
soulx 0:2433ddae2772 470 wait_ms(500);
soulx 0:2433ddae2772 471 min_pos_LL = position_LL.read_u16();
soulx 0:2433ddae2772 472 pc.printf("min_pos_LL= %d\n",min_pos_LL);
soulx 0:2433ddae2772 473 break;
soulx 0:2433ddae2772 474
soulx 0:2433ddae2772 475 case 3:
soulx 0:2433ddae2772 476 uint8_t count=1000;
soulx 0:2433ddae2772 477 pc.printf("motor[3] run up\n");
soulx 0:2433ddae2772 478 while(sw_RU_U) {
soulx 0:2433ddae2772 479 motor_set(id,1);
soulx 0:2433ddae2772 480 }
soulx 0:2433ddae2772 481 motor_stop(id);
soulx 0:2433ddae2772 482 wait_ms(500);
soulx 0:2433ddae2772 483 pc.printf("motor[3] stop up\n");
soulx 0:2433ddae2772 484
soulx 0:2433ddae2772 485 do {
soulx 0:2433ddae2772 486 motor_set(id,2);
soulx 0:2433ddae2772 487
soulx 0:2433ddae2772 488 } while(sw_RU_U ==0);
soulx 0:2433ddae2772 489 motor_stop(id);
soulx 0:2433ddae2772 490 wait_ms(500);
soulx 0:2433ddae2772 491 max_pos_RU = position_RU.read_u16();
soulx 0:2433ddae2772 492 pc.printf("max_pos_RU= %d\n",max_pos_RU);
soulx 0:2433ddae2772 493
soulx 0:2433ddae2772 494 pc.printf("motor[3] run down\n");
soulx 0:2433ddae2772 495
soulx 0:2433ddae2772 496 /*while(count >1) {
soulx 0:2433ddae2772 497 motor_set(id,2);
soulx 0:2433ddae2772 498 if(sw_RU_D)
soulx 0:2433ddae2772 499 {
soulx 0:2433ddae2772 500 count--;
soulx 0:2433ddae2772 501 }
soulx 0:2433ddae2772 502 }*/
soulx 0:2433ddae2772 503 while(sw_RU_D)
soulx 0:2433ddae2772 504 {
soulx 0:2433ddae2772 505
soulx 0:2433ddae2772 506 motor_set(id,2);
soulx 0:2433ddae2772 507 }
soulx 0:2433ddae2772 508 motor_stop(id);
soulx 0:2433ddae2772 509 wait_ms(500);
soulx 0:2433ddae2772 510 pc.printf("motor[3] stop down\n");
soulx 0:2433ddae2772 511 do {
soulx 0:2433ddae2772 512 motor_set(id,1);
soulx 0:2433ddae2772 513 } while(sw_RU_D == 0);
soulx 0:2433ddae2772 514 motor_stop(id);
soulx 0:2433ddae2772 515 wait_ms(500);
soulx 0:2433ddae2772 516 min_pos_RU = position_RU.read_u16();
soulx 0:2433ddae2772 517 pc.printf("min_pos_RU= %d\n",min_pos_RU);
soulx 0:2433ddae2772 518 break;
soulx 0:2433ddae2772 519
soulx 0:2433ddae2772 520 case 4:
soulx 0:2433ddae2772 521
soulx 0:2433ddae2772 522 pc.printf("motor[4] run up\n");
soulx 0:2433ddae2772 523 while(sw_RL_U) {
soulx 0:2433ddae2772 524 motor_set(id,1);
soulx 0:2433ddae2772 525 }
soulx 0:2433ddae2772 526 motor_stop(id);
soulx 0:2433ddae2772 527 wait_ms(500);
soulx 0:2433ddae2772 528 pc.printf("motor[4] stop up\n");
soulx 0:2433ddae2772 529 do {
soulx 0:2433ddae2772 530 motor_set(id,2);
soulx 0:2433ddae2772 531 } while(sw_RL_U==0);
soulx 0:2433ddae2772 532 motor_stop(id);
soulx 0:2433ddae2772 533 wait_ms(500);
soulx 0:2433ddae2772 534 max_pos_RL = position_RL.read_u16();
soulx 0:2433ddae2772 535 pc.printf("max_pos_RL= %d\n",max_pos_RL);
soulx 0:2433ddae2772 536 pc.printf("motor[4] run down\n");
soulx 0:2433ddae2772 537 while(sw_RL_D) {
soulx 0:2433ddae2772 538 motor_set(id,2);
soulx 0:2433ddae2772 539 }
soulx 0:2433ddae2772 540 motor_stop(id);
soulx 0:2433ddae2772 541 wait_ms(500);
soulx 0:2433ddae2772 542 pc.printf("motor[4] stop down\n");
soulx 0:2433ddae2772 543 do {
soulx 0:2433ddae2772 544 motor_set(id,1);
soulx 0:2433ddae2772 545 } while(sw_RL_D == 0);
soulx 0:2433ddae2772 546 motor_stop(id);
soulx 0:2433ddae2772 547 wait_ms(500);
soulx 0:2433ddae2772 548 min_pos_RL = position_RL.read_u16();
soulx 0:2433ddae2772 549 pc.printf("min_pos_RL= %d\n",min_pos_RL);
soulx 0:2433ddae2772 550 break;
soulx 0:2433ddae2772 551 }
soulx 0:2433ddae2772 552
soulx 0:2433ddae2772 553 }