Joseph Bradshaw
Serial interface for controlling robotic arm.
Embed:
(wiki syntax)
Show/hide line numbers
lpc_axis.cpp
00001 // Test program for WSE-PROJ-SBC Scorbot Interface 00002 // J Bradshaw 00003 //lpc_axis.cpp 00004 00005 // 20150731 00006 // 20150827 - Got Ticker Trapeziodal profile command working (moveTrapezoid, muveUpdate) 00007 // 20150924 - Port from the mbed to the LPC1768 processor for the 6 axis robotic arm controller 00008 // 20151218 - Eliminated PCF8574 I2C I/O expander and attached limit switches to P0_22 - P0_17 00009 // pins as external interrupts. 00010 00011 #include "mbed.h" 00012 #include "Axis.h" 00013 #include "stdlib.h" 00014 00015 #include <string> 00016 00017 #define PI 3.14159 00018 #define SP_TOL 100 // SET POINT TOLERANCE is +/- tolerance for set point command 00019 00020 // Assign interrupt function to pin P0_17 (mbed p12) 00021 00022 DigitalOut led1(P1_18); //blue 00023 DigitalOut led2(P1_20); // 00024 DigitalOut led3(P1_21); 00025 00026 Serial pc(P0_2, P0_3); //pc serial interface (USB) 00027 SPI spi(P0_9, P0_8, P0_7); //MOSI, MISO, SCK 00028 00029 DigitalIn limit1_pin(P0_22); 00030 DigitalIn limit2_pin(P0_21); 00031 DigitalIn limit3_pin(P0_20); 00032 DigitalIn limit4_pin(P0_19); 00033 DigitalIn limit5_pin(P0_18); 00034 DigitalIn limit6_pin(P0_17); 00035 00036 InterruptIn limit1_int(P0_22); 00037 InterruptIn limit2_int(P0_21); 00038 InterruptIn limit3_int(P0_20); 00039 InterruptIn limit4_int(P0_19); 00040 InterruptIn limit5_int(P0_18); 00041 InterruptIn limit6_int(P0_17); 00042 00043 int limit1, limit2, limit3, limit4, limit5, limit6; //global limit switch values 00044 float axis1_I,axis2_I,axis3_I,axis4_I,axis5_I,axis6_I; 00045 int streamFlag=0; 00046 Timer t; 00047 //instantiate axis object NAME(spi bus, LS7366_cs, pwm, dir, analog, limitSwitchAddress, TotalEncoderCounts/axis) 00048 Axis axis1(spi, P1_24, P2_5, P1_0, P0_23, &limit1, 25000.0); //base 00049 Axis axis2(spi, P1_25, P2_4, P1_1, P0_24, &limit2, 17500); //shoulder 00050 Axis axis3(spi, P1_26, P2_3, P1_4, P0_25, &limit3, 20500); //elbow 00051 Axis axis4(spi, P1_27, P2_2, P1_8, P0_26, &limit4, 5000); //pitch/roll 00052 Axis axis5(spi, P1_28, P2_1, P1_9, P1_30, &limit5, 5000); //pitch/roll 00053 Axis axis6(spi, P1_29, P2_0, P1_10, P1_31, &limit6, 5400); //grip 00054 00055 Ticker pulse; 00056 Ticker colCheck; 00057 00058 00059 void zero_axis(int axis){ 00060 switch(axis){ 00061 case 1: 00062 axis1.zero(); 00063 break; 00064 00065 case 2: 00066 axis2.zero(); 00067 break; 00068 00069 case 3: 00070 axis3.zero(); 00071 break; 00072 00073 case 4: 00074 axis4.zero(); 00075 break; 00076 00077 case 5: 00078 axis5.zero(); 00079 break; 00080 00081 case 6: 00082 axis6.zero(); 00083 break; 00084 } 00085 } 00086 00087 void zero_all(void){ 00088 for(int i=1;i<=6;i++){ 00089 zero_axis(i); 00090 wait(.005); 00091 } 00092 } 00093 00094 void alive(void){ 00095 led1 = !led1; 00096 if(led1) 00097 pulse.attach(&alive, .2); // the address of the function to be attached (flip) and the interval (2 seconds) 00098 else 00099 pulse.attach(&alive, 1.3); // the address of the function to be attached (flip) and the interval (2 seconds) 00100 } 00101 00102 void collisionCheck(void){ 00103 float stall_I = .3; 00104 00105 axis1_I = axis1.readCurrent(); 00106 axis2_I = axis2.readCurrent(); 00107 axis3_I = axis3.readCurrent(); 00108 axis4_I = axis4.readCurrent(); 00109 axis5_I = axis5.readCurrent(); 00110 axis6_I = axis6.readCurrent(); 00111 // pc.printf("%.3f %.3f %.3f %.3f %.3f %.3f \r\n", axis1_I, axis2_I, axis3_I, axis4_I, axis5_I, axis6_I); 00112 00113 //analog channels 1 and 2 are damaged on initial prototype test bed 00114 if(axis1_I > stall_I){ 00115 pc.printf("Axis 1 collision detected"); 00116 axis1.axisOff(); 00117 axis2.axisOff(); 00118 axis3.axisOff(); 00119 axis4.axisOff(); 00120 axis5.axisOff(); 00121 axis6.axisOff(); 00122 } 00123 if(axis2_I > stall_I){ 00124 pc.printf("Axis 2 collision detected"); 00125 axis1.axisOff(); 00126 axis2.axisOff(); 00127 axis3.axisOff(); 00128 axis4.axisOff(); 00129 axis5.axisOff(); 00130 axis6.axisOff(); 00131 } 00132 if(axis3_I > stall_I){ 00133 pc.printf("Axis 3 collision detected"); 00134 axis1.axisOff(); 00135 axis2.axisOff(); 00136 axis3.axisOff(); 00137 axis4.axisOff(); 00138 axis5.axisOff(); 00139 axis6.axisOff(); 00140 } 00141 if(axis4_I > stall_I){ 00142 pc.printf("Axis 4 collision detected"); 00143 axis1.axisOff(); 00144 axis2.axisOff(); 00145 axis3.axisOff(); 00146 axis4.axisOff(); 00147 axis5.axisOff(); 00148 axis6.axisOff(); 00149 } 00150 if(axis5_I > stall_I){ 00151 pc.printf("Axis 5 collision detected"); 00152 axis1.axisOff(); 00153 axis2.axisOff(); 00154 axis3.axisOff(); 00155 axis4.axisOff(); 00156 axis5.axisOff(); 00157 axis6.axisOff(); 00158 } 00159 if(axis6_I > stall_I){ 00160 pc.printf("Axis 6 collision detected"); 00161 axis1.axisOff(); 00162 axis2.axisOff(); 00163 axis3.axisOff(); 00164 axis4.axisOff(); 00165 axis5.axisOff(); 00166 axis6.axisOff(); 00167 } 00168 } 00169 00170 void home_test(void){ 00171 00172 axis1.zero(); 00173 axis2.zero(); 00174 axis3.zero(); 00175 00176 axis1.pid->setInputLimits(-30000, 30000); 00177 axis2.pid->setInputLimits(-30000, 30000); 00178 axis3.pid->setInputLimits(-30000, 30000); 00179 00180 for(float delta=0.0;delta<200.0 && (*axis2.ptr_limit == 1) && (*axis3.ptr_limit == 1);delta+=100){ 00181 axis3.set_point = delta; 00182 axis4.set_point = .23 * delta; 00183 axis5.set_point = .23 * -delta; 00184 wait(.5); 00185 } 00186 zero_axis(3); 00187 00188 for(float delta=0.0;delta>-13000.0 && (*axis3.ptr_limit == 1);delta-=100){ 00189 axis3.set_point = delta; 00190 axis4.set_point = .249 * delta; 00191 axis5.set_point = .249 * -delta; 00192 wait(.5); 00193 } 00194 00195 for(float delta=0.0;delta>-18000.0 && (*axis2.ptr_limit == 1);delta-=100){ 00196 axis2.set_point = delta; 00197 axis3.set_point = -delta; 00198 axis4.set_point = .249 * delta; 00199 axis5.set_point += .249 * -delta; 00200 wait(.5); 00201 } 00202 zero_axis(2); 00203 00204 for(float delta=0.0;delta<300.0 ;delta-=10){ 00205 axis3.set_point = delta; 00206 axis4.set_point = .249 *-delta; 00207 axis5.set_point = .249 * delta; 00208 wait(.1); 00209 } 00210 zero_axis(2); 00211 } 00212 00213 int home_pitch_wrist(void){ 00214 axis4.pid->setInputLimits(-50000, 50000); 00215 axis5.pid->setInputLimits(-50000, 50000); 00216 00217 axis4.axisOn(); 00218 axis5.axisOn(); 00219 00220 axis4.zero(); 00221 axis5.zero(); 00222 00223 //first test to see if limit switch is already pressed 00224 if(limit4 == 0){ 00225 pc.printf("Stage 1\r\n"); 00226 pc.printf("Limit switch 4 is already closed, moving up to release switch\r\n"); 00227 //move wrist up until limit switch is released again 00228 while(limit4 == 0){ 00229 pc.printf("axis4=%.1f axis5=%.1f sw=%1d I4=%f I5=%f \r\n", axis4.pos, axis5.pos, limit4, axis4.readCurrent(), axis5.readCurrent()); 00230 axis4.set_point -= 10; 00231 axis5.set_point += 10; 00232 wait(.08); 00233 } 00234 pc.printf("Switched released\r\n"); 00235 axis4.zero(); 00236 axis5.zero(); 00237 pc.printf("Encoders zeroed\r\n"); 00238 wait(.02); 00239 00240 pc.printf("Moving up to zero\r\n"); 00241 axis4.set_point = -1350; 00242 axis5.set_point = 1350; 00243 00244 wait(2.0); 00245 00246 return 0; //successful home of gripper 00247 } 00248 else{ 00249 pc.printf("Stage 2\r\n"); 00250 axis4.zero(); //zero wrist motors 00251 axis5.zero(); 00252 pc.printf("Move down\r\n"); 00253 while(limit4 == 1){ 00254 pc.printf("axis4=%.1f axis5=%.1f sw=%1d I4=%f I5=%f \r\n", axis4.pos, axis5.pos, limit4, axis4.readCurrent(), axis5.readCurrent()); 00255 axis4.set_point += 50; 00256 axis5.set_point -= 50; 00257 wait(.05); 00258 if(axis4.readCurrent() > .25){ 00259 pc.printf("Over Current detected on Axis 3\r\n"); 00260 axis4.zero(); 00261 axis5.zero(); 00262 00263 axis4.set_point -= 3500; 00264 axis5.set_point += 3500; 00265 00266 wait(2.0); 00267 00268 axis4.zero(); 00269 axis5.zero(); 00270 00271 return -1; 00272 } 00273 if(axis5.readCurrent() > .25){ 00274 pc.printf("Over Current detected on Axis 5\r\n"); 00275 axis4.zero(); 00276 axis5.zero(); 00277 00278 axis4.set_point -= 3500; 00279 axis5.set_point += 3500; 00280 00281 wait(2.0); 00282 00283 axis4.zero(); 00284 axis5.zero(); 00285 00286 return -2; 00287 } 00288 } 00289 00290 if(limit4 == 0){ 00291 while(limit4 == 0){ 00292 pc.printf("axis4=%.1f axis5=%.1f sw=%1d I4=%f I5=%f \r\n", axis4.pos, axis5.pos, limit4, axis4.readCurrent(), axis5.readCurrent()); 00293 axis4.set_point -= 50; 00294 axis5.set_point += 50; 00295 wait(.08); 00296 if(axis4.readCurrent() > .25){ 00297 pc.printf("Over Current detected on Axis 4\r\n"); 00298 axis4.zero(); 00299 axis5.zero(); 00300 00301 axis4.set_point -= 3500; 00302 axis5.set_point += 3500; 00303 00304 wait(2.0); 00305 00306 axis4.zero(); 00307 axis5.zero(); 00308 00309 return -1; 00310 } 00311 if(axis5.readCurrent() > .25){ 00312 pc.printf("Over Current detected on Axis 5\r\n"); 00313 axis4.zero(); 00314 axis5.zero(); 00315 00316 axis4.set_point -= 3500; 00317 axis5.set_point += 3500; 00318 00319 wait(2.0); 00320 00321 axis4.zero(); 00322 axis5.zero(); 00323 00324 return -2; 00325 } 00326 } 00327 axis4.zero(); 00328 axis5.zero(); 00329 wait(.02); 00330 00331 axis4.set_point = -1350; 00332 axis5.set_point = 1350; 00333 00334 wait(1.2); 00335 axis4.zero(); 00336 axis5.zero(); 00337 00338 return 0; //successful home of gripper 00339 } 00340 } 00341 return -3; //should have homed by now 00342 } 00343 00344 void home_rotate_wrist(void){ 00345 axis4.axisOn(); 00346 axis5.axisOn(); 00347 00348 while(limit5 == 0){ 00349 axis4.set_point -= 100; 00350 axis5.set_point -= 100; 00351 wait(.05); 00352 pc.printf("axis4=%.1f axis5=%.1f sw=%1d I4=%f I5=%f \r\n", axis4.pos, axis5.pos, limit5, axis4.readCurrent(), axis5.readCurrent()); 00353 } 00354 axis4.set_point -= 10; 00355 axis5.set_point -= 10; 00356 wait(.05); 00357 00358 while(limit5 == 1){ 00359 pc.printf("axis4=%.1f axis5=%.1f sw=%1d I4=%f I5=%f \r\n", axis4.pos, axis5.pos, limit5, axis4.readCurrent(), axis5.readCurrent()); 00360 axis4.set_point += 10; 00361 axis5.set_point += 10; 00362 wait(.03); 00363 } 00364 00365 axis4.zero(); 00366 axis5.zero(); 00367 00368 pc.printf("Find amount to rotate to after switch is high again...\r\n"); 00369 wait(1.0); 00370 00371 for(float pos=0;pos>-800.0;pos-=100){ 00372 axis4.set_point = pos; 00373 axis5.set_point = pos; 00374 wait(.05); 00375 pc.printf("axis4=%.1f axis5=%.1f sw=%1d I4=%f I5=%f \r\n", axis4.pos, axis5.pos, limit5, axis4.readCurrent(), axis5.readCurrent()); 00376 } 00377 axis4.zero(); 00378 axis5.zero(); 00379 } 00380 00381 void cal_gripper(void){ 00382 axis6.axisOn(); 00383 pc.printf("\r\n Axis On, Homeing Gripper\r\n"); 00384 pc.printf("axis6=%.1f I6=%f \r\n", axis6.pos, axis6.readCurrent()); 00385 axis6.zero(); 00386 00387 while((axis6.readCurrent() < .8) && (axis6.set_point > -6000)){ 00388 axis6.set_point -= 10; 00389 wait(.01); 00390 pc.printf("axis6=%.1f I6=%f \r\n", axis6.pos, axis6.readCurrent()); 00391 } 00392 axis6.set_point += 50; 00393 wait(.05); 00394 axis6.zero(); 00395 wait(.02); 00396 00397 while((axis6.readCurrent() < .8) && (axis6.set_point < 6000)){ 00398 axis6.set_point += 10; 00399 wait(.01); 00400 pc.printf("axis6=%.1f I6=%f \r\n", axis6.pos, axis6.readCurrent()); 00401 } 00402 00403 axis6.totalCounts = axis6.set_point; 00404 wait(.05); 00405 00406 axis6.set_point = axis6.totalCounts/2.0; //~4500 total span, ~2200 to half close 00407 wait(2.0); 00408 // axis6.zero(); //may remove later for 0 = grip closed 00409 } 00410 00411 void all_on(void){ 00412 axis1.axisOn(); 00413 axis2.axisOn(); 00414 axis3.axisOn(); 00415 axis4.axisOn(); 00416 axis5.axisOn(); 00417 axis6.axisOn(); 00418 } 00419 00420 void all_off(void){ 00421 axis1.axisOff(); 00422 axis2.axisOff(); 00423 axis3.axisOff(); 00424 axis4.axisOff(); 00425 axis5.axisOff(); 00426 axis6.axisOff(); 00427 } 00428 00429 void limit1_irq(void){ 00430 limit1 = limit1_pin; 00431 00432 if(limit1) 00433 limit1_int.fall(&limit1_irq); 00434 else 00435 limit1_int.rise(&limit1_irq); 00436 } 00437 00438 void limit2_irq(void){ 00439 limit2 = limit2_pin; 00440 00441 if(limit2) 00442 limit2_int.fall(&limit2_irq); 00443 else 00444 limit2_int.rise(&limit2_irq); 00445 } 00446 00447 void limit3_irq(void){ 00448 limit3 = limit3_pin; 00449 00450 if(limit3) 00451 limit3_int.fall(&limit3_irq); 00452 else 00453 limit3_int.rise(&limit3_irq); 00454 } 00455 00456 void limit4_irq(void){ 00457 limit4 = limit4_pin; 00458 00459 if(limit4) 00460 limit4_int.fall(&limit4_irq); 00461 else 00462 limit4_int.rise(&limit4_irq); 00463 } 00464 00465 void limit5_irq(void){ 00466 limit5 = limit5_pin; 00467 00468 if(limit5) 00469 limit5_int.fall(&limit5_irq); 00470 else 00471 limit5_int.rise(&limit5_irq); 00472 } 00473 00474 void limit6_irq(void){ 00475 limit6 = limit6_pin; 00476 00477 if(limit6) 00478 limit6_int.fall(&limit6_irq); 00479 else 00480 limit6_int.rise(&limit6_irq); 00481 } 00482 void init_limitSwitches(void){ 00483 00484 //Limit switch 1 initial state 00485 limit1 = limit1_pin; 00486 if(limit1) 00487 limit1_int.fall(&limit1_irq); 00488 else 00489 limit1_int.rise(&limit1_irq); 00490 00491 //Limit switch 2 initial state 00492 limit2 = limit2_pin; 00493 if(limit2) 00494 limit2_int.fall(&limit2_irq); 00495 else 00496 limit2_int.rise(&limit2_irq); 00497 00498 //Limit switch 3 initial state 00499 limit3 = limit3_pin; 00500 if(limit3) 00501 limit3_int.fall(&limit3_irq); 00502 else 00503 limit3_int.rise(&limit3_irq); 00504 00505 //Limit switch 4 initial state 00506 limit4 = limit4_pin; 00507 if(limit4) 00508 limit4_int.fall(&limit4_irq); 00509 else 00510 limit4_int.rise(&limit4_irq); 00511 00512 //Limit switch 5 initial state 00513 limit5 = limit5_pin; 00514 if(limit5) 00515 limit5_int.fall(&limit5_irq); 00516 else 00517 limit5_int.rise(&limit5_irq); 00518 00519 //Limit switch 6 initial state 00520 limit6 = limit6_pin; 00521 if(limit6) 00522 limit6_int.fall(&limit6_irq); 00523 else 00524 limit6_int.rise(&limit6_irq); 00525 00526 } 00527 //------------------- MAIN -------------------------------- 00528 int main() 00529 { 00530 wait(.5); 00531 pulse.attach(&alive, 2.0); // the address of the function to be attached (flip) and the interval (2 seconds) 00532 00533 pc.baud(921600); 00534 pc.printf("\r\n%s\r\n", __FILE__); //display the filename (this source file) 00535 00536 init_limitSwitches(); //get initial states of limit switches 00537 00538 axis1.init(); 00539 axis2.init(); 00540 axis3.init(); 00541 axis4.init(); 00542 axis5.init(); 00543 axis6.init(); 00544 00545 axis6.Pk = 40.0; 00546 axis6.Ik = 20.0; 00547 00548 // axis1.debug = 1; 00549 // axis2.debug = 1; 00550 // axis3.debug = 1; 00551 // axis4.debug = 1; 00552 // axis5.debug = 1; 00553 // axis6.debug = 1; 00554 00555 t.start(); // Set up timer 00556 00557 while(1){ 00558 00559 if(pc.readable()) 00560 { 00561 char c = pc.getc(); 00562 00563 if(c == '?') //get commands 00564 { 00565 pc.printf("? - This menu of commands\r\n"); 00566 pc.printf("0 - Zero all encoder channels\r\n"); 00567 pc.printf("A - All: Enable/Disable All axes. Then 'O' for On and 'F' for Off\r\n"); 00568 pc.printf("C - Current: Stream the Motor Currents\r\n"); 00569 pc.printf("J - Stream Flag: Enable/Disable Stream. Then '1' for On and '0' for Off\r\n"); 00570 pc.printf("W - Wrist: Home the Gripper Wrist\r\n"); 00571 pc.printf("U - Up: Bring up from typical starting position (Not HOME!)\r\n"); 00572 pc.printf("T - Trapezoidal Profile Move command\r\n"); 00573 pc.printf("H- Home\r\n"); 00574 pc.printf("S- Set point in encoder counts\r\n"); 00575 pc.printf("Z - Zero Encoder channel\r\n"); 00576 pc.printf("X - Excercise Robotic Arm\r\n"); 00577 pc.printf("O - Axis to turn On \r\n"); 00578 pc.printf("F - Axis to turn Off (Default)\r\n"); 00579 pc.printf("\r\nP - Set Proportional Gain on an Axis\r\n"); 00580 pc.printf("I - Set Integral Gain on an Axis\r\n"); 00581 pc.printf("D - Set Derivative Gain on an Axis\r\n"); 00582 pc.printf("\r\nB - Pitch Gripper\r\n"); 00583 pc.printf("N - Rotate Gripper\r\n"); 00584 pc.printf("G - Home Gripper\r\n"); 00585 pc.printf("spacebar - Emergency Stop\r\n"); 00586 00587 pc.printf("Press any key to continue.\r\n"); 00588 00589 pc.scanf("%c",&c); 00590 c = '\0'; //re-zero c 00591 } 00592 00593 if(c == '0'){ //zero all encoders and channels 00594 zero_all(); 00595 } 00596 // All: Enable/Disable ALL motors (On or Off) 00597 if((c == 'A') || (c == 'a')){ 00598 pc.printf("All: 'o' for all On, 'f' for all off\r\n"); 00599 00600 pc.scanf("%c",&c); 00601 if((c == 'O' || c == 'o')){ 00602 all_on(); 00603 } 00604 if((c == 'F' || c == 'f')){ 00605 all_off(); 00606 } 00607 c = '\0'; //clear c 00608 } 00609 00610 //Temporary command for Wrist Pitch 00611 if(c == 'B' || c == 'b'){ 00612 pc.printf("Enter wrist pitch counts\r\n"); 00613 00614 float counts; 00615 00616 pc.scanf("%f",&counts); 00617 axis4.set_point += counts; 00618 axis5.set_point += -counts; 00619 00620 pc.printf("%f\r\n",counts); 00621 t.reset(); 00622 while((axis4.pos > (axis4.set_point + SP_TOL) || 00623 axis4.pos < (axis4.set_point - SP_TOL)) && 00624 (axis5.pos > (axis5.set_point + SP_TOL) || 00625 axis5.pos < (axis5.set_point - SP_TOL))){ 00626 pc.printf("T=%.2f SP4=%.3f pos4=%.3f SP5=%.3f pos5=%.3f \r\n", t.read(), axis4.set_point, axis4.pos, axis5.set_point, axis5.pos); 00627 wait(.009); 00628 } 00629 } 00630 00631 //wrist rotate 00632 if(c == 'N' || c == 'n'){ 00633 pc.printf("Enter wrist rotate counts\r\n"); 00634 00635 float counts; 00636 00637 pc.scanf("%f",&counts); 00638 axis4.set_point += counts; 00639 axis5.set_point += counts; 00640 00641 pc.printf("%f\r\n",counts); 00642 t.reset(); 00643 while((axis4.pos > (axis4.set_point + SP_TOL) || 00644 axis4.pos < (axis4.set_point - SP_TOL)) && 00645 (axis5.pos > (axis5.set_point + SP_TOL) || 00646 axis5.pos < (axis5.set_point - SP_TOL))){ 00647 pc.printf("T=%.2f SP=%.3f co=%.3f pos=%.3f vel=%.3f acc=%.3f \r\n", t.read(), axis4.set_point, axis4.enc, axis5.set_point, axis5.enc); 00648 wait(.009); 00649 } 00650 } 00651 00652 //Current Measurement: Stream the motor currents 00653 if((c == 'C') || (c == 'c')){ 00654 int analogFlag = 0; 00655 while(analogFlag == 0){ 00656 axis1.readCurrent(); 00657 axis2.readCurrent(); 00658 axis3.readCurrent(); 00659 axis4.readCurrent(); 00660 axis5.readCurrent(); 00661 axis6.readCurrent(); 00662 pc.printf("%.3f %.3f %.3f %.3f %.3f %.3f \r\n", axis1.motCurrent, axis2.motCurrent, axis3.motCurrent, axis4.motCurrent, axis5.motCurrent, axis6.motCurrent); 00663 wait(.02); 00664 00665 if(pc.readable()){ //if user types a 'q' or 'Q' 00666 char c = pc.getc(); 00667 if(c == 'q' || c == 'Q') //quit after current movement 00668 analogFlag = 1; 00669 } 00670 } 00671 } 00672 00673 //Limit: Limit Switch Monitor 00674 if((c == 'L') || (c == 'l')){ 00675 int limitFlag = 1; 00676 00677 while(limitFlag){ 00678 pc.printf("%1d %1d %1d %1d %1d %1d %1d\r\n", limit1, limit2, limit3, limit4, limit5, limit6); 00679 wait(.02); 00680 00681 if(pc.readable()){ //if user types a 'q' or 'Q' 00682 char c = pc.getc(); 00683 if(c == 'q' || c == 'Q') //quit after current movement 00684 limitFlag = 0;; 00685 } 00686 } 00687 } 00688 00689 //W: Wrist home fuction 00690 if((c == 'W') || (c == 'w')){ 00691 home_pitch_wrist(); 00692 home_rotate_wrist(); 00693 } 00694 00695 //gripper home 00696 if((c == 'G') || (c == 'g')){ 00697 cal_gripper(); 00698 } 00699 00700 //Up: Bring up from typical starting position 00701 if((c == 'U') || (c == 'u')){ 00702 pc.printf("Bring up from typical unpowered position\r\n"); 00703 00704 axis1.zero(); 00705 axis2.zero(); 00706 axis3.zero(); 00707 axis4.zero(); 00708 axis5.zero(); 00709 axis6.zero(); 00710 00711 all_on(); 00712 axis2.set_point += 8000; 00713 wait(3.5); 00714 axis2.zero(); 00715 axis3.set_point -= 4500; 00716 wait(2.5); 00717 axis3.zero(); 00718 00719 home_pitch_wrist(); 00720 home_rotate_wrist(); 00721 cal_gripper(); 00722 } 00723 00724 //Exercise: Randomly exercise all axes 00725 if((c == 'X' || c == 'x')) //Exercise all axes 00726 { 00727 pc.printf("\r\nPress q to quit Exercise\r\n"); 00728 pc.printf("Received move test command\r\n"); 00729 int qFlag=0; 00730 float lastmovetime = 0.0; 00731 while(qFlag==0){ 00732 t.reset(); 00733 float movetime = rand() % 7; 00734 movetime += 1.0; 00735 lastmovetime = t.read() + movetime; 00736 00737 axis1.moveTrapezoid((rand() % 2000) - 1000, movetime); 00738 wait(.05); 00739 axis2.moveTrapezoid((rand() % 5000) - 2500, movetime); 00740 wait(.05); 00741 axis3.moveTrapezoid((rand() % 5000) - 2500, movetime); 00742 wait(.05); 00743 axis4.moveTrapezoid((rand() % 3000) - 1500, movetime); 00744 wait(.05); 00745 axis5.moveTrapezoid((rand() % 3000) - 1500, movetime); 00746 wait(.05); 00747 axis6.moveTrapezoid((rand() % 3000), movetime); 00748 wait(.05); 00749 00750 while(t.read() <= lastmovetime + 1.0){ 00751 //pc.printf("T=%.2f ax1SP=%.3f ax1pos=%.3f ax2SP=%.3f ax2pos=%.3f \r\n", t.read(), axis1.set_point, axis1.pos, axis2.set_point, axis2.pos); 00752 pc.printf("%.2f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f\r\n", t.read(), axis1.set_point, axis1.pos, axis2.set_point, axis2.pos, 00753 axis3.set_point, axis3.pos, axis4.set_point, axis4.pos,axis5.set_point, axis5.pos, axis6.set_point, axis6.pos); 00754 wait(.01); 00755 00756 if(pc.readable()){ //if user types a 'q' or 'Q' 00757 char c = pc.getc(); 00758 if(c == 'q' || c == 'Q') //quit after current movement 00759 qFlag = 1; //set the flag to terminate the excercise 00760 if(c == ' '){ //stop immediately 00761 axis1.moveState = 4; 00762 axis2.moveState = 4; 00763 axis3.moveState = 4; 00764 axis4.moveState = 4; 00765 axis5.moveState = 4; 00766 axis6.moveState = 4; 00767 qFlag = 1; //set the flag to terminate the excercise 00768 break; 00769 } 00770 } 00771 } 00772 } 00773 } 00774 00775 //Trapazoid: move trapazoidal profile on Axis 00776 if(c == 'T' || c == 't'){ //move Trapazoid command 00777 float position = 0.0; 00778 float time = 0.0; 00779 00780 pc.printf("Enter axis to move trapazoid\r\n"); 00781 pc.scanf("%c",&c); 00782 00783 pc.printf("\r\n\r\nEnter position:"); 00784 pc.scanf("%f",&position); 00785 pc.printf("%f\r\n", position); 00786 00787 pc.printf("Enter time:"); 00788 pc.scanf("%f",&time); 00789 pc.printf("%f\r\n", time); 00790 00791 switch(c){ 00792 case '1': 00793 pc.printf("Moving Robotic Axis 1\r\n"); 00794 axis1.moveTrapezoid(position, time); 00795 break; 00796 00797 case '2': 00798 pc.printf("Moving Robotic Axis 2\r\n"); 00799 axis2.moveTrapezoid(position, time); 00800 break; 00801 00802 case '3': 00803 pc.printf("Moving Robotic Axis 3\r\n"); 00804 axis3.moveTrapezoid(position, time); 00805 break; 00806 00807 case '4': 00808 pc.printf("Moving Robotic Axis 4\r\n"); 00809 axis4.moveTrapezoid(position, time); 00810 break; 00811 00812 case '5': 00813 pc.printf("Moving Robotic Axis 5\r\n"); 00814 axis5.moveTrapezoid(position, time); 00815 break; 00816 00817 case '6': 00818 pc.printf("Moving Robotic Axis 6\r\n"); 00819 axis6.moveTrapezoid(position, time); 00820 break; 00821 } 00822 } 00823 00824 //Home: home command 00825 if(c == 'H' || c == 'h'){ 00826 pc.printf("Enter axis to home\r\n"); 00827 pc.scanf("%c",&c); 00828 switch(c){ 00829 case '1': 00830 pc.printf("Homing Robotic Axis 1\r\n"); 00831 axis1.center(); 00832 break; 00833 case '2': 00834 pc.printf("Homing Robotic Axis 2\r\n"); 00835 axis2.center(); 00836 break; 00837 00838 case '3': 00839 pc.printf("Homing Robotic Axis 3\r\n"); 00840 axis3.center(); 00841 break; 00842 00843 case '4': 00844 pc.printf("Homing Robotic Axis 4\r\n"); 00845 axis4.center(); 00846 break; 00847 00848 case '5': 00849 pc.printf("Homing Robotic Axis 5\r\n"); 00850 axis5.center(); 00851 break; 00852 00853 case '6': 00854 pc.printf("Homing Robotic Axis 6\r\n"); 00855 axis6.center(); 00856 break; 00857 } 00858 } 00859 00860 //Set Point: Manually move to specific encoder position set point 00861 if(c == 'S' || c == 's'){ 00862 pc.printf("Enter axis to give set point:"); 00863 pc.scanf("%c",&c); 00864 pc.printf("Axis %c entered.\r\n", c); 00865 pc.printf("Enter value for set point axis %c\r\n", c); 00866 float temp_setpoint; 00867 pc.scanf("%f",&temp_setpoint); 00868 pc.printf("Axis%c set point %.1f\r\n", c, temp_setpoint); 00869 00870 switch(c){ 00871 case '1': 00872 axis1.set_point = temp_setpoint; 00873 00874 t.reset(); 00875 while((axis1.pos > (axis1.set_point + SP_TOL)) || (axis1.pos < (axis1.set_point - SP_TOL))){ 00876 pc.printf("T=%.2f SP=%.3f co=%.3f pos=%.3f vel=%.3f acc=%.3f\r\n", t.read(), axis1.set_point, axis1.co, axis1.pos, axis1.vel, axis1.acc); 00877 wait(.009); 00878 if(t.read() > 10.0){ 00879 pc.printf("Set point timeout!\r\n"); 00880 break; 00881 } 00882 } 00883 pc.printf("T=%.2f SP=%.3f co=%.3f pos=%.3f vel=%.3f acc=%.3f\r\n\n", t.read(), axis1.set_point, axis1.co, axis1.pos, axis1.vel, axis1.acc); 00884 00885 break; 00886 00887 case '2': 00888 // float delta3 = axis2.pos - temp_setpoint; 00889 // axis3.set_point += delta3; 00890 00891 axis2.set_point = temp_setpoint; 00892 t.reset(); 00893 while((axis2.pos > (axis2.set_point + SP_TOL)) || (axis2.pos < (axis2.set_point - SP_TOL))){ 00894 pc.printf("T=%.2f %.3f pos=%.3f vel=%.3f acc=%.3f\r\n", t.read(), axis2.set_point, axis2.pos, axis2.vel, axis2.acc); 00895 wait(.009); 00896 if(t.read() > 10.0){ 00897 pc.printf("Set point timeout!\r\n"); 00898 break; 00899 } 00900 } 00901 pc.printf("T=%.2f SP=%.3f co=%.3f pos=%.3f vel=%.3f acc=%.3f\r\n\n", t.read(), axis2.set_point, axis2.co, axis2.pos, axis2.vel, axis2.acc); 00902 break; 00903 00904 case '3': 00905 axis3.set_point = temp_setpoint; 00906 t.reset(); 00907 while((axis3.pos > (axis3.set_point + SP_TOL)) || (axis3.pos < (axis3.set_point - SP_TOL))){ 00908 pc.printf("T=%.2f SP=%.3f co=%.3f pos=%.3f vel=%.3f acc=%.3f\r\n", t.read(), axis3.set_point, axis3.co, axis3.pos, axis3.vel, axis3.acc); 00909 wait(.009); 00910 if(t.read() > 10.0){ 00911 pc.printf("Set point timeout!\r\n"); 00912 break; 00913 } 00914 } 00915 pc.printf("T=%.2f SP=%.3f co=%.3f pos=%.3f vel=%.3f acc=%.3f\r\n\n", t.read(), axis3.set_point, axis3.co, axis3.pos, axis3.vel, axis3.acc); 00916 break; 00917 00918 case '4': 00919 axis4.set_point = temp_setpoint; 00920 t.reset(); 00921 while((axis4.pos > (axis4.set_point + SP_TOL)) || (axis4.pos < (axis4.set_point - SP_TOL))){ 00922 pc.printf("T=%.2f SP=%.3f co=%.3f pos=%.3f vel=%.3f acc=%.3f\r\n", t.read(), axis4.set_point, axis4.co, axis4.pos, axis4.vel, axis4.acc); 00923 wait(.009); 00924 if(t.read() > 10.0){ 00925 pc.printf("Set point timeout!\r\n"); 00926 break; 00927 } 00928 } 00929 pc.printf("T=%.2f SP=%.3f co=%.3f pos=%.3f vel=%.3f acc=%.3f\r\n\n", t.read(), axis4.set_point, axis4.co, axis4.pos, axis4.vel, axis4.acc); 00930 break; 00931 00932 case '5': 00933 axis5.set_point = temp_setpoint; 00934 t.reset(); 00935 while((axis5.pos > (axis5.set_point + SP_TOL)) || (axis5.pos < (axis5.set_point - 50))){ 00936 pc.printf("T=%.2f SP=%.3f co=%.3f pos=%.3f vel=%.3f acc=%.3f\r\n", t.read(), axis5.set_point, axis5.co, axis5.pos, axis5.vel, axis5.acc); 00937 wait(.009); 00938 if(t.read() > 10.0){ 00939 pc.printf("Set point timeout!\r\n"); 00940 break; 00941 } 00942 } 00943 pc.printf("T=%.2f SP=%.3f co=%.3f pos=%.3f vel=%.3f acc=%.3f\r\n\n", t.read(), axis5.set_point, axis5.co, axis5.pos, axis5.vel, axis5.acc); 00944 break; 00945 00946 case '6': 00947 axis6.set_point = temp_setpoint; 00948 t.reset(); 00949 while((axis6.pos > (axis6.set_point + SP_TOL)) || (axis6.pos < (axis6.set_point - SP_TOL))){ 00950 pc.printf("T=%.2f SP=%.3f co=%.3f pos=%.3f vel=%.3f acc=%.3f\r\n", t.read(), axis6.set_point, axis6.co, axis6.pos, axis6.vel, axis6.acc); 00951 wait(.009); 00952 if(t.read() > 10.0){ 00953 pc.printf("Set point timeout!\r\n"); 00954 break; 00955 } 00956 } 00957 pc.printf("T=%.2f SP=%.3f co=%.3f pos=%.3f vel=%.3f acc=%.3f\r\n\n", t.read(), axis6.set_point, axis6.co, axis6.pos, axis6.vel, axis6.acc); 00958 break; 00959 } 00960 } 00961 00962 if(c == 'P' || c == 'p'){ 00963 float temp_Pk; 00964 pc.printf("Enter axis to set Pk\r\n"); 00965 pc.scanf("%c",&c); 00966 00967 pc.printf("Enter value for Axis%c Pk\r\n", c); 00968 pc.scanf("%f",&temp_Pk); 00969 00970 switch(c){ 00971 case 1: 00972 axis1.Pk = temp_Pk; 00973 break; 00974 00975 case 2: 00976 axis2.Pk = temp_Pk; 00977 break; 00978 00979 case 3: 00980 axis3.Pk = temp_Pk; 00981 break; 00982 00983 case 4: 00984 axis4.Pk = temp_Pk; 00985 break; 00986 00987 case 5: 00988 axis5.Pk = temp_Pk; 00989 break; 00990 00991 case 6: 00992 axis6.Pk = temp_Pk; 00993 break; 00994 } 00995 pc.printf("Axis%c Pk set to %f\r\n", c, temp_Pk); 00996 } 00997 if(c == 'I' || c == 'i'){ 00998 float temp_Ik; 00999 pc.printf("Enter axis to set Ik\r\n"); 01000 pc.scanf("%c",&c); 01001 01002 pc.printf("Enter value for Axis%c Ik\r\n", c); 01003 pc.scanf("%f",&temp_Ik); 01004 01005 switch(c){ 01006 case 1: 01007 axis1.Ik = temp_Ik; 01008 break; 01009 01010 case 2: 01011 axis2.Ik = temp_Ik; 01012 break; 01013 01014 case 3: 01015 axis3.Ik = temp_Ik; 01016 break; 01017 01018 case 4: 01019 axis4.Ik = temp_Ik; 01020 break; 01021 01022 case 5: 01023 axis5.Ik = temp_Ik; 01024 break; 01025 01026 case 6: 01027 axis6.Ik = temp_Ik; 01028 break; 01029 } 01030 pc.printf("Axis%c Ik set to %f\r\n", c, temp_Ik); 01031 } 01032 if(c == 'D' || c == 'd'){ 01033 float temp_Dk; 01034 pc.printf("Enter axis to set Dk\r\n"); 01035 pc.scanf("%c",&c); 01036 01037 pc.printf("Enter value for Axis%c Dk\r\n", c); 01038 pc.scanf("%f",&temp_Dk); 01039 01040 switch(c){ 01041 case 1: 01042 axis1.Dk = temp_Dk; 01043 break; 01044 01045 case 2: 01046 axis2.Dk = temp_Dk; 01047 break; 01048 01049 case 3: 01050 axis3.Dk = temp_Dk; 01051 break; 01052 01053 case 4: 01054 axis4.Dk = temp_Dk; 01055 break; 01056 01057 case 5: 01058 axis5.Dk = temp_Dk; 01059 break; 01060 01061 case 6: 01062 axis6.Dk = temp_Dk; 01063 break; 01064 } 01065 pc.printf("Axis%c Ik set to %f\r\n", c, temp_Dk); 01066 } 01067 01068 //Zero: Zero specific axis 01069 if(c == 'Z' || c == 'z'){ 01070 pc.printf("Enter axis to Zero (1-6, or 'a' for all)\r\n"); 01071 pc.scanf("%c",&c); 01072 switch(c){ 01073 case '1': 01074 axis1.zero(); 01075 break; 01076 01077 case '2': 01078 axis2.zero(); 01079 break; 01080 01081 case '3': 01082 axis3.zero(); 01083 break; 01084 01085 case '4': 01086 axis4.zero(); 01087 break; 01088 01089 case '5': 01090 axis5.zero(); 01091 break; 01092 01093 case '6': 01094 axis6.zero(); 01095 break; 01096 01097 case 'a': //for all 01098 axis1.zero(); 01099 axis2.zero(); 01100 axis3.zero(); 01101 axis4.zero(); 01102 axis5.zero(); 01103 axis6.zero(); 01104 break; 01105 } 01106 01107 } 01108 if(c == 'O' || c == 'o'){ 01109 pc.printf("Enter axis to turn On (1-6, or 'a' for all)\r\n"); 01110 pc.scanf("%c",&c); 01111 01112 switch(c){ 01113 case '1': 01114 axis1.axisOn(); 01115 break; 01116 01117 case '2': 01118 axis2.axisOn(); 01119 break; 01120 01121 case '3': 01122 axis3.axisOn(); 01123 break; 01124 01125 case '4': 01126 axis4.axisOn(); 01127 break; 01128 01129 case '5': 01130 axis5.axisOn(); 01131 break; 01132 01133 case '6': 01134 axis6.axisOn(); 01135 break; 01136 01137 case 'a': 01138 axis1.axisOn(); 01139 axis2.axisOn(); 01140 axis3.axisOn(); 01141 axis4.axisOn(); 01142 axis5.axisOn(); 01143 axis6.axisOn(); 01144 break; 01145 } 01146 pc.printf("Axis%c On\r\n", c); 01147 } 01148 if(c == 'F' || c == 'f'){ 01149 pc.printf("Enter axis to turn Off (1-6, or 'a' for all)\r\n"); 01150 pc.scanf("%c",&c); 01151 01152 switch(c){ 01153 case '1': 01154 axis1.axisOff(); 01155 break; 01156 01157 case '2': 01158 axis2.axisOff(); 01159 break; 01160 01161 case '3': 01162 axis3.axisOff(); 01163 break; 01164 01165 case '4': 01166 axis4.axisOff(); 01167 break; 01168 01169 case '5': 01170 axis5.axisOff(); 01171 break; 01172 01173 case '6': 01174 axis6.axisOff(); 01175 break; 01176 01177 case 'a': 01178 axis1.axisOff(); 01179 axis2.axisOff(); 01180 axis3.axisOff(); 01181 axis4.axisOff(); 01182 axis5.axisOff(); 01183 axis6.axisOff(); 01184 break; 01185 } 01186 pc.printf("Axis%c Off\r\n", c); 01187 } 01188 01189 // Toggle Stream flag (for display purposes 01190 if(c == 'J' || c == 'j'){ 01191 pc.printf("Enter 1 to turn stream On, 0 to turn Off:\r\n"); 01192 pc.scanf("%c",&c); 01193 01194 if(c == '1'){ 01195 streamFlag = 1; 01196 pc.printf("Stream On\r\n"); 01197 } 01198 if(c == '0'){ 01199 streamFlag = 0; 01200 pc.printf("Stream Off\r\n"); 01201 } 01202 c = '\0'; 01203 } 01204 01205 if(c == 'M' || c == 'm'){ //move axis (with joints combined) 01206 pc.printf("Enter axis to move\r\n"); 01207 pc.scanf("%c",&c); 01208 pc.printf("Enter value for axis %c\r\n", c); 01209 float newPosition; 01210 switch(c){ 01211 case '2': 01212 pc.scanf("%f",&newPosition); 01213 axis2.set_point += newPosition; 01214 axis3.set_point += newPosition; 01215 axis4.set_point += (65.5/127.0 * newPosition); 01216 axis5.set_point -= (65.5/127.0 * newPosition); 01217 01218 01219 t.reset(); 01220 while((axis1.pos > (axis1.set_point + SP_TOL)) || (axis1.pos < (axis1.set_point - SP_TOL))){ 01221 pc.printf("T=%.2f SP=%.3f co=%.3f pos=%.3f vel=%.3f acc=%.3f\r\n", t.read(), axis1.set_point, axis1.co, axis1.pos, axis1.vel, axis1.acc); 01222 wait(.009); 01223 if(t.read() > 10.0){ 01224 pc.printf("Set point timeout!\r\n"); 01225 break; 01226 } 01227 } 01228 pc.printf("T=%.2f SP=%.3f co=%.3f pos=%.3f vel=%.3f acc=%.3f\r\n\n", t.read(), axis1.set_point, axis1.co, axis1.pos, axis1.vel, axis1.acc); 01229 01230 break; 01231 } 01232 } 01233 }//command was received 01234 01235 if((axis1.moveState==0) && (axis2.moveState==0) && (axis3.moveState==0) && (axis4.moveState==0) && (axis5.moveState==0) && (axis6.moveState==0)){ 01236 pc.printf("%.2f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f \r", (float)t.read(), (float)axis1.set_point, (float)axis1.pos, (float)axis2.set_point, (float)axis2.pos, 01237 (float)axis3.set_point, (float)axis3.pos, (float)axis4.set_point, (float)axis4.pos,(float)axis5.set_point, (float)axis5.pos, (float)axis6.set_point, (float)axis6.pos); 01238 if(streamFlag) 01239 pc.printf("\n"); 01240 led2 = 0; 01241 } 01242 else 01243 led2 = 1; 01244 01245 // pc.printf("Axis2: pos=%.1f co=%.1f setPoint=%.1f vel=%.1f\r\n", axis2.pos, axis2.co, axis2.set_point, axis2.vel); 01246 wait(.02); 01247 }//while(1) 01248 }//main 01249
Generated on Sat Jul 16 2022 14:04:53 by
1.7.2