Basic Mid-Level control for the rebuilt MorphGI control unit, using PWM to communicate with the low level controllers.

Dependencies:   ros_lib_kinetic

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

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 }