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@2:264ad36f1ad4, 2015-07-15 (annotated)

Committer:
soulx
Date:
Wed Jul 15 07:57:38 2015 +0000
Revision:
2:264ad36f1ad4
Parent:
1:768d359e9d96
Child:
4:a35291005a30
add function

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 1:768d359e9d96 61 uint16_t offset_pos =1000;
soulx 1:768d359e9d96 62
soulx 0:2433ddae2772 63 int16_t MARGIN = 500;
soulx 0:2433ddae2772 64
soulx 0:2433ddae2772 65 //Main function
soulx 0:2433ddae2772 66 int main()
soulx 0:2433ddae2772 67 {
soulx 0:2433ddae2772 68 uint16_t vr_lu,vr_ru;
soulx 0:2433ddae2772 69 uint16_t vr_ll,vr_rl;
soulx 0:2433ddae2772 70 pc.printf("wait\n");
soulx 0:2433ddae2772 71 motor_stop(0);
soulx 0:2433ddae2772 72 // wait(10);
soulx 0:2433ddae2772 73 /*
soulx 0:2433ddae2772 74 while(1) {
soulx 0:2433ddae2772 75 motor_set(1,1);
soulx 0:2433ddae2772 76 motor_set(2,1);
soulx 0:2433ddae2772 77 motor_set(3,1);
soulx 0:2433ddae2772 78 motor_set(4,1);
soulx 0:2433ddae2772 79 wait(1);
soulx 0:2433ddae2772 80 motor_set(1,2);
soulx 0:2433ddae2772 81 motor_set(2,2);
soulx 0:2433ddae2772 82 motor_set(3,2);
soulx 0:2433ddae2772 83 motor_set(4,2);
soulx 0:2433ddae2772 84 wait(1);
soulx 0:2433ddae2772 85 }
soulx 0:2433ddae2772 86 */
soulx 0:2433ddae2772 87
soulx 0:2433ddae2772 88 /*
soulx 0:2433ddae2772 89 while(1) {
soulx 0:2433ddae2772 90 //Read position
soulx 0:2433ddae2772 91 vr_ll = position_LL.read_u16();
soulx 0:2433ddae2772 92 vr_lu = position_LU.read_u16();
soulx 0:2433ddae2772 93 vr_rl = position_RL.read_u16();
soulx 0:2433ddae2772 94 vr_ru = position_RU.read_u16();
soulx 0:2433ddae2772 95 pc.printf("vr_LL = %d\t",vr_ll);
soulx 0:2433ddae2772 96 pc.printf("vr_LU = %d\t",vr_lu);
soulx 0:2433ddae2772 97 pc.printf("vr_RL = %d\t",vr_rl);
soulx 0:2433ddae2772 98 pc.printf("vr_RU = %d\n",vr_ru);
soulx 0:2433ddae2772 99 }
soulx 0:2433ddae2772 100 */
soulx 0:2433ddae2772 101
soulx 0:2433ddae2772 102 /*
soulx 0:2433ddae2772 103 while(1) {
soulx 0:2433ddae2772 104 myled =1;
soulx 0:2433ddae2772 105 wait_ms(200);
soulx 0:2433ddae2772 106
soulx 0:2433ddae2772 107 if(mybutton == 0) {
soulx 0:2433ddae2772 108 myled =0;
soulx 0:2433ddae2772 109 wait_ms(200);
soulx 0:2433ddae2772 110 }
soulx 0:2433ddae2772 111
soulx 0:2433ddae2772 112 if(sw_LU_U == 0) {
soulx 0:2433ddae2772 113 myled =0;
soulx 0:2433ddae2772 114 wait_ms(200);
soulx 0:2433ddae2772 115 }
soulx 0:2433ddae2772 116
soulx 0:2433ddae2772 117 if(sw_LU_D == 0) {
soulx 0:2433ddae2772 118 myled =0;
soulx 0:2433ddae2772 119 wait_ms(200);
soulx 0:2433ddae2772 120 }
soulx 0:2433ddae2772 121
soulx 0:2433ddae2772 122 if(sw_LL_U == 0) {
soulx 0:2433ddae2772 123 myled =0;
soulx 0:2433ddae2772 124 wait_ms(200);
soulx 0:2433ddae2772 125 }
soulx 0:2433ddae2772 126
soulx 0:2433ddae2772 127 if(sw_LL_D == 0) {
soulx 0:2433ddae2772 128 myled =0;
soulx 0:2433ddae2772 129 wait_ms(200);
soulx 0:2433ddae2772 130 }
soulx 0:2433ddae2772 131
soulx 0:2433ddae2772 132 if(sw_RU_U == 0) {
soulx 0:2433ddae2772 133 myled =0;
soulx 0:2433ddae2772 134 wait_ms(200);
soulx 0:2433ddae2772 135 }
soulx 0:2433ddae2772 136
soulx 0:2433ddae2772 137 if(sw_RU_D == 0) {
soulx 0:2433ddae2772 138 myled =0;
soulx 0:2433ddae2772 139 wait_ms(200);
soulx 0:2433ddae2772 140 }
soulx 0:2433ddae2772 141
soulx 0:2433ddae2772 142 if(sw_RL_U == 0) {
soulx 0:2433ddae2772 143 myled =0;
soulx 0:2433ddae2772 144 wait_ms(200);
soulx 0:2433ddae2772 145 }
soulx 0:2433ddae2772 146
soulx 0:2433ddae2772 147 if(sw_RL_D == 0) {
soulx 0:2433ddae2772 148 myled =0;
soulx 0:2433ddae2772 149 wait_ms(200);
soulx 0:2433ddae2772 150 }
soulx 0:2433ddae2772 151
soulx 0:2433ddae2772 152 }
soulx 0:2433ddae2772 153 */
soulx 0:2433ddae2772 154
soulx 1:768d359e9d96 155 // while(1) {
soulx 1:768d359e9d96 156 //calibration
soulx 1:768d359e9d96 157 pc.printf("Welcome to DOGWHEELSCHAIR\n");
soulx 1:768d359e9d96 158 pc.printf("Calibration [START]\n");
soulx 1:768d359e9d96 159 calibration(1);
soulx 1:768d359e9d96 160 calibration(2);
soulx 1:768d359e9d96 161 calibration(3);
soulx 1:768d359e9d96 162 calibration(4);
soulx 1:768d359e9d96 163 pc.printf("Calibration [FINISH]\n");
soulx 1:768d359e9d96 164 pc.printf("RUN mode [START]\n");
soulx 1:768d359e9d96 165 // }
soulx 0:2433ddae2772 166
soulx 0:2433ddae2772 167
soulx 0:2433ddae2772 168 uint8_t count=0;
soulx 0:2433ddae2772 169
soulx 0:2433ddae2772 170 while(1) {
soulx 0:2433ddae2772 171 uint8_t state=0;
soulx 0:2433ddae2772 172 pc.printf("Count %d",count);
soulx 0:2433ddae2772 173 do {
soulx 0:2433ddae2772 174 state=0;
soulx 0:2433ddae2772 175
soulx 1:768d359e9d96 176 if(position_control(1,position_LU.read_u16(),max_pos_LU-offset_pos) == 1) {
soulx 0:2433ddae2772 177 state++;
soulx 0:2433ddae2772 178 }
soulx 0:2433ddae2772 179
soulx 1:768d359e9d96 180 if(position_control(2,position_LL.read_u16(),max_pos_LL-offset_pos) == 1) {
soulx 0:2433ddae2772 181 state++;
soulx 0:2433ddae2772 182 }
soulx 0:2433ddae2772 183
soulx 1:768d359e9d96 184 if(position_control(3,position_RU.read_u16(),max_pos_RU-offset_pos) == 1) {
soulx 0:2433ddae2772 185 state++;
soulx 0:2433ddae2772 186 }
soulx 0:2433ddae2772 187
soulx 1:768d359e9d96 188 if(position_control(4,position_RL.read_u16(),max_pos_RL-offset_pos) == 1) {
soulx 0:2433ddae2772 189 state++;
soulx 0:2433ddae2772 190 }
soulx 0:2433ddae2772 191
soulx 0:2433ddae2772 192 pc.printf("state = %d",state);
soulx 0:2433ddae2772 193 } while(state <= 4 );
soulx 0:2433ddae2772 194
soulx 0:2433ddae2772 195 do {
soulx 0:2433ddae2772 196 state=0;
soulx 0:2433ddae2772 197
soulx 1:768d359e9d96 198 if(position_control(1,position_LU.read_u16(),min_pos_LU+offset_pos) == 1) {
soulx 0:2433ddae2772 199 state++;
soulx 0:2433ddae2772 200 }
soulx 0:2433ddae2772 201
soulx 1:768d359e9d96 202 if(position_control(2,position_LL.read_u16(),min_pos_LL+offset_pos) == 1) {
soulx 0:2433ddae2772 203 state++;
soulx 0:2433ddae2772 204 }
soulx 0:2433ddae2772 205
soulx 1:768d359e9d96 206 if(position_control(3,position_RU.read_u16(),min_pos_RU+offset_pos) == 1) {
soulx 0:2433ddae2772 207 state++;
soulx 0:2433ddae2772 208 }
soulx 0:2433ddae2772 209
soulx 1:768d359e9d96 210 if(position_control(4,position_RL.read_u16(),min_pos_RL+offset_pos) == 1) {
soulx 0:2433ddae2772 211 state++;
soulx 0:2433ddae2772 212 }
soulx 0:2433ddae2772 213
soulx 0:2433ddae2772 214 pc.printf("state = %d",state);
soulx 0:2433ddae2772 215 } while(state <= 4 );
soulx 0:2433ddae2772 216 count++;
soulx 0:2433ddae2772 217 }
soulx 0:2433ddae2772 218 }
soulx 0:2433ddae2772 219
soulx 0:2433ddae2772 220
soulx 0:2433ddae2772 221
soulx 0:2433ddae2772 222 void motor_set(uint8_t id, uint8_t direct)
soulx 0:2433ddae2772 223 {
soulx 0:2433ddae2772 224 //direct: Should be between 0 and 3, with the following result
soulx 0:2433ddae2772 225 //0: Brake to VCC
soulx 0:2433ddae2772 226 //1: Clockwise
soulx 0:2433ddae2772 227 //2: CounterClockwise
soulx 0:2433ddae2772 228 //3: Brake to GND
soulx 0:2433ddae2772 229
soulx 0:2433ddae2772 230 if(direct <=4) {
soulx 0:2433ddae2772 231 switch(id) {
soulx 0:2433ddae2772 232 case 1:
soulx 0:2433ddae2772 233 // Set inA[motor]
soulx 0:2433ddae2772 234 if (direct <=1)
soulx 0:2433ddae2772 235 dirA_LU=0;
soulx 0:2433ddae2772 236 else
soulx 0:2433ddae2772 237 dirA_LU=1;
soulx 0:2433ddae2772 238
soulx 0:2433ddae2772 239 // Set inB[motor]
soulx 0:2433ddae2772 240 if ((direct==0)||(direct==2))
soulx 0:2433ddae2772 241 dirB_LU=0;
soulx 0:2433ddae2772 242 else
soulx 0:2433ddae2772 243 dirB_LU=1;
soulx 0:2433ddae2772 244 break;
soulx 0:2433ddae2772 245
soulx 0:2433ddae2772 246 case 2:
soulx 0:2433ddae2772 247 // Set inA[motor]
soulx 0:2433ddae2772 248 if (direct <=1)
soulx 0:2433ddae2772 249 dirA_LL=0;
soulx 0:2433ddae2772 250 else
soulx 0:2433ddae2772 251 dirA_LL=1;
soulx 0:2433ddae2772 252
soulx 0:2433ddae2772 253 // Set inB[motor]
soulx 0:2433ddae2772 254 if ((direct==0)||(direct==2))
soulx 0:2433ddae2772 255 dirB_LL=0;
soulx 0:2433ddae2772 256 else
soulx 0:2433ddae2772 257 dirB_LL=1;
soulx 0:2433ddae2772 258 break;
soulx 0:2433ddae2772 259
soulx 0:2433ddae2772 260 case 3:
soulx 0:2433ddae2772 261 // Set inA[motor]
soulx 0:2433ddae2772 262 if (direct <=1)
soulx 0:2433ddae2772 263 dirA_RU=0;
soulx 0:2433ddae2772 264 else
soulx 0:2433ddae2772 265 dirA_RU=1;
soulx 0:2433ddae2772 266
soulx 0:2433ddae2772 267 // Set inB[motor]
soulx 0:2433ddae2772 268 if ((direct==0)||(direct==2))
soulx 0:2433ddae2772 269 dirB_RU=0;
soulx 0:2433ddae2772 270 else
soulx 0:2433ddae2772 271 dirB_RU=1;
soulx 0:2433ddae2772 272 break;
soulx 0:2433ddae2772 273
soulx 0:2433ddae2772 274 case 4:
soulx 0:2433ddae2772 275 // Set inA[motor]
soulx 0:2433ddae2772 276 if (direct <=1)
soulx 0:2433ddae2772 277 dirA_RL=0;
soulx 0:2433ddae2772 278 else
soulx 0:2433ddae2772 279 dirA_RL=1;
soulx 0:2433ddae2772 280 // Set inB[motor]
soulx 0:2433ddae2772 281 if ((direct==0)||(direct==2))
soulx 0:2433ddae2772 282 dirB_RL=0;
soulx 0:2433ddae2772 283 else
soulx 0:2433ddae2772 284 dirB_RL=1;
soulx 0:2433ddae2772 285 break;
soulx 0:2433ddae2772 286 }
soulx 0:2433ddae2772 287 }
soulx 0:2433ddae2772 288 }
soulx 0:2433ddae2772 289
soulx 0:2433ddae2772 290 void motor_stop(uint8_t id)
soulx 0:2433ddae2772 291 {
soulx 0:2433ddae2772 292 switch(id) {
soulx 0:2433ddae2772 293 case 1:
soulx 0:2433ddae2772 294 dirA_LU=1;
soulx 0:2433ddae2772 295 dirB_LU=1;
soulx 0:2433ddae2772 296 break;
soulx 0:2433ddae2772 297
soulx 0:2433ddae2772 298 case 2:
soulx 0:2433ddae2772 299 dirA_LL=1;
soulx 0:2433ddae2772 300 dirB_LL=1;
soulx 0:2433ddae2772 301 break;
soulx 0:2433ddae2772 302
soulx 0:2433ddae2772 303 case 3:
soulx 0:2433ddae2772 304 dirA_RU=1;
soulx 0:2433ddae2772 305 dirB_RU=1;
soulx 0:2433ddae2772 306 break;
soulx 0:2433ddae2772 307
soulx 0:2433ddae2772 308 case 4:
soulx 0:2433ddae2772 309 dirA_RL=1;
soulx 0:2433ddae2772 310 dirB_RL=1;
soulx 0:2433ddae2772 311 break;
soulx 0:2433ddae2772 312
soulx 0:2433ddae2772 313 case 0:
soulx 0:2433ddae2772 314 dirA_LU=1;
soulx 0:2433ddae2772 315 dirB_LU=1;
soulx 0:2433ddae2772 316
soulx 0:2433ddae2772 317 dirA_LL=1;
soulx 0:2433ddae2772 318 dirB_LL=1;
soulx 0:2433ddae2772 319
soulx 0:2433ddae2772 320 dirA_RU=1;
soulx 0:2433ddae2772 321 dirB_RU=1;
soulx 0:2433ddae2772 322
soulx 0:2433ddae2772 323 dirA_RL=1;
soulx 0:2433ddae2772 324 dirB_RL=1;
soulx 0:2433ddae2772 325 break;
soulx 0:2433ddae2772 326 }
soulx 0:2433ddae2772 327 }
soulx 0:2433ddae2772 328
soulx 0:2433ddae2772 329
soulx 0:2433ddae2772 330
soulx 0:2433ddae2772 331 uint8_t limit_motor(uint8_t id, uint8_t dirction)
soulx 0:2433ddae2772 332 {
soulx 0:2433ddae2772 333 switch(id) {
soulx 0:2433ddae2772 334 case 1://Left Upper
soulx 0:2433ddae2772 335 if(sw_LU_U && sw_LU_D) {
soulx 0:2433ddae2772 336 motor_set(id,dirction);
soulx 0:2433ddae2772 337 } else {
soulx 0:2433ddae2772 338 motor_stop(id);
soulx 0:2433ddae2772 339 return 0;
soulx 0:2433ddae2772 340 }
soulx 0:2433ddae2772 341 break;
soulx 0:2433ddae2772 342
soulx 0:2433ddae2772 343 case 2://Left Lowwer
soulx 0:2433ddae2772 344 if(sw_LL_U && sw_LL_D) {
soulx 0:2433ddae2772 345 motor_set(id,dirction);
soulx 0:2433ddae2772 346 } else {
soulx 0:2433ddae2772 347 motor_stop(id);
soulx 0:2433ddae2772 348 return 0;
soulx 0:2433ddae2772 349 }
soulx 0:2433ddae2772 350 break;
soulx 0:2433ddae2772 351
soulx 0:2433ddae2772 352 case 3://Right Upper
soulx 0:2433ddae2772 353 if(sw_RU_U && sw_RU_D) {
soulx 0:2433ddae2772 354 motor_set(id,dirction);
soulx 0:2433ddae2772 355 } else {
soulx 0:2433ddae2772 356 motor_stop(id);
soulx 0:2433ddae2772 357 return 0;
soulx 0:2433ddae2772 358 }
soulx 0:2433ddae2772 359 break;
soulx 0:2433ddae2772 360
soulx 0:2433ddae2772 361 case 4://Right Lowwer
soulx 0:2433ddae2772 362 if(sw_RL_U && sw_RL_D) {
soulx 0:2433ddae2772 363 motor_set(id,dirction);
soulx 0:2433ddae2772 364 } else {
soulx 0:2433ddae2772 365 motor_stop(id);
soulx 0:2433ddae2772 366 return 0;
soulx 0:2433ddae2772 367 }
soulx 0:2433ddae2772 368 break;
soulx 0:2433ddae2772 369 }
soulx 0:2433ddae2772 370 return 1;//normally
soulx 0:2433ddae2772 371 }
soulx 0:2433ddae2772 372
soulx 0:2433ddae2772 373
soulx 0:2433ddae2772 374 uint8_t position_control(uint8_t id, uint16_t current, uint16_t target)
soulx 0:2433ddae2772 375 {
soulx 0:2433ddae2772 376 //uint8_t state=0;
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 1:768d359e9d96 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 1:768d359e9d96 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 1:768d359e9d96 503 while(sw_RU_D) {
soulx 1:768d359e9d96 504
soulx 0:2433ddae2772 505 motor_set(id,2);
soulx 1:768d359e9d96 506 }
soulx 0:2433ddae2772 507 motor_stop(id);
soulx 0:2433ddae2772 508 wait_ms(500);
soulx 0:2433ddae2772 509 pc.printf("motor[3] stop down\n");
soulx 0:2433ddae2772 510 do {
soulx 0:2433ddae2772 511 motor_set(id,1);
soulx 0:2433ddae2772 512 } while(sw_RU_D == 0);
soulx 0:2433ddae2772 513 motor_stop(id);
soulx 0:2433ddae2772 514 wait_ms(500);
soulx 0:2433ddae2772 515 min_pos_RU = position_RU.read_u16();
soulx 0:2433ddae2772 516 pc.printf("min_pos_RU= %d\n",min_pos_RU);
soulx 0:2433ddae2772 517 break;
soulx 0:2433ddae2772 518
soulx 0:2433ddae2772 519 case 4:
soulx 0:2433ddae2772 520
soulx 0:2433ddae2772 521 pc.printf("motor[4] run up\n");
soulx 0:2433ddae2772 522 while(sw_RL_U) {
soulx 0:2433ddae2772 523 motor_set(id,1);
soulx 0:2433ddae2772 524 }
soulx 0:2433ddae2772 525 motor_stop(id);
soulx 0:2433ddae2772 526 wait_ms(500);
soulx 0:2433ddae2772 527 pc.printf("motor[4] stop up\n");
soulx 0:2433ddae2772 528 do {
soulx 0:2433ddae2772 529 motor_set(id,2);
soulx 0:2433ddae2772 530 } while(sw_RL_U==0);
soulx 0:2433ddae2772 531 motor_stop(id);
soulx 0:2433ddae2772 532 wait_ms(500);
soulx 0:2433ddae2772 533 max_pos_RL = position_RL.read_u16();
soulx 0:2433ddae2772 534 pc.printf("max_pos_RL= %d\n",max_pos_RL);
soulx 0:2433ddae2772 535 pc.printf("motor[4] run down\n");
soulx 0:2433ddae2772 536 while(sw_RL_D) {
soulx 0:2433ddae2772 537 motor_set(id,2);
soulx 0:2433ddae2772 538 }
soulx 0:2433ddae2772 539 motor_stop(id);
soulx 0:2433ddae2772 540 wait_ms(500);
soulx 0:2433ddae2772 541 pc.printf("motor[4] stop down\n");
soulx 0:2433ddae2772 542 do {
soulx 0:2433ddae2772 543 motor_set(id,1);
soulx 0:2433ddae2772 544 } while(sw_RL_D == 0);
soulx 0:2433ddae2772 545 motor_stop(id);
soulx 0:2433ddae2772 546 wait_ms(500);
soulx 0:2433ddae2772 547 min_pos_RL = position_RL.read_u16();
soulx 0:2433ddae2772 548 pc.printf("min_pos_RL= %d\n",min_pos_RL);
soulx 0:2433ddae2772 549 break;
soulx 0:2433ddae2772 550 }
soulx 0:2433ddae2772 551
soulx 1:768d359e9d96 552 }
soulx 2:264ad36f1ad4 553
soulx 1:768d359e9d96 554 uint16_t convert(uint16_t data)
soulx 1:768d359e9d96 555 {
soulx 1:768d359e9d96 556 uint16_t ans=0;
soulx 1:768d359e9d96 557
soulx 1:768d359e9d96 558 //ans =
soulx 1:768d359e9d96 559
soulx 1:768d359e9d96 560 return
soulx 1:768d359e9d96 561 }
soulx 1:768d359e9d96 562
soulx 1:768d359e9d96 563 uint16_t scale(uint16_t data)
soulx 1:768d359e9d96 564 {
soulx 1:768d359e9d96 565
soulx 2:264ad36f1ad4 566 }