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@5:f07cbb5a86c3, 2015-07-16 (annotated)

Committer:
soulx
Date:
Thu Jul 16 08:19:18 2015 +0000
Revision:
5:f07cbb5a86c3
Parent:
4:a35291005a30
Child:
6:8d80c84e0c09
edit logic input form limit_sw

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 5:f07cbb5a86c3 104 while(1) {
soulx 5:f07cbb5a86c3 105 if(sw_LU_U) {
soulx 5:f07cbb5a86c3 106 pc.printf("sw_LU_U = 1");
soulx 5:f07cbb5a86c3 107 } else {
soulx 5:f07cbb5a86c3 108 pc.printf("sw_LU_U = 0");
soulx 5:f07cbb5a86c3 109 }
soulx 5:f07cbb5a86c3 110
soulx 5:f07cbb5a86c3 111 if(sw_LU_D) {
soulx 5:f07cbb5a86c3 112 pc.printf(" sw_LU_D = 1");
soulx 5:f07cbb5a86c3 113 } else {
soulx 5:f07cbb5a86c3 114 pc.printf(" sw_LU_D = 0");
soulx 5:f07cbb5a86c3 115 }
soulx 5:f07cbb5a86c3 116 pc.printf("\n");
soulx 5:f07cbb5a86c3 117 ///////////////////////////////////
soulx 5:f07cbb5a86c3 118 if(sw_LL_U) {
soulx 5:f07cbb5a86c3 119 pc.printf("sw_LL_U = 1");
soulx 5:f07cbb5a86c3 120 } else {
soulx 5:f07cbb5a86c3 121 pc.printf("sw_LL_U = 0");
soulx 5:f07cbb5a86c3 122 }
soulx 5:f07cbb5a86c3 123
soulx 5:f07cbb5a86c3 124 if(sw_LL_D) {
soulx 5:f07cbb5a86c3 125 pc.printf(" sw_LL_D = 1");
soulx 5:f07cbb5a86c3 126 } else {
soulx 5:f07cbb5a86c3 127 pc.printf(" sw_LL_D = 0");
soulx 5:f07cbb5a86c3 128 }
soulx 5:f07cbb5a86c3 129 pc.printf("\n");
soulx 5:f07cbb5a86c3 130 //////////////////////////////////////
soulx 5:f07cbb5a86c3 131 ///////////////////////////////////
soulx 5:f07cbb5a86c3 132
soulx 5:f07cbb5a86c3 133 /////////////////////
soulx 5:f07cbb5a86c3 134 if(sw_RU_U) {
soulx 5:f07cbb5a86c3 135 pc.printf("sw_RU_U = 1");
soulx 5:f07cbb5a86c3 136 } else {
soulx 5:f07cbb5a86c3 137 pc.printf("sw_RU_U = 0");
soulx 5:f07cbb5a86c3 138 }
soulx 5:f07cbb5a86c3 139
soulx 5:f07cbb5a86c3 140 if(sw_RU_D) {
soulx 5:f07cbb5a86c3 141 pc.printf(" sw_RU_D = 1");
soulx 5:f07cbb5a86c3 142 } else {
soulx 5:f07cbb5a86c3 143 pc.printf(" sw_RU_D = 0");
soulx 5:f07cbb5a86c3 144 }
soulx 5:f07cbb5a86c3 145 pc.printf("\n");
soulx 5:f07cbb5a86c3 146 ///////////////////////////////////
soulx 5:f07cbb5a86c3 147 if(sw_RL_U) {
soulx 5:f07cbb5a86c3 148 pc.printf("sw_RL_U = 1");
soulx 5:f07cbb5a86c3 149 } else {
soulx 5:f07cbb5a86c3 150 pc.printf("sw_RL_U = 0");
soulx 5:f07cbb5a86c3 151 }
soulx 5:f07cbb5a86c3 152
soulx 5:f07cbb5a86c3 153 if(sw_RL_D) {
soulx 5:f07cbb5a86c3 154 pc.printf(" sw_RL_D = 1");
soulx 5:f07cbb5a86c3 155 } else {
soulx 5:f07cbb5a86c3 156 pc.printf(" sw_RL_D = 0");
soulx 5:f07cbb5a86c3 157 }
soulx 5:f07cbb5a86c3 158 //////////////////////////////////////
soulx 5:f07cbb5a86c3 159 pc.printf("\n");
soulx 5:f07cbb5a86c3 160 pc.printf("\n");
soulx 5:f07cbb5a86c3 161 wait(1);
soulx 5:f07cbb5a86c3 162 }
soulx 5:f07cbb5a86c3 163 */
soulx 5:f07cbb5a86c3 164
soulx 5:f07cbb5a86c3 165 /*
soulx 0:2433ddae2772 166 while(1) {
soulx 0:2433ddae2772 167 myled =1;
soulx 0:2433ddae2772 168 wait_ms(200);
soulx 0:2433ddae2772 169
soulx 0:2433ddae2772 170 if(mybutton == 0) {
soulx 0:2433ddae2772 171 myled =0;
soulx 0:2433ddae2772 172 wait_ms(200);
soulx 0:2433ddae2772 173 }
soulx 0:2433ddae2772 174
soulx 0:2433ddae2772 175 if(sw_LU_U == 0) {
soulx 0:2433ddae2772 176 myled =0;
soulx 0:2433ddae2772 177 wait_ms(200);
soulx 0:2433ddae2772 178 }
soulx 0:2433ddae2772 179
soulx 0:2433ddae2772 180 if(sw_LU_D == 0) {
soulx 0:2433ddae2772 181 myled =0;
soulx 0:2433ddae2772 182 wait_ms(200);
soulx 0:2433ddae2772 183 }
soulx 0:2433ddae2772 184
soulx 0:2433ddae2772 185 if(sw_LL_U == 0) {
soulx 0:2433ddae2772 186 myled =0;
soulx 0:2433ddae2772 187 wait_ms(200);
soulx 0:2433ddae2772 188 }
soulx 0:2433ddae2772 189
soulx 0:2433ddae2772 190 if(sw_LL_D == 0) {
soulx 0:2433ddae2772 191 myled =0;
soulx 0:2433ddae2772 192 wait_ms(200);
soulx 0:2433ddae2772 193 }
soulx 0:2433ddae2772 194
soulx 0:2433ddae2772 195 if(sw_RU_U == 0) {
soulx 0:2433ddae2772 196 myled =0;
soulx 0:2433ddae2772 197 wait_ms(200);
soulx 0:2433ddae2772 198 }
soulx 0:2433ddae2772 199
soulx 0:2433ddae2772 200 if(sw_RU_D == 0) {
soulx 0:2433ddae2772 201 myled =0;
soulx 0:2433ddae2772 202 wait_ms(200);
soulx 0:2433ddae2772 203 }
soulx 0:2433ddae2772 204
soulx 0:2433ddae2772 205 if(sw_RL_U == 0) {
soulx 0:2433ddae2772 206 myled =0;
soulx 0:2433ddae2772 207 wait_ms(200);
soulx 0:2433ddae2772 208 }
soulx 0:2433ddae2772 209
soulx 0:2433ddae2772 210 if(sw_RL_D == 0) {
soulx 0:2433ddae2772 211 myled =0;
soulx 0:2433ddae2772 212 wait_ms(200);
soulx 0:2433ddae2772 213 }
soulx 0:2433ddae2772 214
soulx 0:2433ddae2772 215 }
soulx 0:2433ddae2772 216 */
soulx 0:2433ddae2772 217
soulx 5:f07cbb5a86c3 218 while(1) {
soulx 1:768d359e9d96 219 //calibration
soulx 1:768d359e9d96 220 pc.printf("Welcome to DOGWHEELSCHAIR\n");
soulx 1:768d359e9d96 221 pc.printf("Calibration [START]\n");
soulx 1:768d359e9d96 222 calibration(1);
soulx 1:768d359e9d96 223 calibration(2);
soulx 1:768d359e9d96 224 calibration(3);
soulx 1:768d359e9d96 225 calibration(4);
soulx 1:768d359e9d96 226 pc.printf("Calibration [FINISH]\n");
soulx 1:768d359e9d96 227 pc.printf("RUN mode [START]\n");
soulx 5:f07cbb5a86c3 228 }
soulx 0:2433ddae2772 229
soulx 0:2433ddae2772 230
soulx 0:2433ddae2772 231 uint8_t count=0;
soulx 0:2433ddae2772 232
soulx 0:2433ddae2772 233 while(1) {
soulx 0:2433ddae2772 234 uint8_t state=0;
soulx 0:2433ddae2772 235 pc.printf("Count %d",count);
soulx 0:2433ddae2772 236 do {
soulx 0:2433ddae2772 237 state=0;
soulx 0:2433ddae2772 238
soulx 1:768d359e9d96 239 if(position_control(1,position_LU.read_u16(),max_pos_LU-offset_pos) == 1) {
soulx 0:2433ddae2772 240 state++;
soulx 0:2433ddae2772 241 }
soulx 0:2433ddae2772 242
soulx 1:768d359e9d96 243 if(position_control(2,position_LL.read_u16(),max_pos_LL-offset_pos) == 1) {
soulx 0:2433ddae2772 244 state++;
soulx 0:2433ddae2772 245 }
soulx 0:2433ddae2772 246
soulx 1:768d359e9d96 247 if(position_control(3,position_RU.read_u16(),max_pos_RU-offset_pos) == 1) {
soulx 0:2433ddae2772 248 state++;
soulx 0:2433ddae2772 249 }
soulx 0:2433ddae2772 250
soulx 1:768d359e9d96 251 if(position_control(4,position_RL.read_u16(),max_pos_RL-offset_pos) == 1) {
soulx 0:2433ddae2772 252 state++;
soulx 0:2433ddae2772 253 }
soulx 0:2433ddae2772 254
soulx 0:2433ddae2772 255 pc.printf("state = %d",state);
soulx 0:2433ddae2772 256 } while(state <= 4 );
soulx 0:2433ddae2772 257
soulx 0:2433ddae2772 258 do {
soulx 0:2433ddae2772 259 state=0;
soulx 0:2433ddae2772 260
soulx 1:768d359e9d96 261 if(position_control(1,position_LU.read_u16(),min_pos_LU+offset_pos) == 1) {
soulx 0:2433ddae2772 262 state++;
soulx 0:2433ddae2772 263 }
soulx 0:2433ddae2772 264
soulx 1:768d359e9d96 265 if(position_control(2,position_LL.read_u16(),min_pos_LL+offset_pos) == 1) {
soulx 0:2433ddae2772 266 state++;
soulx 0:2433ddae2772 267 }
soulx 0:2433ddae2772 268
soulx 1:768d359e9d96 269 if(position_control(3,position_RU.read_u16(),min_pos_RU+offset_pos) == 1) {
soulx 0:2433ddae2772 270 state++;
soulx 0:2433ddae2772 271 }
soulx 0:2433ddae2772 272
soulx 1:768d359e9d96 273 if(position_control(4,position_RL.read_u16(),min_pos_RL+offset_pos) == 1) {
soulx 0:2433ddae2772 274 state++;
soulx 0:2433ddae2772 275 }
soulx 0:2433ddae2772 276
soulx 0:2433ddae2772 277 pc.printf("state = %d",state);
soulx 0:2433ddae2772 278 } while(state <= 4 );
soulx 0:2433ddae2772 279 count++;
soulx 0:2433ddae2772 280 }
soulx 0:2433ddae2772 281 }
soulx 0:2433ddae2772 282
soulx 0:2433ddae2772 283
soulx 0:2433ddae2772 284
soulx 0:2433ddae2772 285 void motor_set(uint8_t id, uint8_t direct)
soulx 0:2433ddae2772 286 {
soulx 0:2433ddae2772 287 //direct: Should be between 0 and 3, with the following result
soulx 0:2433ddae2772 288 //0: Brake to VCC
soulx 0:2433ddae2772 289 //1: Clockwise
soulx 0:2433ddae2772 290 //2: CounterClockwise
soulx 0:2433ddae2772 291 //3: Brake to GND
soulx 0:2433ddae2772 292
soulx 0:2433ddae2772 293 if(direct <=4) {
soulx 0:2433ddae2772 294 switch(id) {
soulx 0:2433ddae2772 295 case 1:
soulx 0:2433ddae2772 296 // Set inA[motor]
soulx 0:2433ddae2772 297 if (direct <=1)
soulx 0:2433ddae2772 298 dirA_LU=0;
soulx 0:2433ddae2772 299 else
soulx 0:2433ddae2772 300 dirA_LU=1;
soulx 0:2433ddae2772 301
soulx 0:2433ddae2772 302 // Set inB[motor]
soulx 0:2433ddae2772 303 if ((direct==0)||(direct==2))
soulx 0:2433ddae2772 304 dirB_LU=0;
soulx 0:2433ddae2772 305 else
soulx 0:2433ddae2772 306 dirB_LU=1;
soulx 0:2433ddae2772 307 break;
soulx 0:2433ddae2772 308
soulx 0:2433ddae2772 309 case 2:
soulx 0:2433ddae2772 310 // Set inA[motor]
soulx 0:2433ddae2772 311 if (direct <=1)
soulx 0:2433ddae2772 312 dirA_LL=0;
soulx 0:2433ddae2772 313 else
soulx 0:2433ddae2772 314 dirA_LL=1;
soulx 0:2433ddae2772 315
soulx 0:2433ddae2772 316 // Set inB[motor]
soulx 0:2433ddae2772 317 if ((direct==0)||(direct==2))
soulx 0:2433ddae2772 318 dirB_LL=0;
soulx 0:2433ddae2772 319 else
soulx 0:2433ddae2772 320 dirB_LL=1;
soulx 0:2433ddae2772 321 break;
soulx 0:2433ddae2772 322
soulx 0:2433ddae2772 323 case 3:
soulx 0:2433ddae2772 324 // Set inA[motor]
soulx 0:2433ddae2772 325 if (direct <=1)
soulx 0:2433ddae2772 326 dirA_RU=0;
soulx 0:2433ddae2772 327 else
soulx 0:2433ddae2772 328 dirA_RU=1;
soulx 0:2433ddae2772 329
soulx 0:2433ddae2772 330 // Set inB[motor]
soulx 0:2433ddae2772 331 if ((direct==0)||(direct==2))
soulx 0:2433ddae2772 332 dirB_RU=0;
soulx 0:2433ddae2772 333 else
soulx 0:2433ddae2772 334 dirB_RU=1;
soulx 0:2433ddae2772 335 break;
soulx 0:2433ddae2772 336
soulx 0:2433ddae2772 337 case 4:
soulx 0:2433ddae2772 338 // Set inA[motor]
soulx 0:2433ddae2772 339 if (direct <=1)
soulx 0:2433ddae2772 340 dirA_RL=0;
soulx 0:2433ddae2772 341 else
soulx 0:2433ddae2772 342 dirA_RL=1;
soulx 0:2433ddae2772 343 // Set inB[motor]
soulx 0:2433ddae2772 344 if ((direct==0)||(direct==2))
soulx 0:2433ddae2772 345 dirB_RL=0;
soulx 0:2433ddae2772 346 else
soulx 0:2433ddae2772 347 dirB_RL=1;
soulx 0:2433ddae2772 348 break;
soulx 0:2433ddae2772 349 }
soulx 0:2433ddae2772 350 }
soulx 0:2433ddae2772 351 }
soulx 0:2433ddae2772 352
soulx 0:2433ddae2772 353 void motor_stop(uint8_t id)
soulx 0:2433ddae2772 354 {
soulx 0:2433ddae2772 355 switch(id) {
soulx 0:2433ddae2772 356 case 1:
soulx 0:2433ddae2772 357 dirA_LU=1;
soulx 0:2433ddae2772 358 dirB_LU=1;
soulx 0:2433ddae2772 359 break;
soulx 0:2433ddae2772 360
soulx 0:2433ddae2772 361 case 2:
soulx 0:2433ddae2772 362 dirA_LL=1;
soulx 0:2433ddae2772 363 dirB_LL=1;
soulx 0:2433ddae2772 364 break;
soulx 0:2433ddae2772 365
soulx 0:2433ddae2772 366 case 3:
soulx 0:2433ddae2772 367 dirA_RU=1;
soulx 0:2433ddae2772 368 dirB_RU=1;
soulx 0:2433ddae2772 369 break;
soulx 0:2433ddae2772 370
soulx 0:2433ddae2772 371 case 4:
soulx 0:2433ddae2772 372 dirA_RL=1;
soulx 0:2433ddae2772 373 dirB_RL=1;
soulx 0:2433ddae2772 374 break;
soulx 0:2433ddae2772 375
soulx 0:2433ddae2772 376 case 0:
soulx 0:2433ddae2772 377 dirA_LU=1;
soulx 0:2433ddae2772 378 dirB_LU=1;
soulx 0:2433ddae2772 379
soulx 0:2433ddae2772 380 dirA_LL=1;
soulx 0:2433ddae2772 381 dirB_LL=1;
soulx 0:2433ddae2772 382
soulx 0:2433ddae2772 383 dirA_RU=1;
soulx 0:2433ddae2772 384 dirB_RU=1;
soulx 0:2433ddae2772 385
soulx 0:2433ddae2772 386 dirA_RL=1;
soulx 0:2433ddae2772 387 dirB_RL=1;
soulx 0:2433ddae2772 388 break;
soulx 0:2433ddae2772 389 }
soulx 0:2433ddae2772 390 }
soulx 0:2433ddae2772 391
soulx 0:2433ddae2772 392
soulx 0:2433ddae2772 393
soulx 0:2433ddae2772 394 uint8_t limit_motor(uint8_t id, uint8_t dirction)
soulx 0:2433ddae2772 395 {
soulx 0:2433ddae2772 396 switch(id) {
soulx 0:2433ddae2772 397 case 1://Left Upper
soulx 5:f07cbb5a86c3 398 if(~sw_LU_U && ~sw_LU_D) {
soulx 0:2433ddae2772 399 motor_set(id,dirction);
soulx 0:2433ddae2772 400 } else {
soulx 0:2433ddae2772 401 motor_stop(id);
soulx 0:2433ddae2772 402 return 0;
soulx 0:2433ddae2772 403 }
soulx 0:2433ddae2772 404 break;
soulx 0:2433ddae2772 405
soulx 0:2433ddae2772 406 case 2://Left Lowwer
soulx 5:f07cbb5a86c3 407 if(~sw_LL_U && ~sw_LL_D) {
soulx 0:2433ddae2772 408 motor_set(id,dirction);
soulx 0:2433ddae2772 409 } else {
soulx 0:2433ddae2772 410 motor_stop(id);
soulx 0:2433ddae2772 411 return 0;
soulx 0:2433ddae2772 412 }
soulx 0:2433ddae2772 413 break;
soulx 0:2433ddae2772 414
soulx 0:2433ddae2772 415 case 3://Right Upper
soulx 5:f07cbb5a86c3 416 if(~sw_RU_U && ~sw_RU_D) {
soulx 0:2433ddae2772 417 motor_set(id,dirction);
soulx 0:2433ddae2772 418 } else {
soulx 0:2433ddae2772 419 motor_stop(id);
soulx 0:2433ddae2772 420 return 0;
soulx 0:2433ddae2772 421 }
soulx 0:2433ddae2772 422 break;
soulx 0:2433ddae2772 423
soulx 0:2433ddae2772 424 case 4://Right Lowwer
soulx 5:f07cbb5a86c3 425 if(~sw_RL_U && ~sw_RL_D) {
soulx 0:2433ddae2772 426 motor_set(id,dirction);
soulx 0:2433ddae2772 427 } else {
soulx 0:2433ddae2772 428 motor_stop(id);
soulx 0:2433ddae2772 429 return 0;
soulx 0:2433ddae2772 430 }
soulx 0:2433ddae2772 431 break;
soulx 0:2433ddae2772 432 }
soulx 0:2433ddae2772 433 return 1;//normally
soulx 0:2433ddae2772 434 }
soulx 0:2433ddae2772 435
soulx 0:2433ddae2772 436
soulx 0:2433ddae2772 437 uint8_t position_control(uint8_t id, uint16_t current, uint16_t target)
soulx 0:2433ddae2772 438 {
soulx 0:2433ddae2772 439 //uint8_t state=0;
soulx 0:2433ddae2772 440 int16_t error = target-current;
soulx 0:2433ddae2772 441
soulx 0:2433ddae2772 442 pc.printf("error[%d]=%d\n",id,error);
soulx 0:2433ddae2772 443 if(error > MARGIN) {
soulx 0:2433ddae2772 444 if(limit_motor(id,1)==0 ) { //limit sens
soulx 0:2433ddae2772 445
soulx 0:2433ddae2772 446 pc.printf("motor[%d]=limit error\n",id);
soulx 0:2433ddae2772 447
soulx 0:2433ddae2772 448 return 2;
soulx 0:2433ddae2772 449 }
soulx 0:2433ddae2772 450
soulx 0:2433ddae2772 451
soulx 0:2433ddae2772 452 } else if(error < -MARGIN) {
soulx 0:2433ddae2772 453 if(limit_motor(id,2)==0 ) { //limit sens
soulx 0:2433ddae2772 454
soulx 0:2433ddae2772 455 pc.printf("motor[%d]=limit error\n",id);
soulx 0:2433ddae2772 456
soulx 0:2433ddae2772 457 return 2;
soulx 0:2433ddae2772 458 }
soulx 0:2433ddae2772 459
soulx 0:2433ddae2772 460 } else { //in zone
soulx 0:2433ddae2772 461 motor_stop(2);
soulx 0:2433ddae2772 462
soulx 0:2433ddae2772 463 pc.printf("motor[%d]=complete\n",id);
soulx 0:2433ddae2772 464
soulx 0:2433ddae2772 465 return 1; //in zone complete
soulx 0:2433ddae2772 466 }
soulx 0:2433ddae2772 467
soulx 0:2433ddae2772 468 pc.printf("motor[%d]=in process\n",id);
soulx 0:2433ddae2772 469 return 0; //in process
soulx 0:2433ddae2772 470 }
soulx 0:2433ddae2772 471
soulx 0:2433ddae2772 472 void calibration(uint8_t id)
soulx 0:2433ddae2772 473 {
soulx 0:2433ddae2772 474 switch(id) {
soulx 0:2433ddae2772 475 case 1:
soulx 0:2433ddae2772 476 pc.printf("motor[1] run up\n");
soulx 5:f07cbb5a86c3 477 do {
soulx 0:2433ddae2772 478 motor_set(id,1);
soulx 5:f07cbb5a86c3 479 } while(sw_LU_U ==0);
soulx 0:2433ddae2772 480 pc.printf("motor[1] stop up\n");
soulx 0:2433ddae2772 481 //wait_ms(500);
soulx 0:2433ddae2772 482 do {
soulx 0:2433ddae2772 483 motor_set(id,2);
soulx 5:f07cbb5a86c3 484 } while(sw_LU_U);
soulx 0:2433ddae2772 485 motor_stop(id);
soulx 0:2433ddae2772 486 wait_ms(500);
soulx 0:2433ddae2772 487 pc.printf("motor[1] read position\n");
soulx 0:2433ddae2772 488 max_pos_LU = position_LU.read_u16();
soulx 0:2433ddae2772 489 pc.printf("max_pos_LU= %d\n",max_pos_LU);
soulx 0:2433ddae2772 490
soulx 0:2433ddae2772 491 pc.printf("motor[1] run down\n");
soulx 5:f07cbb5a86c3 492 do {
soulx 0:2433ddae2772 493 motor_set(id,2);
soulx 5:f07cbb5a86c3 494 } while(sw_LU_D==0) ;
soulx 0:2433ddae2772 495 pc.printf("motor[1] stop down\n");
soulx 0:2433ddae2772 496 do {
soulx 0:2433ddae2772 497 motor_set(id,1);
soulx 5:f07cbb5a86c3 498 } while(sw_LU_D);
soulx 0:2433ddae2772 499 motor_stop(id);
soulx 0:2433ddae2772 500 wait_ms(500);
soulx 0:2433ddae2772 501 pc.printf("motor[1] read position\n");
soulx 0:2433ddae2772 502 min_pos_LU = position_LU.read_u16();
soulx 0:2433ddae2772 503 pc.printf("min_pos_LU= %d\n",min_pos_LU);
soulx 0:2433ddae2772 504 break;
soulx 0:2433ddae2772 505
soulx 0:2433ddae2772 506 case 2:
soulx 0:2433ddae2772 507
soulx 0:2433ddae2772 508 pc.printf("motor[2] run up\n");
soulx 5:f07cbb5a86c3 509 do{
soulx 0:2433ddae2772 510 motor_set(id,1);
soulx 5:f07cbb5a86c3 511 }while(sw_LL_U==0) ;
soulx 0:2433ddae2772 512 motor_stop(id);
soulx 0:2433ddae2772 513 wait_ms(500);
soulx 0:2433ddae2772 514 pc.printf("motor[2] stop up\n");
soulx 0:2433ddae2772 515 do {
soulx 0:2433ddae2772 516 motor_set(id,2);
soulx 5:f07cbb5a86c3 517 } while(sw_LL_U );
soulx 0:2433ddae2772 518 motor_stop(id);
soulx 0:2433ddae2772 519 wait_ms(500);
soulx 0:2433ddae2772 520 max_pos_LL = position_LL.read_u16();
soulx 0:2433ddae2772 521 pc.printf("max_pos_LL= %d\n",max_pos_LL);
soulx 0:2433ddae2772 522 pc.printf("motor[2] run down\n");
soulx 5:f07cbb5a86c3 523 do{
soulx 0:2433ddae2772 524 motor_set(id,2);
soulx 5:f07cbb5a86c3 525 }while(sw_LL_D==0) ;
soulx 0:2433ddae2772 526 motor_stop(id);
soulx 0:2433ddae2772 527 wait_ms(500);
soulx 0:2433ddae2772 528 pc.printf("motor[2] stop down\n");
soulx 0:2433ddae2772 529 do {
soulx 0:2433ddae2772 530 motor_set(id,1);
soulx 5:f07cbb5a86c3 531 } while(sw_LL_D);
soulx 0:2433ddae2772 532 motor_stop(id);
soulx 0:2433ddae2772 533 wait_ms(500);
soulx 0:2433ddae2772 534 min_pos_LL = position_LL.read_u16();
soulx 0:2433ddae2772 535 pc.printf("min_pos_LL= %d\n",min_pos_LL);
soulx 0:2433ddae2772 536 break;
soulx 0:2433ddae2772 537
soulx 0:2433ddae2772 538 case 3:
soulx 5:f07cbb5a86c3 539 // uint8_t count=1000;
soulx 0:2433ddae2772 540 pc.printf("motor[3] run up\n");
soulx 5:f07cbb5a86c3 541 do {
soulx 0:2433ddae2772 542 motor_set(id,1);
soulx 5:f07cbb5a86c3 543 }while(sw_RU_U==0);
soulx 0:2433ddae2772 544 motor_stop(id);
soulx 0:2433ddae2772 545 wait_ms(500);
soulx 0:2433ddae2772 546 pc.printf("motor[3] stop up\n");
soulx 1:768d359e9d96 547
soulx 0:2433ddae2772 548 do {
soulx 0:2433ddae2772 549 motor_set(id,2);
soulx 0:2433ddae2772 550
soulx 5:f07cbb5a86c3 551 } while(sw_RU_U);
soulx 0:2433ddae2772 552 motor_stop(id);
soulx 0:2433ddae2772 553 wait_ms(500);
soulx 0:2433ddae2772 554 max_pos_RU = position_RU.read_u16();
soulx 0:2433ddae2772 555 pc.printf("max_pos_RU= %d\n",max_pos_RU);
soulx 0:2433ddae2772 556
soulx 0:2433ddae2772 557 pc.printf("motor[3] run down\n");
soulx 1:768d359e9d96 558
soulx 0:2433ddae2772 559 /*while(count >1) {
soulx 0:2433ddae2772 560 motor_set(id,2);
soulx 0:2433ddae2772 561 if(sw_RU_D)
soulx 0:2433ddae2772 562 {
soulx 0:2433ddae2772 563 count--;
soulx 0:2433ddae2772 564 }
soulx 0:2433ddae2772 565 }*/
soulx 5:f07cbb5a86c3 566 do {
soulx 1:768d359e9d96 567
soulx 0:2433ddae2772 568 motor_set(id,2);
soulx 5:f07cbb5a86c3 569 }while(sw_RU_D==0);
soulx 0:2433ddae2772 570 motor_stop(id);
soulx 0:2433ddae2772 571 wait_ms(500);
soulx 0:2433ddae2772 572 pc.printf("motor[3] stop down\n");
soulx 0:2433ddae2772 573 do {
soulx 0:2433ddae2772 574 motor_set(id,1);
soulx 5:f07cbb5a86c3 575 } while(sw_RU_D);
soulx 0:2433ddae2772 576 motor_stop(id);
soulx 0:2433ddae2772 577 wait_ms(500);
soulx 0:2433ddae2772 578 min_pos_RU = position_RU.read_u16();
soulx 0:2433ddae2772 579 pc.printf("min_pos_RU= %d\n",min_pos_RU);
soulx 0:2433ddae2772 580 break;
soulx 0:2433ddae2772 581
soulx 0:2433ddae2772 582 case 4:
soulx 0:2433ddae2772 583
soulx 0:2433ddae2772 584 pc.printf("motor[4] run up\n");
soulx 5:f07cbb5a86c3 585 do {
soulx 0:2433ddae2772 586 motor_set(id,1);
soulx 5:f07cbb5a86c3 587 }while(sw_RL_U==0);
soulx 0:2433ddae2772 588 motor_stop(id);
soulx 0:2433ddae2772 589 wait_ms(500);
soulx 0:2433ddae2772 590 pc.printf("motor[4] stop up\n");
soulx 0:2433ddae2772 591 do {
soulx 0:2433ddae2772 592 motor_set(id,2);
soulx 5:f07cbb5a86c3 593 } while(sw_RL_U);
soulx 0:2433ddae2772 594 motor_stop(id);
soulx 0:2433ddae2772 595 wait_ms(500);
soulx 0:2433ddae2772 596 max_pos_RL = position_RL.read_u16();
soulx 0:2433ddae2772 597 pc.printf("max_pos_RL= %d\n",max_pos_RL);
soulx 0:2433ddae2772 598 pc.printf("motor[4] run down\n");
soulx 5:f07cbb5a86c3 599 do{
soulx 0:2433ddae2772 600 motor_set(id,2);
soulx 5:f07cbb5a86c3 601 }while(sw_RL_D==0);
soulx 0:2433ddae2772 602 motor_stop(id);
soulx 0:2433ddae2772 603 wait_ms(500);
soulx 0:2433ddae2772 604 pc.printf("motor[4] stop down\n");
soulx 0:2433ddae2772 605 do {
soulx 0:2433ddae2772 606 motor_set(id,1);
soulx 5:f07cbb5a86c3 607 } while(sw_RL_D);
soulx 0:2433ddae2772 608 motor_stop(id);
soulx 0:2433ddae2772 609 wait_ms(500);
soulx 0:2433ddae2772 610 min_pos_RL = position_RL.read_u16();
soulx 0:2433ddae2772 611 pc.printf("min_pos_RL= %d\n",min_pos_RL);
soulx 0:2433ddae2772 612 break;
soulx 0:2433ddae2772 613 }
soulx 0:2433ddae2772 614
soulx 1:768d359e9d96 615 }
soulx 5:f07cbb5a86c3 616 /*
soulx 1:768d359e9d96 617 uint16_t convert(uint16_t data)
soulx 1:768d359e9d96 618 {
soulx 1:768d359e9d96 619 uint16_t ans=0;
soulx 1:768d359e9d96 620
soulx 1:768d359e9d96 621 //ans =
soulx 1:768d359e9d96 622
soulx 1:768d359e9d96 623 return
soulx 1:768d359e9d96 624 }
soulx 1:768d359e9d96 625
soulx 1:768d359e9d96 626 uint16_t scale(uint16_t data)
soulx 1:768d359e9d96 627 {
soulx 5:f07cbb5a86c3 628
soulx 5:f07cbb5a86c3 629 }
soulx 5:f07cbb5a86c3 630 */