Basic Mid-Level control for the rebuilt MorphGI control unit, using PWM to communicate with the low level controllers.
Dependencies: ros_lib_kinetic
main.cpp
00001 // STANDARD IMPORTS 00002 #include "math.h" 00003 // MBED IMPORTS 00004 #include "mbed.h" 00005 00006 // CUSTOM IMPORTS 00007 #include "MLSettings.h" 00008 //#include "HLComms.h" 00009 00010 #define N_CHANNELS 4 00011 00012 // DEMAND VARIABLES 00013 00014 Serial pc(USBTX, USBRX); // tx, rx for usb debugging 00015 00016 //HLComms hlcomms(HL_COMMS_FREQ_HZ); 00017 00018 double limDouble(double var, double min, double max){ 00019 double output = var; 00020 if(var <min){ 00021 output = min; 00022 } 00023 if (var>max){ 00024 output = max; 00025 } 00026 return output; 00027 } 00028 00029 00030 //Ticker setDemandsTicker; 00031 00032 00033 //joystick input pins 00034 00035 InterruptIn pinZeroIn (PD_4); 00036 InterruptIn pinCycleIn (PD_6); 00037 InterruptIn pinSucIn (PD_7); 00038 InterruptIn pinGasIn (PF_5); 00039 InterruptIn pinDebugIn (PF_9); 00040 InterruptIn pinFlushIn (PG_1); 00041 00042 DigitalIn pinWashIn (PD_5); 00043 DigitalIn pinFwd (PD_3); 00044 DigitalIn pinRev (PC_3); 00045 00046 00047 AnalogIn pinJSX (PC_0); 00048 AnalogIn pinJSY (PF_3); 00049 00050 DigitalOut pinFlushOut (PG_0); 00051 DigitalOut pinGasOut (PD_0); 00052 DigitalOut pinWashOut (PF_1); 00053 DigitalOut pinSucOut (PD_1); 00054 DigitalOut pinThirdPump(PF_2); 00055 DigitalOut pinJetOut (PF_0); 00056 00057 //DigitalIn arPins[7] = {pinZeroIn, pinCycleIn, pinSucIn, pinWashIn, pinGasIn, pinFwd, pinRev}; 00058 00059 Thread threadReadHHC(osPriorityHigh); 00060 Thread threadSendFeedback(osPriorityNormal); 00061 00062 Semaphore semReadHHC(1); 00063 Semaphore semSendFeedback(1); 00064 Semaphore semReceiveAndReplan(1); 00065 00066 //Ticker SendSensorDataTicker; 00067 Ticker tickerReadHHC; 00068 Ticker tickerSendFeedback; 00069 00070 void signalReadHHC(){ 00071 semReadHHC.release(); 00072 } 00073 00074 void signalSendFeedback() { 00075 semSendFeedback.release(); 00076 } 00077 00078 //support function states; 00079 00080 bool isZero = 0; 00081 bool isCycle = 0; 00082 bool isSuc = 0; 00083 bool isWash = 0; 00084 bool isGas = 0; 00085 bool isFwd = 0; 00086 bool isRev = 0; 00087 bool isBusy = 0; 00088 bool DemPos=0; 00089 bool isDebug = 0; 00090 00091 //set up PWM output pins for Demand POSITION AND VELOCITY 00092 PwmOut pinPosOut[N_CHANNELS] = {PB_15, PB_8, PB_9 ,PC_6}; 00093 PwmOut pinVelOut[N_CHANNELS] = {PB_10, PA_0, PB_11 ,PB_3}; 00094 00095 00096 void FlushActivate(){ 00097 if(!isBusy){ 00098 pinFlushOut = 1; 00099 } 00100 } 00101 00102 void FlushStop(){ 00103 if(!isBusy){ 00104 pinFlushOut = 0; 00105 } 00106 } 00107 00108 void ZeroActivate(){ 00109 if(!isBusy){ 00110 isZero = 1; 00111 } 00112 } 00113 void ZeroStop(){ 00114 00115 } 00116 00117 void DebugActivate(){ 00118 isDebug = 1; 00119 } 00120 00121 void DebugStop(){ 00122 isDebug = 0; 00123 } 00124 00125 void CycleActivate(){ 00126 if(!isBusy){ 00127 isCycle = 1; 00128 } 00129 } 00130 void CycleStop(){ 00131 00132 } 00133 00134 void GasActivate(){ 00135 if(!isBusy){ 00136 pinGasOut = 1; 00137 } 00138 } 00139 void GasStop(){ 00140 pinGasOut = 0; 00141 } 00142 00143 void SucActivate(){ 00144 if(!isBusy){ 00145 pinSucOut = 1; 00146 } 00147 } 00148 void SucStop(){ 00149 pinSucOut = 0; 00150 } 00151 00152 bool isChange = 1; 00153 00154 Timer timer1; 00155 00156 Ticker DemTicker; 00157 00158 int intAnchorDirection = 0; 00159 00160 00161 00162 //global variables 00163 double dblSensitivity = 1.0; 00164 double dblDriveStep = 0.05; 00165 bool isSteerLock = 0; 00166 double dblAlpha; 00167 double dblBeta; 00168 double dblTheta; 00169 double dblPhi; 00170 double dblS; 00171 double dblSdot; 00172 double dblX[3]; 00173 double dblXdot[3]; 00174 double dblAlphaDot; 00175 double dblBetaDot; 00176 double dblDeltaAlpha; 00177 double dblDeltaBeta; 00178 00179 double x_axis_sign = -1.0; 00180 00181 //kinematic constants 00182 double K_theta = 0.77;// rad/mL 00183 double d_theta = 0.3;// no unit 00184 00185 void SendFeedback(){ 00186 //support function interrupt states 00187 while(1){ 00188 semSendFeedback.wait(); 00189 if(!isBusy){ 00190 if(pinFwd.read()){ 00191 isFwd = 1; 00192 } else { 00193 isFwd = 0; 00194 } 00195 00196 if(pinRev.read()){ 00197 isRev = 1; 00198 } else { 00199 isRev = 0; 00200 } 00201 00202 if(pinSucIn.read()){ 00203 isSuc = 1; 00204 } else { 00205 isSuc = 0; 00206 } 00207 00208 if(pinWashIn.read()){ 00209 isWash = 1; 00210 } else { 00211 isWash = 0; 00212 } 00213 00214 if(pinGasIn.read()){ 00215 isGas = 1; 00216 } else { 00217 isGas = 0; 00218 } 00219 00220 00221 // 00222 //printf("\r\n"); 00223 //printf("%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t", isZero, isGas, isSuc, isRev, isWash, isCycle, isFwd, isDebug); 00224 // 00225 // double dblJSXprint = pinJSX.read(); 00226 // double dblJSYprint = pinJSY.read(); 00227 // printf("X:%f\tY:%f\t", dblJSXprint, dblJSYprint); 00228 00229 // printf("%f\t",dblAlphaDot); 00230 // printf("%f\t",dblBetaDot); 00231 // printf(":: "); 00232 // 00233 // printf("%f\t",dblAlpha); 00234 // printf("%f\t",dblBeta); 00235 // printf(":: "); 00236 // 00237 // printf("%f\t",dblTheta); 00238 // printf("%f\t",dblPhi); 00239 // printf(":: "); 00240 printf("\t"); 00241 00242 printf("%f\t",dblX[1]); 00243 printf("%f\t",dblX[0]); 00244 printf("%f\t",dblX[2]); 00245 printf(":: "); 00246 00247 // printf("%f\t",dblXdot[0]); 00248 // printf("%f\t",dblXdot[2]); 00249 // printf("%f\t",dblXdot[1]); 00250 // printf(":: "); 00251 00252 printf("%f\t",dblS); 00253 printf("%f\t",dblSdot); 00254 printf("\r\n"); 00255 } 00256 } 00257 } 00258 00259 double DeadZone(double var, double threshold, double max, double min){ 00260 00261 double output = var; 00262 output -= 0.5; 00263 min -= 0.5; 00264 max -= 0.5; 00265 00266 if(output > threshold){ 00267 output = (output - threshold); 00268 output /= (max - threshold); 00269 } else if(output < -1.0*threshold){ 00270 output = -1.0*(output + threshold); 00271 output /= (threshold + min); 00272 } else { 00273 output = 0.0; 00274 } 00275 00276 output = limDouble(output, -1.0, 1.0); 00277 return output; 00278 } 00279 00280 double ConvertToPWM(double target, double max){ 00281 double output; 00282 output = target*0.8/max ; 00283 output += 0.1; 00284 return output; 00285 } 00286 00287 void SetFrontDemand(double demandPos[], double demandVel[]){ 00288 00289 double demandPos_abs[3]; 00290 double demandVel_abs[3]; 00291 double pwmPos_front[3]; 00292 double pwmVel_front[3]; 00293 00294 for(int ii = 0; ii<3; ii++){ 00295 demandPos_abs[ii] = abs(demandPos[ii]); 00296 demandVel_abs[ii] = abs(demandVel[ii]); 00297 pwmPos_front[ii] = ConvertToPWM(demandPos_abs[ii], (double)MAX_ACTUATOR_LIMIT_MM); 00298 pwmVel_front[ii] = ConvertToPWM(demandVel_abs[ii], (double)MAX_SPEED_MMPS); 00299 pinPosOut[ii].write(pwmPos_front[ii]); 00300 pinVelOut[ii].write(pwmVel_front[ii]); 00301 00302 } 00303 00304 /* 00305 //print for debugging 00306 printf("X: "); 00307 for(int ii = 0; ii<3; ii++){ 00308 printf("%f\t",pwmPos_front[ii]); 00309 } 00310 printf("Xdot: "); 00311 for(int ii = 0; ii<3; ii++){ 00312 printf("%f\t",pwmVel_front[ii]); 00313 } 00314 */ 00315 } 00316 00317 void SetDriveDemand(double drivePos, double driveVel){ 00318 00319 double drivePos_abs; 00320 double driveVel_abs; 00321 double pwmPos_drive; 00322 double pwmVel_drive; 00323 00324 drivePos_abs = abs(drivePos); 00325 driveVel_abs = abs(driveVel); 00326 00327 pwmPos_drive = ConvertToPWM(drivePos_abs, MAX_DRIVE_SEGMENT_POSITION); 00328 pwmVel_drive = ConvertToPWM(driveVel_abs, MAX_DRIVE_SEGMENT_SPEED); 00329 00330 pinPosOut[3].write(pwmPos_drive); 00331 pinVelOut[3].write(pwmVel_drive); 00332 00333 /* 00334 //print for debugging 00335 printf("S:%f\tSdot:%f\t",pwmPos_drive,pwmVel_drive); 00336 */ 00337 } 00338 00339 double GetMaxTravelTime(double startPositions[], double endPositions[], double speed){ 00340 double arT[3]; 00341 for(int ii = 0; ii< 3; ii++){ 00342 arT[ii] = endPositions[ii] - startPositions[ii]; 00343 arT[ii] /= speed; 00344 arT[ii] = abs(arT[ii]); 00345 } 00346 double Tmax = 0.0; 00347 for(int ii = 0; ii<3; ii++){ 00348 if( Tmax < arT[ii] ){ 00349 Tmax = arT[ii]; 00350 } 00351 } 00352 return Tmax; 00353 } 00354 00355 void UpdateFrontSegPosition(double position[]){ 00356 for(int ii = 0; ii<3; ii++){ 00357 dblX[ii] = position[ii]; 00358 } 00359 } 00360 00361 void GoToNeutralPose(double speed){ 00362 isBusy = 1; 00363 double T_wait; 00364 double arSpeed[3] = {speed, speed, speed}; 00365 double arPosition[3] = {ACTUATOR_OFFSET, ACTUATOR_OFFSET, ACTUATOR_OFFSET}; 00366 SetFrontDemand(arPosition, arSpeed); 00367 00368 //T_wait = 40.0 / speed; 00369 T_wait = GetMaxTravelTime(dblX, arPosition, speed); 00370 T_wait += 0.25;//add a little bit of time for security 00371 //print for debug 00372 printf("\r\nGoing to neutral. %f\r\n", T_wait); 00373 00374 wait(T_wait); 00375 UpdateFrontSegPosition(arPosition); 00376 //isZero = 0; 00377 isBusy = 0; 00378 } 00379 00380 void GoToAnchorPose(double speed){ 00381 //reset front segment position 00382 //isAnchor = 0; 00383 isBusy = 1; 00384 double T_wait; 00385 double arSpeed[3] = {speed, speed, speed}; 00386 double arStartPosition[3]; 00387 //save starting position 00388 for (int ii = 0; ii<3; ii++){ 00389 arStartPosition[ii] = dblX[ii]; 00390 } 00391 00392 double arAnchorPosition[3] = {ACTUATOR_OFFSET, ACTUATOR_OFFSET, ACTUATOR_OFFSET}; 00393 00394 //Go to neutral 00395 GoToNeutralPose(speed); 00396 00397 //select direction of anchor 00398 arAnchorPosition[intAnchorDirection] = ANCHOR_POSITION; 00399 intAnchorDirection++; //rotate next anchoring position 00400 if(intAnchorDirection >= 3){ 00401 intAnchorDirection = 0; 00402 } 00403 //go to anchor position 00404 SetFrontDemand(arAnchorPosition, arSpeed); 00405 T_wait = GetMaxTravelTime(dblX, arAnchorPosition, speed); 00406 T_wait += 0.25;//add a little bit of time for security 00407 printf("Going to anchor. %f s\r\n", T_wait); 00408 wait(T_wait); 00409 00410 //update front segment position. 00411 UpdateFrontSegPosition(arAnchorPosition); 00412 00413 //contract 00414 SetDriveDemand(HOME_DRIVE_POSITION, SAFE_CONTRACT_SPEED); 00415 T_wait = dblS - HOME_DRIVE_POSITION; 00416 T_wait /= SAFE_CONTRACT_SPEED; 00417 printf("Contracting. %f s\r\n", T_wait); 00418 wait(T_wait); 00419 //update drive segment position 00420 dblS = HOME_DRIVE_POSITION; 00421 00422 //go back to neutral pose 00423 GoToNeutralPose(speed); 00424 00425 //go back to starting position 00426 SetFrontDemand(arStartPosition, arSpeed); 00427 T_wait = GetMaxTravelTime(dblX, arStartPosition, speed); 00428 T_wait += 0.25;//add a little bit of time for security 00429 printf("Going to intial position. %f\r\n", T_wait); 00430 wait(T_wait); 00431 //UpdateFrontSegPosition(arStartPosition); 00432 for(int ii = 0; ii<3; ii++){ 00433 dblX[ii] = arStartPosition[ii]; 00434 } 00435 00436 //print for debug 00437 isBusy = 0; 00438 } 00439 00440 00441 void GoToCustomPose(double position[], double speed){ 00442 //reset front segment position 00443 //isBusy = 1; 00444 double T_wait; 00445 double arSpeed[3] = {speed, speed, speed}; 00446 for (int ii = 0; ii<3; ii++){ 00447 position[ii] = limDouble(position[ii], 0.0, (double)MAX_ACTUATOR_LIMIT_MM); 00448 } 00449 SetFrontDemand(position, arSpeed); 00450 00451 //T_wait = 40.0 / speed; 00452 T_wait = GetMaxTravelTime(dblX, position, speed); 00453 T_wait += 0.25;//add a little bit of time for security 00454 //print for debug 00455 printf("\r\nGoing to custom pose. T_wait: %f s \r\n", T_wait); 00456 wait(T_wait); 00457 UpdateFrontSegPosition(position); 00458 printf("Done\r\n"); 00459 //isBusy = 0; 00460 } 00461 00462 00463 void AllGoTo(double position, double speed){ 00464 //reset front segment position 00465 //isBusy = 1; 00466 double T_wait; 00467 double arSpeed[3] = {speed, speed, speed}; 00468 position = limDouble(position, 0.0, (double)MAX_ACTUATOR_LIMIT_MM); 00469 00470 double arPosition[3] = {position, position, position}; 00471 SetFrontDemand(arPosition, arSpeed); 00472 00473 //T_wait = 40.0 / speed; 00474 T_wait = GetMaxTravelTime(dblX, arPosition, speed); 00475 T_wait += 0.25;//add a little bit of time for security 00476 //print for debug 00477 printf("\r\nMoving all to %f mm. T_wait: %f\r\n",position, T_wait); 00478 00479 wait(T_wait); 00480 UpdateFrontSegPosition(arPosition); 00481 printf("Done\r\n"); 00482 //isBusy = 0; 00483 } 00484 00485 void FlushSyringes(int repetitions){ 00486 //isBusy = 1; 00487 printf("\r\n\r\n:::Flushing Syringes:::\r\n"); 00488 pinFlushOut = 1; 00489 wait(1.0); 00490 for(int ii = 0; ii<repetitions; ii++){ 00491 AllGoTo(40.0,20.0); 00492 wait(0.5); 00493 AllGoTo(0.0,2.0); 00494 wait(0.5); 00495 } 00496 pinFlushOut = 0; 00497 printf("Done\r\n"); 00498 //isBusy = 0; 00499 } 00500 00501 void FlushSegment(int repetitions){ 00502 //isBusy = 1; 00503 printf("\r\n\r\n:::Flushing Segment:::\r\n"); 00504 FlushSyringes(1);//ensure syringes are flushed. 00505 double arZero[3] = {0.0, 0.0, 0.0}; 00506 00507 double flushPosition = 30.0; 00508 00509 double arFlushPose1[3] = {flushPosition, 0.0, 0.0}; 00510 double arFlushPose2[3] = {0.0, flushPosition, 0.0}; 00511 double arFlushPose3[3] = {0.0, 0.0, flushPosition}; 00512 for(int ii = 0; ii<repetitions; ii++){ 00513 //flush first chamber 00514 printf("Flushing first chamber\r\n"); 00515 GoToCustomPose(arFlushPose1, SAFE_SPEED_MMpS); 00516 AllGoTo(0.0, SAFE_SPEED_MMpS); 00517 00518 //flush second chamber 00519 printf("Flushing second chamber\r\n"); 00520 GoToCustomPose(arFlushPose2, SAFE_SPEED_MMpS); 00521 AllGoTo(0.0, SAFE_SPEED_MMpS); 00522 00523 //flush third chamber 00524 printf("Flushing third chamber\r\n"); 00525 GoToCustomPose(arFlushPose3, SAFE_SPEED_MMpS); 00526 AllGoTo(0.0, SAFE_SPEED_MMpS); 00527 00528 FlushSyringes(1);//flush syringes 00529 } 00530 printf("Done\r\n"); 00531 //isBusy = 0; 00532 } 00533 00534 void FlushTubing(int repetitions){ 00535 //isBusy = 1; 00536 printf("\r\n\r\n:::Flushing Tubing:::\r\n"); 00537 for (int ii = 0; ii<repetitions; ii++){ 00538 AllGoTo(40.0,15.0); 00539 pinFlushOut = 1; 00540 wait(1.0); 00541 AllGoTo(0.0, 2.0); 00542 pinFlushOut = 0; 00543 wait(0.5); 00544 } 00545 } 00546 00547 00548 void DebugMode(){ 00549 isBusy = 1; 00550 printf("\r\nEntering debug mode. Going to zero.\r\n"); 00551 double zeroPos[3] = {0.0, 0.0, 0.0}; 00552 double safeVel[3] = {SAFE_SPEED_MMpS, SAFE_SPEED_MMpS, SAFE_SPEED_MMpS}; 00553 00554 00555 SetFrontDemand(zeroPos, safeVel); 00556 SetDriveDemand(0.0, SAFE_CONTRACT_SPEED); 00557 double T_wait = GetMaxTravelTime(dblX, zeroPos, SAFE_SPEED_MMpS); 00558 T_wait += 0.25; 00559 wait(T_wait); 00560 UpdateFrontSegPosition(zeroPos); 00561 printf("Done\r\nPlease enter command\r\n"); 00562 00563 int countSyringeFlush = 0; 00564 int countSegmentFlush = 0; 00565 int countTubingFlush = 0; 00566 00567 while(isDebug){ 00568 isBusy = 1; 00569 //printf("x\r\n"); 00570 00571 if( pinZeroIn.read() ){ 00572 //printf("zero%d\r\n",countSyringeFlush); 00573 00574 if(countSyringeFlush == 0){ 00575 printf("\r\n Syringe Flush Selected\r\n"); 00576 } 00577 countSyringeFlush++; 00578 countSegmentFlush = 0; 00579 countTubingFlush = 0; 00580 if(countSyringeFlush > 50){ 00581 FlushSyringes(4); 00582 countSyringeFlush = 0; 00583 } 00584 //printf("%d\r\n", countSyringeFlush); 00585 00586 }else { 00587 countSyringeFlush = 0; 00588 } 00589 00590 if( pinSucIn.read() ){ 00591 //printf("suc%d\r\n", countSegmentFlush); 00592 00593 if(countSegmentFlush == 0){ 00594 printf("\r\n Segment Flush Selected\r\n"); 00595 } 00596 countSyringeFlush = 0; 00597 countSegmentFlush++; 00598 countTubingFlush = 0; 00599 if(countSegmentFlush > 50){ 00600 FlushSegment(2); 00601 countSegmentFlush = 0; 00602 } 00603 }else { 00604 countSegmentFlush = 0; 00605 } 00606 00607 if( pinWashIn.read() ){ 00608 //printf("wash%d\r\n", countTubingFlush); 00609 if(countTubingFlush == 0){ 00610 printf("\r\n Tubing Flush Selected\r\n"); 00611 } 00612 countSyringeFlush = 0; 00613 countSegmentFlush = 0; 00614 countTubingFlush++; 00615 if(countTubingFlush > 50){ 00616 FlushTubing(2); 00617 countTubingFlush = 0; 00618 } 00619 }else { 00620 countTubingFlush = 0; 00621 } 00622 00623 wait(0.1); 00624 } 00625 00626 GoToNeutralPose(SAFE_SPEED_MMpS); 00627 //reset front segment position 00628 dblAlpha = 0.0; 00629 dblBeta = 0.0; 00630 printf("\r\nManual control enabled\r\n"); 00631 isBusy = 0; 00632 } 00633 00634 void ReadHHC(){ 00635 printf("Homing. Please wait...\r\n"); 00636 00637 00638 double startingPos[3] = {ACTUATOR_OFFSET, ACTUATOR_OFFSET, ACTUATOR_OFFSET}; 00639 double startingVel[3] = {SAFE_SPEED_MMpS, SAFE_SPEED_MMpS, SAFE_SPEED_MMpS}; 00640 SetFrontDemand(startingPos, startingVel); 00641 SetDriveDemand(HOME_DRIVE_POSITION, SAFE_CONTRACT_SPEED); 00642 wait(5); 00643 printf("Done.\r\n"); 00644 threadSendFeedback.start(SendFeedback); 00645 tickerSendFeedback.attach(&signalSendFeedback, 1/(float)SENSOR_FEEDBACK_HZ); 00646 while(1){ 00647 semReadHHC.wait(); 00648 00649 if(isZero){ 00650 GoToNeutralPose(SAFE_SPEED_MMpS); 00651 //reset front segment position 00652 dblAlpha = 0.0; 00653 dblBeta = 0.0; 00654 isZero = 0; 00655 printf("Done\r\n"); 00656 } 00657 00658 if(isCycle){ 00659 GoToAnchorPose(SAFE_SPEED_MMpS); 00660 isCycle = 0; 00661 printf("Done\r\n"); 00662 } 00663 if(isDebug){ 00664 DebugMode(); 00665 } 00666 00667 if(!isSteerLock){ 00668 //read Joystick sensors 00669 double dblJSX = pinJSX.read(); 00670 double dblJSY = pinJSY.read(); 00671 00672 dblJSX = DeadZone(dblJSX, 0.07, 0.85, 0.15); 00673 dblJSY = DeadZone(dblJSY, 0.07, 0.85, 0.15); 00674 00675 if(pinWashIn.read()){ 00676 pinWashOut = 0; 00677 }else{ 00678 pinWashOut = 1; 00679 } 00680 00681 //convert to angular velocity for front segment 00682 00683 //Find changes in angle 00684 dblDeltaAlpha = dblJSX * x_axis_sign * dblSensitivity * MAX_STEER_SPEED_RADpS / HHC_READ_FREQ_HZ; 00685 dblDeltaBeta = dblJSY*dblSensitivity*MAX_STEER_SPEED_RADpS/HHC_READ_FREQ_HZ; 00686 //convert to speeds in [rad/s] 00687 dblAlphaDot = dblDeltaAlpha*HHC_READ_FREQ_HZ; 00688 dblBetaDot = dblDeltaBeta*HHC_READ_FREQ_HZ; 00689 00690 //calculate required velocity based on current possition and new inputs. 00691 dblPhi = atan2(dblBeta,dblAlpha); 00692 double dblPhiDot = dblAlpha * dblBetaDot - dblBeta * dblAlphaDot; 00693 dblPhiDot /= (dblAlpha * dblAlpha + dblBeta*dblBeta); 00694 if( fabs( cos(dblPhi) ) >= 0.7106781 ){ 00695 dblTheta = dblAlpha / cos(dblPhi); 00696 double dblThetaDot = dblAlphaDot * cos(dblPhi) + dblAlpha * dblPhiDot * sin(dblPhi); 00697 dblThetaDot /= cos(dblPhi)*cos(dblPhi); 00698 double dblPsi[3]; 00699 for (int ii =0; ii<3; ii++){ 00700 dblPsi[ii] = dblPhi - ii*2*PI/3; 00701 if(dblBeta == 0){ 00702 dblXdot[ii] = ACT_CONV * K_theta*( cos(dblPsi[ii]) + d_theta); 00703 dblXdot[ii] *= ( dblAlphaDot - dblBetaDot * sin(dblPsi[ii]) ) / cos(dblPhi); 00704 } 00705 else{ 00706 dblXdot[ii] = ACT_CONV * K_theta * ( ( cos(dblPsi[ii]) + d_theta) * dblThetaDot - dblTheta * sin(dblPsi[ii]) * dblPhiDot); 00707 } 00708 } 00709 } 00710 else { 00711 dblTheta = dblBeta / sin(dblPhi); 00712 double dblThetaDot = dblBetaDot * sin(dblPhi) + dblBeta * dblPhiDot * cos(dblPhi); 00713 dblThetaDot /= sin(dblPhi)*sin(dblPhi); 00714 double dblPsi[3]; 00715 for (int ii =0; ii<3; ii++){ 00716 dblPsi[ii] = dblPhi - ii*2*PI/3; 00717 if(dblAlpha == 0){ 00718 dblXdot[ii] = ACT_CONV * K_theta * ( cos(dblPsi[ii]) + d_theta); 00719 dblXdot[ii] *= ( dblBetaDot - dblAlphaDot * sin(dblPsi[ii]) ) / sin(dblPhi); 00720 } 00721 else{ 00722 dblXdot[ii] = ACT_CONV * K_theta * ( ( cos(dblPsi[ii]) + d_theta ) * dblThetaDot - dblTheta * sin(dblPsi[ii]) * dblPhiDot); 00723 } 00724 } 00725 } 00726 //calculate new angles for next front segment position 00727 dblAlpha += dblDeltaAlpha; // update new alpha postion 00728 dblBeta += dblDeltaBeta; // update new beta position 00729 00730 dblAlpha = limDouble(dblAlpha, (double)-1.0*MAX_THETA_RAD, (double)MAX_THETA_RAD); 00731 dblBeta = limDouble(dblBeta, (double)-1.0*MAX_THETA_RAD, (double)MAX_THETA_RAD); 00732 dblPhi = atan2(dblBeta,dblAlpha); // update new phi position 00733 if( fabs( cos(dblPhi) ) >= 0.7106781 ){ 00734 dblTheta = dblAlpha / cos(dblPhi);//calculate new theta value 00735 } 00736 else { 00737 dblTheta = dblBeta / sin(dblPhi); 00738 } 00739 dblTheta = limDouble(dblTheta, 0.0, (double)MAX_THETA_RAD ); //limit angle 00740 //update alpha and beta if limit has occured 00741 dblAlpha = dblTheta *cos(dblPhi); 00742 dblBeta = dblTheta *sin(dblPhi); 00743 00744 double dblPsi[3]; 00745 for (int ii = 0; ii <3; ii++) {//calculate new acuator positions 00746 dblPsi[ii] = dblPhi - ii*2*PI/3; 00747 dblX[ii] = ACT_CONV * K_theta * ( cos(dblPsi[ii]) + d_theta) * dblTheta; 00748 //add offset 00749 dblX[ii] += ACTUATOR_OFFSET; 00750 00751 //limit positions 00752 dblX[ii] = limDouble( dblX[ii], 0.0, (double)MAX_ACTUATOR_LIMIT_MM ); 00753 } 00754 //calculate new drive segment position 00755 //read drive segment buttons 00756 //double dblDriveStep = 0.4; 00757 00758 //printf("pin:%d\t", pinFwd.read()); 00759 00760 if(pinFwd.read()){ 00761 if(!pinRev.read()){ 00762 dblSdot = SAFE_CONTRACT_SPEED; 00763 } else{ 00764 dblSdot = 0.0; 00765 } 00766 } else { 00767 if(pinRev.read()){ 00768 dblSdot = -1.0*SAFE_CONTRACT_SPEED; 00769 } else{ 00770 dblSdot = 0.0; 00771 } 00772 } 00773 00774 dblS += dblSdot/HHC_READ_FREQ_HZ; 00775 //limit extension (uses word position, but actually refers to pressure) 00776 dblS = limDouble( dblS, 0.0, (double)MAX_EXTENSION_PRESSURE ); 00777 00778 // printf("S:%f\tSdot:%f\t", dblS, dblSdot); 00779 //Send signals to actuators. 00780 SetFrontDemand(dblX, dblXdot);//front segment 00781 SetDriveDemand(dblS, dblSdot);//drive segment 00782 00783 00784 } 00785 } 00786 } 00787 00788 void SetUp(){ 00789 for(int ii = 0; ii<N_CHANNELS; ii++){ 00790 pinPosOut[ii].write(0.0); 00791 pinVelOut[ii].write(0.0); 00792 } 00793 } 00794 00795 int main() { 00796 pinThirdPump = 1; 00797 pinJetOut = 1; 00798 pinWashOut = 1; 00799 pc.baud(BAUD_RATE); 00800 for(int ii = 0; ii<N_CHANNELS; ii++){ 00801 pinPosOut[ii].period_ms(2.0); 00802 pinVelOut[ii].period_ms(2.0); 00803 } 00804 wait(1.0); 00805 printf("ML engage. Compiled at %s\r\n",__TIME__); 00806 00807 00808 //set up support function interrupts 00809 //Rising 00810 00811 pinZeroIn.mode(PullNone); 00812 pinCycleIn.mode(PullNone); 00813 pinWashIn.mode(PullNone); 00814 pinGasIn.mode(PullNone); 00815 pinSucIn.mode(PullNone); 00816 pinFwd.mode(PullNone); 00817 pinRev.mode(PullNone); 00818 pinFlushIn.mode(PullDown); 00819 pinDebugIn.mode(PullDown); 00820 00821 pinZeroIn.rise(&ZeroActivate); 00822 pinCycleIn.rise(&CycleActivate); 00823 pinFlushIn.rise(&FlushActivate); 00824 pinSucIn.rise(&SucActivate); 00825 pinGasIn.rise(&GasActivate); 00826 pinDebugIn.rise(&DebugActivate); 00827 //falling 00828 pinZeroIn.fall(&ZeroStop); 00829 pinCycleIn.fall(&CycleStop); 00830 pinFlushIn.fall(&FlushStop); 00831 pinSucIn.fall(&SucStop); 00832 pinGasIn.fall(&GasStop); 00833 pinDebugIn.fall(&DebugStop); 00834 threadReadHHC.start(ReadHHC); 00835 00836 00837 tickerReadHHC.attach(&signalReadHHC, 1/(float)HHC_READ_FREQ_HZ); // set up 00838 00839 00840 while(1) { 00841 Thread::wait(osWaitForever); 00842 } 00843 }
Generated on Wed Dec 6 2023 18:04:00 by
1.7.2