jmMotor.c

00001 /*************************************************************************
00002  * @file    jmMotor.c                 
00003  * @version 1.0
00004  * @date    Feb 12, 2011
00005  */
00006 
00007 #include "jmMotor.h"
00008 #include "LPC17xx.h"
00009 #include "main.h"
00010 #include "jmCommands.h"
00011 #include "jmMessages.h"
00012 #include "jmRingBuffer.h"
00013 #include "stdio.h"
00014 
00015 #ifndef nonStop
00016   #define nonStop 65535  // for continuous mode operation
00017 #endif
00018 
00019 #define mtNotInit 0
00020 #define mtRunningCW 1
00021 #define mtRunningCCW 2
00022 #define mtStopped 3
00023 
00024 // Module Data Structure
00025 struct StructMotor sMotor[motorQty]; // static creation
00026 
00027 /** Module Data Structure Initialization
00028  * @brief   All State Machines non initialized
00029  * @param[in]    none
00030  * @returns none
00031  */
00032 void MotorInit(void){
00033    int i;
00034    for(i=0;i<motorQty;i++){
00035      sMotor[i].active = mtNotInit;
00036    }
00037 }
00038 
00039 /** Speed control for motors (1 or up to 4)
00040  * @brief motorSpeed Command Line Interface.
00041  * Format: command name (arg info)min..max values 
00042  * motorSpeed (Motor IDs ORed)1..15 (tOn Value)0..255 (tOff Value)0..255
00043  * @param[in] Extracted from command line (Motor IDs ORed)1..15 (tOn Value)0..255 (tOff Value)0..255 
00044  * @returns Message: motorSpeed ids tOn tOff
00045  */
00046 void cli_MotorSpeed(void){
00047    unsigned int id, i, tOn,tOff;
00048    if(ExtractUInteger(pLine,&id,1,15)){          //extract id
00049       if(ExtractUInteger(pLine,&tOn,0,255)){      //extract tOn
00050          if(ExtractUInteger(pLine,&tOff,0,255)){ //extract tOff
00051               for(i=0;i<motorQty;i++){              // check each motor
00052                   if(id & 1<<i){                 // motor included ?     
00053                       if(sMotor[i].active){         // motor initialized ?
00054                            sMotor[i].tOn=(uint8_t)tOn;
00055                            sMotor[i].tOff=(uint8_t)tOff;
00056 
00057                          
00058                          if(tOn==0){             // 0% ?
00059                              sMotor[i].cycles=0;            
00060                             sMotor[i].drivePort->FIOPIN &= ~sMotor[i].driveBitValue;    // output pin low 
00061                           }
00062     
00063                           if(tOff==0){              // 100% ?            
00064                             sMotor[i].drivePort->FIOPIN |= sMotor[i].driveBitValue;    // output pin high
00065                           }
00066     
00067                           if(tOn!=0 && tOff!=0){   // PWM ?
00068                              // change motor tOn and tOff values
00069                              sMotor[i].cycles=nonStop;
00070                           }
00071                           // report to gui
00072                           rGPPMT(i);
00073                       }    
00074                   }             
00075               }
00076           }
00077       }
00078 
00079       if(Feedback)printf("MotorSpeed ID %d tOn %d tOff %d\n",id,tOn, tOff);
00080       return;
00081    }
00082    if(Help)printf("motorSpeed (Motor IDs ORed)1..15 (tOn Value)1..255 (tOff Value)1..255\n");
00083    // Ignore pending command line
00084    NextCommand(nl,pLine);
00085 }
00086 
00087 
00088 /***********************************************************************
00089  * @brief    Module State Machine.  
00090  * Enable different motor control concurrently.
00091  * @param none
00092  * @returns Message at end of cycles: GPPMT id dirPin drivePinB tOn tOff status
00093  */ 
00094 void MotorSM(void){
00095   int i;
00096    for(i=0;i<motorQty;i++){ // for each SM define by pins
00097      // SM active ?
00098      if(!sMotor[i].active) continue;
00099    
00100      if(sMotor[i].cycles!=0){  // Cycles to DO ?
00101        switch(sMotor[i].state){       
00102 
00103          // pulse is low
00104          case  0: if(sMotor[i].eggTimer==0){             // time to change ?     
00105                      sMotor[i].drivePort->FIOPIN |= sMotor[i].driveBitValue;  // set pin High         
00106                      sMotor[i].eggTimer=sMotor[i].tOn;   // load timer with tOn   
00107                      sMotor[i].state=1;                  // next state 
00108                   }
00109                   break;
00110 
00111          // pulse is High
00112          case  1: if(sMotor[i].eggTimer==0){                    // time to change ?
00113                      sMotor[i].drivePort->FIOPIN &= ~sMotor[i].driveBitValue;    // reset pin low      
00114                      sMotor[i].eggTimer=sMotor[i].tOff;  // load timer with tOff  
00115                      if(sMotor[i].cycles != nonStop){    // continuous mode ?
00116                         if(--sMotor[i].cycles == 0)      // decrease qty if not continuous mode
00117                            rGPPMT(i);   // Message: GPPMT id dirPin drivePinB tOn tOff status
00118                      }
00119                      sMotor[i].state=0;                  // state 0
00120                   }
00121                   break;
00122          }//switch
00123        }//if
00124   }//for
00125 }//MotorSM
00126 
00127 
00128 /** @brief motor Command Line Interface
00129  * Command Line Interface to control Motor on mbed pins DIP5 to 30.
00130  * Format: command name (arg info)min..max values 
00131  * Command Line Format: motor (Motor ID)0..3 (Direction Pin)0..432 (Drive Pin)0..432 (Direction)0..1 (tOn)0..255 (tOff)0..255 (Cycles)[1..65535]
00132  * @param[in]    Extracted from command line ((Motor ID)0..3 (Direction Pin)0..432 (Drive Pin)0..432 (Direction)0..1 (tOn)1..255 (tOff)1..255 (Cycles)[1..65535]
00133  * @returns    Message: GPPMT id dirPin drivePinB tOn tOff status
00134  */
00135 void cli_Motor(void){
00136    unsigned int id,dirPin,drivePin, dir, tOn,tOff,cycles;
00137 
00138    if(ExtractUInteger(pLine,&id,0,motorQty-1)){         // extract motor id
00139       if(ExtractUInteger(pLine,&dirPin,0,432)){            // extract direction    pin
00140          if(ExtractUInteger(pLine,&drivePin,0,432)){       // extract drive pin
00141             if(ExtractUInteger(pLine,&dir,0,1)){            // extract direction
00142                 if(ExtractUInteger(pLine,&tOn,0,255)){       // extract tOn
00143                    if(ExtractUInteger(pLine,&tOff,0,255)){   // extract tOff
00144                        // extract cycles and test for nonStop mode
00145                        if(!ExtractUInteger(pLine,&cycles,1,nonStop))cycles = nonStop;
00146     
00147                        sMotor[id].dirBitValue = 1<<(dirPin%100);
00148                        sMotor[id].driveBitValue = 1<<(drivePin%100);;
00149                        sMotor[id].drivePort = jmGPIO[drivePin/100];
00150                        sMotor[id].dirPort = jmGPIO[dirPin/100];
00151                        sMotor[id].active = 1;
00152             
00153                        // Port.Pin set to outputs
00154                        sMotor[id].dirPort->FIODIR |= sMotor[id].dirBitValue;
00155                        sMotor[id].drivePort->FIODIR |= sMotor[id].driveBitValue;
00156         
00157                        sMotor[id].direction = dir;
00158                        sMotor[id].tOn = (uint8_t)tOn;
00159                        sMotor[id].tOff = (uint8_t)tOff;
00160                        sMotor[id].cycles = (uint16_t)cycles;
00161                        sMotor[id].state = 0;
00162                        sMotor[id].eggTimer = 0;
00163                        sMotor[id].dirPin = dirPin;
00164                        sMotor[id].drivePin = drivePin;
00165     
00166                       // set output direction bit according to given direction
00167                       if(dir) sMotor[id].dirPort->FIOPIN |=    sMotor[id].dirBitValue;
00168                       else sMotor[id].dirPort->FIOPIN &=    ~sMotor[id].dirBitValue;
00169     
00170                       // 100%
00171                       if(tOff==0){
00172                          sMotor[id].drivePort->FIOPIN |= sMotor[id].driveBitValue;
00173                          // SM off
00174                          sMotor[id].cycles = 0;
00175                       }
00176     
00177                       // 0%
00178                       if(tOn==0){
00179                          sMotor[id].drivePort->FIOPIN &= ~sMotor[id].driveBitValue;
00180                          // SM off
00181                          sMotor[id].cycles = 0;
00182                       }
00183                       
00184                       rGPPMT(id);   // Message: GPPMT id dirPin drivePinB tOn tOff status
00185         
00186                       if(Feedback)printf("Motor %d DirPin %d DrivePin %d Dir %d tOn %d tOff %d Cycles %d\n",id,
00187                                          dirPin, drivePin, dir,tOn,tOff,cycles); 
00188                        return;
00189                 }
00190              }
00191           }
00192        }
00193        }
00194     // Ignore pending command line
00195        NextCommand(nl,pLine);
00196     return;
00197    }
00198 
00199    if(Help)printf("motor (Motor ID)0..%d (Direction Pin)0..432 (Drive Pin)0..432 (Direction)0..1 (tOn)0..255 (tOff)0..255 (Cycles)[1..65535]\n",motorQty-1);  
00200    // Ignore pending command line
00201    NextCommand(nl,pLine);
00202 }
00203 
00204 /** Module Get Module Process Properties Command Line Interface
00205  * @brief        Command Line Interface to Get Module Public Process Properties
00206  * @param[in]    id Extracted from command line
00207  * @returns    Message: GPPMT id dirPin drivePinB tOn tOff status
00208  */
00209 void cli_GPPMT(void)
00210 {    unsigned int id;
00211    if(ExtractUInteger(pLine,&id,0,motorQty-1)){  // extract id
00212        rGPPMT(id);   // Message: GPPMT id dirPin drivePinB tOn tOff status
00213        return;
00214     }
00215 
00216     if(Help)printf("GPPST (Motor id)0..%d\n",motorQty-1);  
00217     // Ignore pending command line
00218     NextCommand(nl,pLine);
00219 }
00220 
00221 /** Public Properties Message
00222  * @brief    Send Process Properties to update GUI
00223  * @param[in] id Process identification
00224  * @returns   Message: GPPMT id dirPin drivePinB tOn tOff status
00225  */
00226 void rGPPMT(unsigned int id ){
00227     int status;
00228     if( sMotor[id].active )
00229     {   if(sMotor[id].cycles == 0)status = mtStopped;
00230         else 
00231         {
00232          if( sMotor[id].direction) status = mtRunningCW;
00233          else status = mtRunningCCW;
00234         }
00235     }
00236     else  status = mtNotInit;                     
00237     printf("GPPMT %d %d %d %d %d %d\n",id,sMotor[id].dirPin,sMotor[id].drivePin,sMotor[id].tOn,sMotor[id].tOff,status);
00238 }