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@3:bb5fbe510fa5, 2015-07-15 (annotated)

Committer:
Pitiwut
Date:
Wed Jul 15 07:58:50 2015 +0000
Revision:
3:bb5fbe510fa5
Parent:
1:768d359e9d96
Child:
4:a35291005a30
test

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;
Pitiwut 3:bb5fbe510fa5 62 uint16_t test_variable =1000;
soulx 1:768d359e9d96 63
soulx 0:2433ddae2772 64 int16_t MARGIN = 500;
soulx 0:2433ddae2772 65
soulx 0:2433ddae2772 66 //Main function
soulx 0:2433ddae2772 67 int main()
soulx 0:2433ddae2772 68 {
soulx 0:2433ddae2772 69 uint16_t vr_lu,vr_ru;
soulx 0:2433ddae2772 70 uint16_t vr_ll,vr_rl;
soulx 0:2433ddae2772 71 pc.printf("wait\n");
soulx 0:2433ddae2772 72 motor_stop(0);
soulx 0:2433ddae2772 73 // wait(10);
soulx 0:2433ddae2772 74 /*
soulx 0:2433ddae2772 75 while(1) {
soulx 0:2433ddae2772 76 motor_set(1,1);
soulx 0:2433ddae2772 77 motor_set(2,1);
soulx 0:2433ddae2772 78 motor_set(3,1);
soulx 0:2433ddae2772 79 motor_set(4,1);
soulx 0:2433ddae2772 80 wait(1);
soulx 0:2433ddae2772 81 motor_set(1,2);
soulx 0:2433ddae2772 82 motor_set(2,2);
soulx 0:2433ddae2772 83 motor_set(3,2);
soulx 0:2433ddae2772 84 motor_set(4,2);
soulx 0:2433ddae2772 85 wait(1);
soulx 0:2433ddae2772 86 }
soulx 0:2433ddae2772 87 */
soulx 0:2433ddae2772 88
soulx 0:2433ddae2772 89 /*
soulx 0:2433ddae2772 90 while(1) {
soulx 0:2433ddae2772 91 //Read position
soulx 0:2433ddae2772 92 vr_ll = position_LL.read_u16();
soulx 0:2433ddae2772 93 vr_lu = position_LU.read_u16();
soulx 0:2433ddae2772 94 vr_rl = position_RL.read_u16();
soulx 0:2433ddae2772 95 vr_ru = position_RU.read_u16();
soulx 0:2433ddae2772 96 pc.printf("vr_LL = %d\t",vr_ll);
soulx 0:2433ddae2772 97 pc.printf("vr_LU = %d\t",vr_lu);
soulx 0:2433ddae2772 98 pc.printf("vr_RL = %d\t",vr_rl);
soulx 0:2433ddae2772 99 pc.printf("vr_RU = %d\n",vr_ru);
soulx 0:2433ddae2772 100 }
soulx 0:2433ddae2772 101 */
soulx 0:2433ddae2772 102
soulx 0:2433ddae2772 103 /*
soulx 0:2433ddae2772 104 while(1) {
soulx 0:2433ddae2772 105 myled =1;
soulx 0:2433ddae2772 106 wait_ms(200);
soulx 0:2433ddae2772 107
soulx 0:2433ddae2772 108 if(mybutton == 0) {
soulx 0:2433ddae2772 109 myled =0;
soulx 0:2433ddae2772 110 wait_ms(200);
soulx 0:2433ddae2772 111 }
soulx 0:2433ddae2772 112
soulx 0:2433ddae2772 113 if(sw_LU_U == 0) {
soulx 0:2433ddae2772 114 myled =0;
soulx 0:2433ddae2772 115 wait_ms(200);
soulx 0:2433ddae2772 116 }
soulx 0:2433ddae2772 117
soulx 0:2433ddae2772 118 if(sw_LU_D == 0) {
soulx 0:2433ddae2772 119 myled =0;
soulx 0:2433ddae2772 120 wait_ms(200);
soulx 0:2433ddae2772 121 }
soulx 0:2433ddae2772 122
soulx 0:2433ddae2772 123 if(sw_LL_U == 0) {
soulx 0:2433ddae2772 124 myled =0;
soulx 0:2433ddae2772 125 wait_ms(200);
soulx 0:2433ddae2772 126 }
soulx 0:2433ddae2772 127
soulx 0:2433ddae2772 128 if(sw_LL_D == 0) {
soulx 0:2433ddae2772 129 myled =0;
soulx 0:2433ddae2772 130 wait_ms(200);
soulx 0:2433ddae2772 131 }
soulx 0:2433ddae2772 132
soulx 0:2433ddae2772 133 if(sw_RU_U == 0) {
soulx 0:2433ddae2772 134 myled =0;
soulx 0:2433ddae2772 135 wait_ms(200);
soulx 0:2433ddae2772 136 }
soulx 0:2433ddae2772 137
soulx 0:2433ddae2772 138 if(sw_RU_D == 0) {
soulx 0:2433ddae2772 139 myled =0;
soulx 0:2433ddae2772 140 wait_ms(200);
soulx 0:2433ddae2772 141 }
soulx 0:2433ddae2772 142
soulx 0:2433ddae2772 143 if(sw_RL_U == 0) {
soulx 0:2433ddae2772 144 myled =0;
soulx 0:2433ddae2772 145 wait_ms(200);
soulx 0:2433ddae2772 146 }
soulx 0:2433ddae2772 147
soulx 0:2433ddae2772 148 if(sw_RL_D == 0) {
soulx 0:2433ddae2772 149 myled =0;
soulx 0:2433ddae2772 150 wait_ms(200);
soulx 0:2433ddae2772 151 }
soulx 0:2433ddae2772 152
soulx 0:2433ddae2772 153 }
soulx 0:2433ddae2772 154 */
soulx 0:2433ddae2772 155
soulx 1:768d359e9d96 156 // while(1) {
soulx 1:768d359e9d96 157 //calibration
soulx 1:768d359e9d96 158 pc.printf("Welcome to DOGWHEELSCHAIR\n");
soulx 1:768d359e9d96 159 pc.printf("Calibration [START]\n");
soulx 1:768d359e9d96 160 calibration(1);
soulx 1:768d359e9d96 161 calibration(2);
soulx 1:768d359e9d96 162 calibration(3);
soulx 1:768d359e9d96 163 calibration(4);
soulx 1:768d359e9d96 164 pc.printf("Calibration [FINISH]\n");
soulx 1:768d359e9d96 165 pc.printf("RUN mode [START]\n");
soulx 1:768d359e9d96 166 // }
soulx 0:2433ddae2772 167
soulx 0:2433ddae2772 168
soulx 0:2433ddae2772 169 uint8_t count=0;
soulx 0:2433ddae2772 170
soulx 0:2433ddae2772 171 while(1) {
soulx 0:2433ddae2772 172 uint8_t state=0;
soulx 0:2433ddae2772 173 pc.printf("Count %d",count);
soulx 0:2433ddae2772 174 do {
soulx 0:2433ddae2772 175 state=0;
soulx 0:2433ddae2772 176
soulx 1:768d359e9d96 177 if(position_control(1,position_LU.read_u16(),max_pos_LU-offset_pos) == 1) {
soulx 0:2433ddae2772 178 state++;
soulx 0:2433ddae2772 179 }
soulx 0:2433ddae2772 180
soulx 1:768d359e9d96 181 if(position_control(2,position_LL.read_u16(),max_pos_LL-offset_pos) == 1) {
soulx 0:2433ddae2772 182 state++;
soulx 0:2433ddae2772 183 }
soulx 0:2433ddae2772 184
soulx 1:768d359e9d96 185 if(position_control(3,position_RU.read_u16(),max_pos_RU-offset_pos) == 1) {
soulx 0:2433ddae2772 186 state++;
soulx 0:2433ddae2772 187 }
soulx 0:2433ddae2772 188
soulx 1:768d359e9d96 189 if(position_control(4,position_RL.read_u16(),max_pos_RL-offset_pos) == 1) {
soulx 0:2433ddae2772 190 state++;
soulx 0:2433ddae2772 191 }
soulx 0:2433ddae2772 192
soulx 0:2433ddae2772 193 pc.printf("state = %d",state);
soulx 0:2433ddae2772 194 } while(state <= 4 );
soulx 0:2433ddae2772 195
soulx 0:2433ddae2772 196 do {
soulx 0:2433ddae2772 197 state=0;
soulx 0:2433ddae2772 198
soulx 1:768d359e9d96 199 if(position_control(1,position_LU.read_u16(),min_pos_LU+offset_pos) == 1) {
soulx 0:2433ddae2772 200 state++;
soulx 0:2433ddae2772 201 }
soulx 0:2433ddae2772 202
soulx 1:768d359e9d96 203 if(position_control(2,position_LL.read_u16(),min_pos_LL+offset_pos) == 1) {
soulx 0:2433ddae2772 204 state++;
soulx 0:2433ddae2772 205 }
soulx 0:2433ddae2772 206
soulx 1:768d359e9d96 207 if(position_control(3,position_RU.read_u16(),min_pos_RU+offset_pos) == 1) {
soulx 0:2433ddae2772 208 state++;
soulx 0:2433ddae2772 209 }
soulx 0:2433ddae2772 210
soulx 1:768d359e9d96 211 if(position_control(4,position_RL.read_u16(),min_pos_RL+offset_pos) == 1) {
soulx 0:2433ddae2772 212 state++;
soulx 0:2433ddae2772 213 }
soulx 0:2433ddae2772 214
soulx 0:2433ddae2772 215 pc.printf("state = %d",state);
soulx 0:2433ddae2772 216 } while(state <= 4 );
soulx 0:2433ddae2772 217 count++;
soulx 0:2433ddae2772 218 }
soulx 0:2433ddae2772 219 }
soulx 0:2433ddae2772 220
soulx 0:2433ddae2772 221
soulx 0:2433ddae2772 222
soulx 0:2433ddae2772 223 void motor_set(uint8_t id, uint8_t direct)
soulx 0:2433ddae2772 224 {
soulx 0:2433ddae2772 225 //direct: Should be between 0 and 3, with the following result
soulx 0:2433ddae2772 226 //0: Brake to VCC
soulx 0:2433ddae2772 227 //1: Clockwise
soulx 0:2433ddae2772 228 //2: CounterClockwise
soulx 0:2433ddae2772 229 //3: Brake to GND
soulx 0:2433ddae2772 230
soulx 0:2433ddae2772 231 if(direct <=4) {
soulx 0:2433ddae2772 232 switch(id) {
soulx 0:2433ddae2772 233 case 1:
soulx 0:2433ddae2772 234 // Set inA[motor]
soulx 0:2433ddae2772 235 if (direct <=1)
soulx 0:2433ddae2772 236 dirA_LU=0;
soulx 0:2433ddae2772 237 else
soulx 0:2433ddae2772 238 dirA_LU=1;
soulx 0:2433ddae2772 239
soulx 0:2433ddae2772 240 // Set inB[motor]
soulx 0:2433ddae2772 241 if ((direct==0)||(direct==2))
soulx 0:2433ddae2772 242 dirB_LU=0;
soulx 0:2433ddae2772 243 else
soulx 0:2433ddae2772 244 dirB_LU=1;
soulx 0:2433ddae2772 245 break;
soulx 0:2433ddae2772 246
soulx 0:2433ddae2772 247 case 2:
soulx 0:2433ddae2772 248 // Set inA[motor]
soulx 0:2433ddae2772 249 if (direct <=1)
soulx 0:2433ddae2772 250 dirA_LL=0;
soulx 0:2433ddae2772 251 else
soulx 0:2433ddae2772 252 dirA_LL=1;
soulx 0:2433ddae2772 253
soulx 0:2433ddae2772 254 // Set inB[motor]
soulx 0:2433ddae2772 255 if ((direct==0)||(direct==2))
soulx 0:2433ddae2772 256 dirB_LL=0;
soulx 0:2433ddae2772 257 else
soulx 0:2433ddae2772 258 dirB_LL=1;
soulx 0:2433ddae2772 259 break;
soulx 0:2433ddae2772 260
soulx 0:2433ddae2772 261 case 3:
soulx 0:2433ddae2772 262 // Set inA[motor]
soulx 0:2433ddae2772 263 if (direct <=1)
soulx 0:2433ddae2772 264 dirA_RU=0;
soulx 0:2433ddae2772 265 else
soulx 0:2433ddae2772 266 dirA_RU=1;
soulx 0:2433ddae2772 267
soulx 0:2433ddae2772 268 // Set inB[motor]
soulx 0:2433ddae2772 269 if ((direct==0)||(direct==2))
soulx 0:2433ddae2772 270 dirB_RU=0;
soulx 0:2433ddae2772 271 else
soulx 0:2433ddae2772 272 dirB_RU=1;
soulx 0:2433ddae2772 273 break;
soulx 0:2433ddae2772 274
soulx 0:2433ddae2772 275 case 4:
soulx 0:2433ddae2772 276 // Set inA[motor]
soulx 0:2433ddae2772 277 if (direct <=1)
soulx 0:2433ddae2772 278 dirA_RL=0;
soulx 0:2433ddae2772 279 else
soulx 0:2433ddae2772 280 dirA_RL=1;
soulx 0:2433ddae2772 281 // Set inB[motor]
soulx 0:2433ddae2772 282 if ((direct==0)||(direct==2))
soulx 0:2433ddae2772 283 dirB_RL=0;
soulx 0:2433ddae2772 284 else
soulx 0:2433ddae2772 285 dirB_RL=1;
soulx 0:2433ddae2772 286 break;
soulx 0:2433ddae2772 287 }
soulx 0:2433ddae2772 288 }
soulx 0:2433ddae2772 289 }
soulx 0:2433ddae2772 290
soulx 0:2433ddae2772 291 void motor_stop(uint8_t id)
soulx 0:2433ddae2772 292 {
soulx 0:2433ddae2772 293 switch(id) {
soulx 0:2433ddae2772 294 case 1:
soulx 0:2433ddae2772 295 dirA_LU=1;
soulx 0:2433ddae2772 296 dirB_LU=1;
soulx 0:2433ddae2772 297 break;
soulx 0:2433ddae2772 298
soulx 0:2433ddae2772 299 case 2:
soulx 0:2433ddae2772 300 dirA_LL=1;
soulx 0:2433ddae2772 301 dirB_LL=1;
soulx 0:2433ddae2772 302 break;
soulx 0:2433ddae2772 303
soulx 0:2433ddae2772 304 case 3:
soulx 0:2433ddae2772 305 dirA_RU=1;
soulx 0:2433ddae2772 306 dirB_RU=1;
soulx 0:2433ddae2772 307 break;
soulx 0:2433ddae2772 308
soulx 0:2433ddae2772 309 case 4:
soulx 0:2433ddae2772 310 dirA_RL=1;
soulx 0:2433ddae2772 311 dirB_RL=1;
soulx 0:2433ddae2772 312 break;
soulx 0:2433ddae2772 313
soulx 0:2433ddae2772 314 case 0:
soulx 0:2433ddae2772 315 dirA_LU=1;
soulx 0:2433ddae2772 316 dirB_LU=1;
soulx 0:2433ddae2772 317
soulx 0:2433ddae2772 318 dirA_LL=1;
soulx 0:2433ddae2772 319 dirB_LL=1;
soulx 0:2433ddae2772 320
soulx 0:2433ddae2772 321 dirA_RU=1;
soulx 0:2433ddae2772 322 dirB_RU=1;
soulx 0:2433ddae2772 323
soulx 0:2433ddae2772 324 dirA_RL=1;
soulx 0:2433ddae2772 325 dirB_RL=1;
soulx 0:2433ddae2772 326 break;
soulx 0:2433ddae2772 327 }
soulx 0:2433ddae2772 328 }
soulx 0:2433ddae2772 329
soulx 0:2433ddae2772 330
soulx 0:2433ddae2772 331
soulx 0:2433ddae2772 332 uint8_t limit_motor(uint8_t id, uint8_t dirction)
soulx 0:2433ddae2772 333 {
soulx 0:2433ddae2772 334 switch(id) {
soulx 0:2433ddae2772 335 case 1://Left Upper
soulx 0:2433ddae2772 336 if(sw_LU_U && sw_LU_D) {
soulx 0:2433ddae2772 337 motor_set(id,dirction);
soulx 0:2433ddae2772 338 } else {
soulx 0:2433ddae2772 339 motor_stop(id);
soulx 0:2433ddae2772 340 return 0;
soulx 0:2433ddae2772 341 }
soulx 0:2433ddae2772 342 break;
soulx 0:2433ddae2772 343
soulx 0:2433ddae2772 344 case 2://Left Lowwer
soulx 0:2433ddae2772 345 if(sw_LL_U && sw_LL_D) {
soulx 0:2433ddae2772 346 motor_set(id,dirction);
soulx 0:2433ddae2772 347 } else {
soulx 0:2433ddae2772 348 motor_stop(id);
soulx 0:2433ddae2772 349 return 0;
soulx 0:2433ddae2772 350 }
soulx 0:2433ddae2772 351 break;
soulx 0:2433ddae2772 352
soulx 0:2433ddae2772 353 case 3://Right Upper
soulx 0:2433ddae2772 354 if(sw_RU_U && sw_RU_D) {
soulx 0:2433ddae2772 355 motor_set(id,dirction);
soulx 0:2433ddae2772 356 } else {
soulx 0:2433ddae2772 357 motor_stop(id);
soulx 0:2433ddae2772 358 return 0;
soulx 0:2433ddae2772 359 }
soulx 0:2433ddae2772 360 break;
soulx 0:2433ddae2772 361
soulx 0:2433ddae2772 362 case 4://Right Lowwer
soulx 0:2433ddae2772 363 if(sw_RL_U && sw_RL_D) {
soulx 0:2433ddae2772 364 motor_set(id,dirction);
soulx 0:2433ddae2772 365 } else {
soulx 0:2433ddae2772 366 motor_stop(id);
soulx 0:2433ddae2772 367 return 0;
soulx 0:2433ddae2772 368 }
soulx 0:2433ddae2772 369 break;
soulx 0:2433ddae2772 370 }
soulx 0:2433ddae2772 371 return 1;//normally
soulx 0:2433ddae2772 372 }
soulx 0:2433ddae2772 373
soulx 0:2433ddae2772 374
soulx 0:2433ddae2772 375 uint8_t position_control(uint8_t id, uint16_t current, uint16_t target)
soulx 0:2433ddae2772 376 {
soulx 0:2433ddae2772 377 //uint8_t state=0;
soulx 0:2433ddae2772 378 int16_t error = target-current;
soulx 0:2433ddae2772 379
soulx 0:2433ddae2772 380 pc.printf("error[%d]=%d\n",id,error);
soulx 0:2433ddae2772 381 if(error > MARGIN) {
soulx 0:2433ddae2772 382 if(limit_motor(id,1)==0 ) { //limit sens
soulx 0:2433ddae2772 383
soulx 0:2433ddae2772 384 pc.printf("motor[%d]=limit error\n",id);
soulx 0:2433ddae2772 385
soulx 0:2433ddae2772 386 return 2;
soulx 0:2433ddae2772 387 }
soulx 0:2433ddae2772 388
soulx 0:2433ddae2772 389
soulx 0:2433ddae2772 390 } else if(error < -MARGIN) {
soulx 0:2433ddae2772 391 if(limit_motor(id,2)==0 ) { //limit sens
soulx 0:2433ddae2772 392
soulx 0:2433ddae2772 393 pc.printf("motor[%d]=limit error\n",id);
soulx 0:2433ddae2772 394
soulx 0:2433ddae2772 395 return 2;
soulx 0:2433ddae2772 396 }
soulx 0:2433ddae2772 397
soulx 0:2433ddae2772 398 } else { //in zone
soulx 0:2433ddae2772 399 motor_stop(2);
soulx 0:2433ddae2772 400
soulx 0:2433ddae2772 401 pc.printf("motor[%d]=complete\n",id);
soulx 0:2433ddae2772 402
soulx 0:2433ddae2772 403 return 1; //in zone complete
soulx 0:2433ddae2772 404 }
soulx 0:2433ddae2772 405
soulx 0:2433ddae2772 406 pc.printf("motor[%d]=in process\n",id);
soulx 0:2433ddae2772 407 return 0; //in process
soulx 0:2433ddae2772 408 }
soulx 0:2433ddae2772 409
soulx 0:2433ddae2772 410 void calibration(uint8_t id)
soulx 0:2433ddae2772 411 {
soulx 0:2433ddae2772 412 switch(id) {
soulx 0:2433ddae2772 413 case 1:
soulx 0:2433ddae2772 414 pc.printf("motor[1] run up\n");
soulx 0:2433ddae2772 415 while(sw_LU_U) {
soulx 0:2433ddae2772 416 motor_set(id,1);
soulx 0:2433ddae2772 417 }
soulx 0:2433ddae2772 418 pc.printf("motor[1] stop up\n");
soulx 0:2433ddae2772 419 //wait_ms(500);
soulx 0:2433ddae2772 420 do {
soulx 0:2433ddae2772 421 motor_set(id,2);
soulx 0:2433ddae2772 422 } while(sw_LU_U ==0);
soulx 0:2433ddae2772 423 motor_stop(id);
soulx 0:2433ddae2772 424 wait_ms(500);
soulx 0:2433ddae2772 425 pc.printf("motor[1] read position\n");
soulx 0:2433ddae2772 426 max_pos_LU = position_LU.read_u16();
soulx 0:2433ddae2772 427 pc.printf("max_pos_LU= %d\n",max_pos_LU);
soulx 0:2433ddae2772 428
soulx 0:2433ddae2772 429 pc.printf("motor[1] run down\n");
soulx 0:2433ddae2772 430 while(sw_LU_D) {
soulx 0:2433ddae2772 431 motor_set(id,2);
soulx 0:2433ddae2772 432 }
soulx 0:2433ddae2772 433 pc.printf("motor[1] stop down\n");
soulx 0:2433ddae2772 434 do {
soulx 0:2433ddae2772 435 motor_set(id,1);
soulx 0:2433ddae2772 436 } while(sw_LU_D ==0);
soulx 0:2433ddae2772 437 motor_stop(id);
soulx 0:2433ddae2772 438 wait_ms(500);
soulx 0:2433ddae2772 439 pc.printf("motor[1] read position\n");
soulx 0:2433ddae2772 440 min_pos_LU = position_LU.read_u16();
soulx 0:2433ddae2772 441 pc.printf("min_pos_LU= %d\n",min_pos_LU);
soulx 0:2433ddae2772 442 break;
soulx 0:2433ddae2772 443
soulx 0:2433ddae2772 444 case 2:
soulx 0:2433ddae2772 445
soulx 0:2433ddae2772 446 pc.printf("motor[2] run up\n");
soulx 0:2433ddae2772 447 while(sw_LL_U) {
soulx 0:2433ddae2772 448 motor_set(id,1);
soulx 0:2433ddae2772 449 }
soulx 0:2433ddae2772 450 motor_stop(id);
soulx 0:2433ddae2772 451 wait_ms(500);
soulx 0:2433ddae2772 452 pc.printf("motor[2] stop up\n");
soulx 0:2433ddae2772 453 do {
soulx 0:2433ddae2772 454 motor_set(id,2);
soulx 0:2433ddae2772 455 } while(sw_LL_U == 0);
soulx 0:2433ddae2772 456 motor_stop(id);
soulx 0:2433ddae2772 457 wait_ms(500);
soulx 0:2433ddae2772 458 max_pos_LL = position_LL.read_u16();
soulx 0:2433ddae2772 459 pc.printf("max_pos_LL= %d\n",max_pos_LL);
soulx 0:2433ddae2772 460 pc.printf("motor[2] run down\n");
soulx 0:2433ddae2772 461 while(sw_LL_D) {
soulx 0:2433ddae2772 462 motor_set(id,2);
soulx 0:2433ddae2772 463 }
soulx 0:2433ddae2772 464 motor_stop(id);
soulx 0:2433ddae2772 465 wait_ms(500);
soulx 0:2433ddae2772 466 pc.printf("motor[2] stop down\n");
soulx 0:2433ddae2772 467 do {
soulx 0:2433ddae2772 468 motor_set(id,1);
soulx 0:2433ddae2772 469 } while(sw_LL_D == 0);
soulx 0:2433ddae2772 470 motor_stop(id);
soulx 0:2433ddae2772 471 wait_ms(500);
soulx 0:2433ddae2772 472 min_pos_LL = position_LL.read_u16();
soulx 0:2433ddae2772 473 pc.printf("min_pos_LL= %d\n",min_pos_LL);
soulx 0:2433ddae2772 474 break;
soulx 0:2433ddae2772 475
soulx 0:2433ddae2772 476 case 3:
soulx 0:2433ddae2772 477 uint8_t count=1000;
soulx 0:2433ddae2772 478 pc.printf("motor[3] run up\n");
soulx 0:2433ddae2772 479 while(sw_RU_U) {
soulx 0:2433ddae2772 480 motor_set(id,1);
soulx 0:2433ddae2772 481 }
soulx 0:2433ddae2772 482 motor_stop(id);
soulx 0:2433ddae2772 483 wait_ms(500);
soulx 0:2433ddae2772 484 pc.printf("motor[3] stop up\n");
soulx 1:768d359e9d96 485
soulx 0:2433ddae2772 486 do {
soulx 0:2433ddae2772 487 motor_set(id,2);
soulx 0:2433ddae2772 488
soulx 0:2433ddae2772 489 } while(sw_RU_U ==0);
soulx 0:2433ddae2772 490 motor_stop(id);
soulx 0:2433ddae2772 491 wait_ms(500);
soulx 0:2433ddae2772 492 max_pos_RU = position_RU.read_u16();
soulx 0:2433ddae2772 493 pc.printf("max_pos_RU= %d\n",max_pos_RU);
soulx 0:2433ddae2772 494
soulx 0:2433ddae2772 495 pc.printf("motor[3] run down\n");
soulx 1:768d359e9d96 496
soulx 0:2433ddae2772 497 /*while(count >1) {
soulx 0:2433ddae2772 498 motor_set(id,2);
soulx 0:2433ddae2772 499 if(sw_RU_D)
soulx 0:2433ddae2772 500 {
soulx 0:2433ddae2772 501 count--;
soulx 0:2433ddae2772 502 }
soulx 0:2433ddae2772 503 }*/
soulx 1:768d359e9d96 504 while(sw_RU_D) {
soulx 1:768d359e9d96 505
soulx 0:2433ddae2772 506 motor_set(id,2);
soulx 1:768d359e9d96 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 1:768d359e9d96 553 }
soulx 1:768d359e9d96 554 /*
soulx 1:768d359e9d96 555 uint16_t convert(uint16_t data)
soulx 1:768d359e9d96 556 {
soulx 1:768d359e9d96 557 uint16_t ans=0;
soulx 1:768d359e9d96 558
soulx 1:768d359e9d96 559 //ans =
soulx 1:768d359e9d96 560
soulx 1:768d359e9d96 561 return
soulx 1:768d359e9d96 562 }
soulx 1:768d359e9d96 563
soulx 1:768d359e9d96 564 uint16_t scale(uint16_t data)
soulx 1:768d359e9d96 565 {
soulx 1:768d359e9d96 566
soulx 1:768d359e9d96 567 }
soulx 1:768d359e9d96 568 */