Jean Mercier
/
jmMotor
Control up to 4 DC Motors from a serial port
jmMotor.c@0:dedab08b24ea, 2011-02-12 (annotated)
- Committer:
- jm
- Date:
- Sat Feb 12 16:39:20 2011 +0000
- Revision:
- 0:dedab08b24ea
jmMotor Command Line Interface Module
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jm | 0:dedab08b24ea | 1 | /************************************************************************* |
jm | 0:dedab08b24ea | 2 | * @file jmMotor.c |
jm | 0:dedab08b24ea | 3 | * @version 1.0 |
jm | 0:dedab08b24ea | 4 | * @date Feb 12, 2011 |
jm | 0:dedab08b24ea | 5 | */ |
jm | 0:dedab08b24ea | 6 | |
jm | 0:dedab08b24ea | 7 | #include "jmMotor.h" |
jm | 0:dedab08b24ea | 8 | #include "LPC17xx.h" |
jm | 0:dedab08b24ea | 9 | #include "main.h" |
jm | 0:dedab08b24ea | 10 | #include "jmCommands.h" |
jm | 0:dedab08b24ea | 11 | #include "jmMessages.h" |
jm | 0:dedab08b24ea | 12 | #include "jmRingBuffer.h" |
jm | 0:dedab08b24ea | 13 | #include "stdio.h" |
jm | 0:dedab08b24ea | 14 | |
jm | 0:dedab08b24ea | 15 | #ifndef nonStop |
jm | 0:dedab08b24ea | 16 | #define nonStop 65535 // for continuous mode operation |
jm | 0:dedab08b24ea | 17 | #endif |
jm | 0:dedab08b24ea | 18 | |
jm | 0:dedab08b24ea | 19 | #define mtNotInit 0 |
jm | 0:dedab08b24ea | 20 | #define mtRunningCW 1 |
jm | 0:dedab08b24ea | 21 | #define mtRunningCCW 2 |
jm | 0:dedab08b24ea | 22 | #define mtStopped 3 |
jm | 0:dedab08b24ea | 23 | |
jm | 0:dedab08b24ea | 24 | extern LPC_GPIO_TypeDef *jmGPIO[5]; |
jm | 0:dedab08b24ea | 25 | |
jm | 0:dedab08b24ea | 26 | // Module Data Structure |
jm | 0:dedab08b24ea | 27 | struct StructMotor sMotor[motorQty]; // static creation |
jm | 0:dedab08b24ea | 28 | |
jm | 0:dedab08b24ea | 29 | /** Module Data Structure Initialization |
jm | 0:dedab08b24ea | 30 | * @brief All State Machines non initialized |
jm | 0:dedab08b24ea | 31 | * @param[in] none |
jm | 0:dedab08b24ea | 32 | * @returns none |
jm | 0:dedab08b24ea | 33 | */ |
jm | 0:dedab08b24ea | 34 | void MotorInit(void){ |
jm | 0:dedab08b24ea | 35 | int i; |
jm | 0:dedab08b24ea | 36 | for(i=0;i<motorQty;i++){ |
jm | 0:dedab08b24ea | 37 | sMotor[i].active = mtNotInit; |
jm | 0:dedab08b24ea | 38 | } |
jm | 0:dedab08b24ea | 39 | } |
jm | 0:dedab08b24ea | 40 | |
jm | 0:dedab08b24ea | 41 | /** Speed control for motors (1 or up to 4) |
jm | 0:dedab08b24ea | 42 | * @brief motorSpeed Command Line Interface. |
jm | 0:dedab08b24ea | 43 | * Format: command name (arg info)min..max values |
jm | 0:dedab08b24ea | 44 | * motorSpeed (Motor IDs ORed)1..15 (tOn Value)0..255 (tOff Value)0..255 |
jm | 0:dedab08b24ea | 45 | * @param[in] Extracted from command line (Motor IDs ORed)1..15 (tOn Value)0..255 (tOff Value)0..255 |
jm | 0:dedab08b24ea | 46 | * @returns Message: motorSpeed ids tOn tOff |
jm | 0:dedab08b24ea | 47 | */ |
jm | 0:dedab08b24ea | 48 | void cli_MotorSpeed(void){ |
jm | 0:dedab08b24ea | 49 | unsigned int id, i, tOn,tOff; |
jm | 0:dedab08b24ea | 50 | if(ExtractUInteger(pLine,&id,1,15)){ //extract id |
jm | 0:dedab08b24ea | 51 | if(ExtractUInteger(pLine,&tOn,0,255)){ //extract tOn |
jm | 0:dedab08b24ea | 52 | if(ExtractUInteger(pLine,&tOff,0,255)){ //extract tOff |
jm | 0:dedab08b24ea | 53 | for(i=0;i<motorQty;i++){ // check each motor |
jm | 0:dedab08b24ea | 54 | if(id & 1<<i){ // motor included ? |
jm | 0:dedab08b24ea | 55 | if(sMotor[i].active){ // motor initialized ? |
jm | 0:dedab08b24ea | 56 | sMotor[i].tOn=(uint8_t)tOn; |
jm | 0:dedab08b24ea | 57 | sMotor[i].tOff=(uint8_t)tOff; |
jm | 0:dedab08b24ea | 58 | |
jm | 0:dedab08b24ea | 59 | |
jm | 0:dedab08b24ea | 60 | if(tOn==0){ // 0% ? |
jm | 0:dedab08b24ea | 61 | sMotor[i].cycles=0; |
jm | 0:dedab08b24ea | 62 | sMotor[i].drivePort->FIOPIN &= ~sMotor[i].driveBitValue; // output pin low |
jm | 0:dedab08b24ea | 63 | } |
jm | 0:dedab08b24ea | 64 | |
jm | 0:dedab08b24ea | 65 | if(tOff==0){ // 100% ? |
jm | 0:dedab08b24ea | 66 | sMotor[i].drivePort->FIOPIN |= sMotor[i].driveBitValue; // output pin high |
jm | 0:dedab08b24ea | 67 | } |
jm | 0:dedab08b24ea | 68 | |
jm | 0:dedab08b24ea | 69 | if(tOn!=0 && tOff!=0){ // PWM ? |
jm | 0:dedab08b24ea | 70 | // change motor tOn and tOff values |
jm | 0:dedab08b24ea | 71 | sMotor[i].cycles=nonStop; |
jm | 0:dedab08b24ea | 72 | } |
jm | 0:dedab08b24ea | 73 | // report to gui |
jm | 0:dedab08b24ea | 74 | rGPPMT(i); |
jm | 0:dedab08b24ea | 75 | } |
jm | 0:dedab08b24ea | 76 | } |
jm | 0:dedab08b24ea | 77 | } |
jm | 0:dedab08b24ea | 78 | } |
jm | 0:dedab08b24ea | 79 | } |
jm | 0:dedab08b24ea | 80 | |
jm | 0:dedab08b24ea | 81 | if(Feedback)printf("MotorSpeed ID %d tOn %d tOff %d\n",id,tOn, tOff); |
jm | 0:dedab08b24ea | 82 | return; |
jm | 0:dedab08b24ea | 83 | } |
jm | 0:dedab08b24ea | 84 | if(Help)printf("motorSpeed (Motor IDs ORed)1..15 (tOn Value)1..255 (tOff Value)1..255\n"); |
jm | 0:dedab08b24ea | 85 | // Ignore pending command line |
jm | 0:dedab08b24ea | 86 | NextCommand(nl,pLine); |
jm | 0:dedab08b24ea | 87 | } |
jm | 0:dedab08b24ea | 88 | |
jm | 0:dedab08b24ea | 89 | |
jm | 0:dedab08b24ea | 90 | /*********************************************************************** |
jm | 0:dedab08b24ea | 91 | * @brief Module State Machine. |
jm | 0:dedab08b24ea | 92 | * Enable different motor control concurrently. |
jm | 0:dedab08b24ea | 93 | * @param none |
jm | 0:dedab08b24ea | 94 | * @returns Message at end of cycles: GPPMT id dirPin drivePinB tOn tOff status |
jm | 0:dedab08b24ea | 95 | */ |
jm | 0:dedab08b24ea | 96 | void MotorSM(void){ |
jm | 0:dedab08b24ea | 97 | int i; |
jm | 0:dedab08b24ea | 98 | for(i=0;i<motorQty;i++){ // for each SM define by pins |
jm | 0:dedab08b24ea | 99 | // SM active ? |
jm | 0:dedab08b24ea | 100 | if(!sMotor[i].active) continue; |
jm | 0:dedab08b24ea | 101 | |
jm | 0:dedab08b24ea | 102 | if(sMotor[i].cycles!=0){ // Cycles to DO ? |
jm | 0:dedab08b24ea | 103 | switch(sMotor[i].state){ |
jm | 0:dedab08b24ea | 104 | |
jm | 0:dedab08b24ea | 105 | // pulse is low |
jm | 0:dedab08b24ea | 106 | case 0: if(sMotor[i].eggTimer==0){ // time to change ? |
jm | 0:dedab08b24ea | 107 | sMotor[i].drivePort->FIOPIN |= sMotor[i].driveBitValue; // set pin High |
jm | 0:dedab08b24ea | 108 | sMotor[i].eggTimer=sMotor[i].tOn; // load timer with tOn |
jm | 0:dedab08b24ea | 109 | sMotor[i].state=1; // next state |
jm | 0:dedab08b24ea | 110 | } |
jm | 0:dedab08b24ea | 111 | break; |
jm | 0:dedab08b24ea | 112 | |
jm | 0:dedab08b24ea | 113 | // pulse is High |
jm | 0:dedab08b24ea | 114 | case 1: if(sMotor[i].eggTimer==0){ // time to change ? |
jm | 0:dedab08b24ea | 115 | sMotor[i].drivePort->FIOPIN &= ~sMotor[i].driveBitValue; // reset pin low |
jm | 0:dedab08b24ea | 116 | sMotor[i].eggTimer=sMotor[i].tOff; // load timer with tOff |
jm | 0:dedab08b24ea | 117 | if(sMotor[i].cycles != nonStop){ // continuous mode ? |
jm | 0:dedab08b24ea | 118 | if(--sMotor[i].cycles == 0) // decrease qty if not continuous mode |
jm | 0:dedab08b24ea | 119 | rGPPMT(i); // Message: GPPMT id dirPin drivePinB tOn tOff status |
jm | 0:dedab08b24ea | 120 | } |
jm | 0:dedab08b24ea | 121 | sMotor[i].state=0; // state 0 |
jm | 0:dedab08b24ea | 122 | } |
jm | 0:dedab08b24ea | 123 | break; |
jm | 0:dedab08b24ea | 124 | }//switch |
jm | 0:dedab08b24ea | 125 | }//if |
jm | 0:dedab08b24ea | 126 | }//for |
jm | 0:dedab08b24ea | 127 | }//MotorSM |
jm | 0:dedab08b24ea | 128 | |
jm | 0:dedab08b24ea | 129 | |
jm | 0:dedab08b24ea | 130 | /** @brief motor Command Line Interface |
jm | 0:dedab08b24ea | 131 | * Command Line Interface to control Motor on mbed pins DIP5 to 30. |
jm | 0:dedab08b24ea | 132 | * Format: command name (arg info)min..max values |
jm | 0:dedab08b24ea | 133 | * 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] |
jm | 0:dedab08b24ea | 134 | * @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] |
jm | 0:dedab08b24ea | 135 | * @returns Message: GPPMT id dirPin drivePinB tOn tOff status |
jm | 0:dedab08b24ea | 136 | */ |
jm | 0:dedab08b24ea | 137 | void cli_Motor(void){ |
jm | 0:dedab08b24ea | 138 | unsigned int id,dirPin,drivePin, dir, tOn,tOff,cycles; |
jm | 0:dedab08b24ea | 139 | |
jm | 0:dedab08b24ea | 140 | if(ExtractUInteger(pLine,&id,0,motorQty-1)){ // extract motor id |
jm | 0:dedab08b24ea | 141 | if(ExtractUInteger(pLine,&dirPin,0,432)){ // extract direction pin |
jm | 0:dedab08b24ea | 142 | if(ExtractUInteger(pLine,&drivePin,0,432)){ // extract drive pin |
jm | 0:dedab08b24ea | 143 | if(ExtractUInteger(pLine,&dir,0,1)){ // extract direction |
jm | 0:dedab08b24ea | 144 | if(ExtractUInteger(pLine,&tOn,0,255)){ // extract tOn |
jm | 0:dedab08b24ea | 145 | if(ExtractUInteger(pLine,&tOff,0,255)){ // extract tOff |
jm | 0:dedab08b24ea | 146 | // extract cycles and test for nonStop mode |
jm | 0:dedab08b24ea | 147 | if(!ExtractUInteger(pLine,&cycles,1,nonStop))cycles = nonStop; |
jm | 0:dedab08b24ea | 148 | |
jm | 0:dedab08b24ea | 149 | sMotor[id].dirBitValue = 1<<(dirPin%100); |
jm | 0:dedab08b24ea | 150 | sMotor[id].driveBitValue = 1<<(drivePin%100);; |
jm | 0:dedab08b24ea | 151 | sMotor[id].drivePort = jmGPIO[drivePin/100]; |
jm | 0:dedab08b24ea | 152 | sMotor[id].dirPort = jmGPIO[dirPin/100]; |
jm | 0:dedab08b24ea | 153 | sMotor[id].active = 1; |
jm | 0:dedab08b24ea | 154 | |
jm | 0:dedab08b24ea | 155 | // Port.Pin set to outputs |
jm | 0:dedab08b24ea | 156 | sMotor[id].dirPort->FIODIR |= sMotor[id].dirBitValue; |
jm | 0:dedab08b24ea | 157 | sMotor[id].drivePort->FIODIR |= sMotor[id].driveBitValue; |
jm | 0:dedab08b24ea | 158 | |
jm | 0:dedab08b24ea | 159 | sMotor[id].direction = dir; |
jm | 0:dedab08b24ea | 160 | sMotor[id].tOn = (uint8_t)tOn; |
jm | 0:dedab08b24ea | 161 | sMotor[id].tOff = (uint8_t)tOff; |
jm | 0:dedab08b24ea | 162 | sMotor[id].cycles = (uint16_t)cycles; |
jm | 0:dedab08b24ea | 163 | sMotor[id].state = 0; |
jm | 0:dedab08b24ea | 164 | sMotor[id].eggTimer = 0; |
jm | 0:dedab08b24ea | 165 | sMotor[id].dirPin = dirPin; |
jm | 0:dedab08b24ea | 166 | sMotor[id].drivePin = drivePin; |
jm | 0:dedab08b24ea | 167 | |
jm | 0:dedab08b24ea | 168 | // set output direction bit according to given direction |
jm | 0:dedab08b24ea | 169 | if(dir) sMotor[id].dirPort->FIOPIN |= sMotor[id].dirBitValue; |
jm | 0:dedab08b24ea | 170 | else sMotor[id].dirPort->FIOPIN &= ~sMotor[id].dirBitValue; |
jm | 0:dedab08b24ea | 171 | |
jm | 0:dedab08b24ea | 172 | // 100% |
jm | 0:dedab08b24ea | 173 | if(tOff==0){ |
jm | 0:dedab08b24ea | 174 | sMotor[id].drivePort->FIOPIN |= sMotor[id].driveBitValue; |
jm | 0:dedab08b24ea | 175 | // SM off |
jm | 0:dedab08b24ea | 176 | sMotor[id].cycles = 0; |
jm | 0:dedab08b24ea | 177 | } |
jm | 0:dedab08b24ea | 178 | |
jm | 0:dedab08b24ea | 179 | // 0% |
jm | 0:dedab08b24ea | 180 | if(tOn==0){ |
jm | 0:dedab08b24ea | 181 | sMotor[id].drivePort->FIOPIN &= ~sMotor[id].driveBitValue; |
jm | 0:dedab08b24ea | 182 | // SM off |
jm | 0:dedab08b24ea | 183 | sMotor[id].cycles = 0; |
jm | 0:dedab08b24ea | 184 | } |
jm | 0:dedab08b24ea | 185 | |
jm | 0:dedab08b24ea | 186 | rGPPMT(id); // Message: GPPMT id dirPin drivePinB tOn tOff status |
jm | 0:dedab08b24ea | 187 | |
jm | 0:dedab08b24ea | 188 | if(Feedback)printf("Motor %d DirPin %d DrivePin %d Dir %d tOn %d tOff %d Cycles %d\n",id, |
jm | 0:dedab08b24ea | 189 | dirPin, drivePin, dir,tOn,tOff,cycles); |
jm | 0:dedab08b24ea | 190 | return; |
jm | 0:dedab08b24ea | 191 | } |
jm | 0:dedab08b24ea | 192 | } |
jm | 0:dedab08b24ea | 193 | } |
jm | 0:dedab08b24ea | 194 | } |
jm | 0:dedab08b24ea | 195 | } |
jm | 0:dedab08b24ea | 196 | // Ignore pending command line |
jm | 0:dedab08b24ea | 197 | NextCommand(nl,pLine); |
jm | 0:dedab08b24ea | 198 | return; |
jm | 0:dedab08b24ea | 199 | } |
jm | 0:dedab08b24ea | 200 | |
jm | 0:dedab08b24ea | 201 | 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); |
jm | 0:dedab08b24ea | 202 | // Ignore pending command line |
jm | 0:dedab08b24ea | 203 | NextCommand(nl,pLine); |
jm | 0:dedab08b24ea | 204 | } |
jm | 0:dedab08b24ea | 205 | |
jm | 0:dedab08b24ea | 206 | /** Module Get Module Process Properties Command Line Interface |
jm | 0:dedab08b24ea | 207 | * @brief Command Line Interface to Get Module Public Process Properties |
jm | 0:dedab08b24ea | 208 | * @param[in] id Extracted from command line |
jm | 0:dedab08b24ea | 209 | * @returns Message: GPPMT id dirPin drivePinB tOn tOff status |
jm | 0:dedab08b24ea | 210 | */ |
jm | 0:dedab08b24ea | 211 | void cli_GPPMT(void) |
jm | 0:dedab08b24ea | 212 | { unsigned int id; |
jm | 0:dedab08b24ea | 213 | if(ExtractUInteger(pLine,&id,0,motorQty-1)){ // extract id |
jm | 0:dedab08b24ea | 214 | rGPPMT(id); // Message: GPPMT id dirPin drivePinB tOn tOff status |
jm | 0:dedab08b24ea | 215 | return; |
jm | 0:dedab08b24ea | 216 | } |
jm | 0:dedab08b24ea | 217 | |
jm | 0:dedab08b24ea | 218 | if(Help)printf("GPPST (Motor id)0..%d\n",motorQty-1); |
jm | 0:dedab08b24ea | 219 | // Ignore pending command line |
jm | 0:dedab08b24ea | 220 | NextCommand(nl,pLine); |
jm | 0:dedab08b24ea | 221 | } |
jm | 0:dedab08b24ea | 222 | |
jm | 0:dedab08b24ea | 223 | /** Public Properties Message |
jm | 0:dedab08b24ea | 224 | * @brief Send Process Properties to update GUI |
jm | 0:dedab08b24ea | 225 | * @param[in] id Process identification |
jm | 0:dedab08b24ea | 226 | * @returns Message: GPPMT id dirPin drivePinB tOn tOff status |
jm | 0:dedab08b24ea | 227 | */ |
jm | 0:dedab08b24ea | 228 | void rGPPMT(unsigned int id ){ |
jm | 0:dedab08b24ea | 229 | int status; |
jm | 0:dedab08b24ea | 230 | if( sMotor[id].active ) |
jm | 0:dedab08b24ea | 231 | { if(sMotor[id].cycles == 0)status = mtStopped; |
jm | 0:dedab08b24ea | 232 | else |
jm | 0:dedab08b24ea | 233 | { |
jm | 0:dedab08b24ea | 234 | if( sMotor[id].direction) status = mtRunningCW; |
jm | 0:dedab08b24ea | 235 | else status = mtRunningCCW; |
jm | 0:dedab08b24ea | 236 | } |
jm | 0:dedab08b24ea | 237 | } |
jm | 0:dedab08b24ea | 238 | else status = mtNotInit; |
jm | 0:dedab08b24ea | 239 | printf("GPPMT %d %d %d %d %d %d\n",id,sMotor[id].dirPin,sMotor[id].drivePin,sMotor[id].tOn,sMotor[id].tOff,status); |
jm | 0:dedab08b24ea | 240 | } |