![](/media/cache/profiles/0f0ee3310223fe38a989b2c818709393.50x50_q85.jpg)
Control up to 4 DC Motors from a serial port
Diff: jmMotor.c
- Revision:
- 0:dedab08b24ea
diff -r 000000000000 -r dedab08b24ea jmMotor.c --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/jmMotor.c Sat Feb 12 16:39:20 2011 +0000 @@ -0,0 +1,240 @@ +/************************************************************************* + * @file jmMotor.c + * @version 1.0 + * @date Feb 12, 2011 + */ + +#include "jmMotor.h" +#include "LPC17xx.h" +#include "main.h" +#include "jmCommands.h" +#include "jmMessages.h" +#include "jmRingBuffer.h" +#include "stdio.h" + +#ifndef nonStop + #define nonStop 65535 // for continuous mode operation +#endif + +#define mtNotInit 0 +#define mtRunningCW 1 +#define mtRunningCCW 2 +#define mtStopped 3 + +extern LPC_GPIO_TypeDef *jmGPIO[5]; + +// Module Data Structure +struct StructMotor sMotor[motorQty]; // static creation + +/** Module Data Structure Initialization + * @brief All State Machines non initialized + * @param[in] none + * @returns none + */ +void MotorInit(void){ + int i; + for(i=0;i<motorQty;i++){ + sMotor[i].active = mtNotInit; + } +} + +/** Speed control for motors (1 or up to 4) + * @brief motorSpeed Command Line Interface. + * Format: command name (arg info)min..max values + * motorSpeed (Motor IDs ORed)1..15 (tOn Value)0..255 (tOff Value)0..255 + * @param[in] Extracted from command line (Motor IDs ORed)1..15 (tOn Value)0..255 (tOff Value)0..255 + * @returns Message: motorSpeed ids tOn tOff + */ +void cli_MotorSpeed(void){ + unsigned int id, i, tOn,tOff; + if(ExtractUInteger(pLine,&id,1,15)){ //extract id + if(ExtractUInteger(pLine,&tOn,0,255)){ //extract tOn + if(ExtractUInteger(pLine,&tOff,0,255)){ //extract tOff + for(i=0;i<motorQty;i++){ // check each motor + if(id & 1<<i){ // motor included ? + if(sMotor[i].active){ // motor initialized ? + sMotor[i].tOn=(uint8_t)tOn; + sMotor[i].tOff=(uint8_t)tOff; + + + if(tOn==0){ // 0% ? + sMotor[i].cycles=0; + sMotor[i].drivePort->FIOPIN &= ~sMotor[i].driveBitValue; // output pin low + } + + if(tOff==0){ // 100% ? + sMotor[i].drivePort->FIOPIN |= sMotor[i].driveBitValue; // output pin high + } + + if(tOn!=0 && tOff!=0){ // PWM ? + // change motor tOn and tOff values + sMotor[i].cycles=nonStop; + } + // report to gui + rGPPMT(i); + } + } + } + } + } + + if(Feedback)printf("MotorSpeed ID %d tOn %d tOff %d\n",id,tOn, tOff); + return; + } + if(Help)printf("motorSpeed (Motor IDs ORed)1..15 (tOn Value)1..255 (tOff Value)1..255\n"); + // Ignore pending command line + NextCommand(nl,pLine); +} + + +/*********************************************************************** + * @brief Module State Machine. + * Enable different motor control concurrently. + * @param none + * @returns Message at end of cycles: GPPMT id dirPin drivePinB tOn tOff status + */ +void MotorSM(void){ + int i; + for(i=0;i<motorQty;i++){ // for each SM define by pins + // SM active ? + if(!sMotor[i].active) continue; + + if(sMotor[i].cycles!=0){ // Cycles to DO ? + switch(sMotor[i].state){ + + // pulse is low + case 0: if(sMotor[i].eggTimer==0){ // time to change ? + sMotor[i].drivePort->FIOPIN |= sMotor[i].driveBitValue; // set pin High + sMotor[i].eggTimer=sMotor[i].tOn; // load timer with tOn + sMotor[i].state=1; // next state + } + break; + + // pulse is High + case 1: if(sMotor[i].eggTimer==0){ // time to change ? + sMotor[i].drivePort->FIOPIN &= ~sMotor[i].driveBitValue; // reset pin low + sMotor[i].eggTimer=sMotor[i].tOff; // load timer with tOff + if(sMotor[i].cycles != nonStop){ // continuous mode ? + if(--sMotor[i].cycles == 0) // decrease qty if not continuous mode + rGPPMT(i); // Message: GPPMT id dirPin drivePinB tOn tOff status + } + sMotor[i].state=0; // state 0 + } + break; + }//switch + }//if + }//for +}//MotorSM + + +/** @brief motor Command Line Interface + * Command Line Interface to control Motor on mbed pins DIP5 to 30. + * Format: command name (arg info)min..max values + * 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] + * @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] + * @returns Message: GPPMT id dirPin drivePinB tOn tOff status + */ +void cli_Motor(void){ + unsigned int id,dirPin,drivePin, dir, tOn,tOff,cycles; + + if(ExtractUInteger(pLine,&id,0,motorQty-1)){ // extract motor id + if(ExtractUInteger(pLine,&dirPin,0,432)){ // extract direction pin + if(ExtractUInteger(pLine,&drivePin,0,432)){ // extract drive pin + if(ExtractUInteger(pLine,&dir,0,1)){ // extract direction + if(ExtractUInteger(pLine,&tOn,0,255)){ // extract tOn + if(ExtractUInteger(pLine,&tOff,0,255)){ // extract tOff + // extract cycles and test for nonStop mode + if(!ExtractUInteger(pLine,&cycles,1,nonStop))cycles = nonStop; + + sMotor[id].dirBitValue = 1<<(dirPin%100); + sMotor[id].driveBitValue = 1<<(drivePin%100);; + sMotor[id].drivePort = jmGPIO[drivePin/100]; + sMotor[id].dirPort = jmGPIO[dirPin/100]; + sMotor[id].active = 1; + + // Port.Pin set to outputs + sMotor[id].dirPort->FIODIR |= sMotor[id].dirBitValue; + sMotor[id].drivePort->FIODIR |= sMotor[id].driveBitValue; + + sMotor[id].direction = dir; + sMotor[id].tOn = (uint8_t)tOn; + sMotor[id].tOff = (uint8_t)tOff; + sMotor[id].cycles = (uint16_t)cycles; + sMotor[id].state = 0; + sMotor[id].eggTimer = 0; + sMotor[id].dirPin = dirPin; + sMotor[id].drivePin = drivePin; + + // set output direction bit according to given direction + if(dir) sMotor[id].dirPort->FIOPIN |= sMotor[id].dirBitValue; + else sMotor[id].dirPort->FIOPIN &= ~sMotor[id].dirBitValue; + + // 100% + if(tOff==0){ + sMotor[id].drivePort->FIOPIN |= sMotor[id].driveBitValue; + // SM off + sMotor[id].cycles = 0; + } + + // 0% + if(tOn==0){ + sMotor[id].drivePort->FIOPIN &= ~sMotor[id].driveBitValue; + // SM off + sMotor[id].cycles = 0; + } + + rGPPMT(id); // Message: GPPMT id dirPin drivePinB tOn tOff status + + if(Feedback)printf("Motor %d DirPin %d DrivePin %d Dir %d tOn %d tOff %d Cycles %d\n",id, + dirPin, drivePin, dir,tOn,tOff,cycles); + return; + } + } + } + } + } + // Ignore pending command line + NextCommand(nl,pLine); + return; + } + + 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); + // Ignore pending command line + NextCommand(nl,pLine); +} + +/** Module Get Module Process Properties Command Line Interface + * @brief Command Line Interface to Get Module Public Process Properties + * @param[in] id Extracted from command line + * @returns Message: GPPMT id dirPin drivePinB tOn tOff status + */ +void cli_GPPMT(void) +{ unsigned int id; + if(ExtractUInteger(pLine,&id,0,motorQty-1)){ // extract id + rGPPMT(id); // Message: GPPMT id dirPin drivePinB tOn tOff status + return; + } + + if(Help)printf("GPPST (Motor id)0..%d\n",motorQty-1); + // Ignore pending command line + NextCommand(nl,pLine); +} + +/** Public Properties Message + * @brief Send Process Properties to update GUI + * @param[in] id Process identification + * @returns Message: GPPMT id dirPin drivePinB tOn tOff status + */ +void rGPPMT(unsigned int id ){ + int status; + if( sMotor[id].active ) + { if(sMotor[id].cycles == 0)status = mtStopped; + else + { + if( sMotor[id].direction) status = mtRunningCW; + else status = mtRunningCCW; + } + } + else status = mtNotInit; + printf("GPPMT %d %d %d %d %d %d\n",id,sMotor[id].dirPin,sMotor[id].drivePin,sMotor[id].tOn,sMotor[id].tOff,status); +}