/********************************* espdwarf.c ******************************* * $Source: /home/cvs/ESP/gen2/software/msp430/dwarves/espdwarf.c,v $ * Copyright (C) 2003 MBARI * * MBARI Proprietary Information. All rights reserved. * $Id: espdwarf.c,v 1.88 2004/12/06 17:54:42 alana Exp $ * * Mainline initialization and background loop for the servo control * dwarf microcontroller of the gen2 ESP. * *****************************************************************************/ static const char welcome[] = "ESP2 dwarf v0.9h"; //#define I2CtraceDepth 16 // Clock rates #define CPURATE 4000000 //target CPU clock in hz (may be adjusted) #define BAUDRATE 38400 //desired USART baud rate #define I2CRATE 100000 //standard I2C bit rate #define DCOUPRATE 1*TIMERHZ //adjust the DCO every second #include "dwarftop.h" //must be the first file that includes dwarf.h #include "string.h" //for memcpy #include "stdio.h" //just for sprintf #include "stdarg.h" //for reportErr's var args #include "espdwarf.h" //common definitions for timers, debug, etc. #include "I2Cnode.h" //parse messages from host #include "TrapServo.h" #include "heater.h" #include "rvalve.h" #include "debugPort.h" #include "dwarfcore.h" #include "cpld.h" #include "adc.h" #include "servobits.h" #include "jogServo.h" #include "solenoid.h" #include "gripper.h" #include "espmsg.h" //message formats #define validTag(tag) ((unsigned)((tag)-1) < (unsigned)0x7f) #define abs(a) (((a)>=0)?(a):(-a)) // RS232 serial FIFO sizes and Handshaking thresholds static FIFO (toHost, 61); //bytes bound for host machine static FIFO (fromHost, 181); //bytes received from host machine #define CTSstop 150 //cancel CTS if fifo has < this many more bytes free #define CTSresume 160 //reassert CTS when input fifo has this much room static FIFO (incoming, 541); //I2C events struct I2Cnode current; //current state of I2C msg processing static MotorChannel mc[NSERVO]; static Heater h1; static RotaryValve rv1, rv[NROTARY]; static Gripper grp[NSERVO]; static int16 heaterCounter=0; static SolStruct SS; trapFn *MoveType0, *MoveType1; //masks defined in espdwarf.h enum debugMask debugFlag; uint16 opComplete; //operation complete mask struct I2Cnode current; //current state of I2C msg processing //declare and initialize a reply message buffer // must be invoked as the last declaration in a block // returns a byte pointer to the first byte in the reply body // (ie. its command code) #define beginReply(msgType, tag, cursor) \ byte msgType[2 + sizeof (struct msgType##Reply)]; \ byte *cursor=bufAddByteW (bufAddByte(msgType, current.ownAdr|0x80), tag|0x80); #define bufAddServoPosition(cursor, position) bufAdd24signed(cursor, position) #define FIFOreadServoPosition(fifo) FIFOread24signed(fifo) // instance structures in ROM //parser structure (just for serial I/O) parserInstance debugPort = { &fromHost,&toHost,pollIn,kickstartXmit,startResync }; static void filterMsg (I2CnodeInstance *instance) { acceptNewI2CnodeEvent (instance); //append new event to the events fifo } static void serviceReadRequest (I2CnodeInstance *instance, byte ourAddress) { I2CnodeAnswer (instance, welcome, sizeof(welcome)); //I2CnodeAddEvent (instance, I2CnodeReadRequest); //inform main loop of read } //a complete description of the I2C node message parser static I2CnodeInstance node = { ¤t, //low-level configuration & state &incoming, //event fifo filterMsg, //called from ISR when a message is received serviceReadRequest, //called from ISR when requests read data 3*TIMERHZ, //3 second bus timeout 0x84, //1-bit CRC ACK, 4 message send retries (5 tries total) I2Clow, I2Chigh //I2C clock waveform in SMCLKS }; //hook into dwarfmain.h to process CPLD interrupts static inline void serviceDwarfIRQ (byte pending) /* Service Pending Dwarf Interrupts Global Interrupts are enabled on entry. */ { if (pending & DWRFI2CHELD) I2CnodeCkHeld (&node); if (pending & DCOR0HOMED) homed(&mc[0]); if (pending & DCOR1HOMED) homed(&mc[1]); if (pending & DCOR1PASSED) latchActive1(&mc[1]); // if (pending & DCORCRASHED) limitReached(&mc[0],&mc[1]); //add more CPLD interrupt handler calls here... //each MUST clear the source of the interrupt or mask it before exit } // specify framework's hooks for this application #define dwarfTic() I2CnodeTic(&node) #define dwarfInResume() I2CnodeInResume(&node) #define dwarfOutResume() I2CnodeOutResume(&node) #define DWARFdebugPort debugPort #define DWARFI2CISRargs &node #define DWARFI2Cstate (current.state) //hook into dwarfmain.h to process rising I2C clock edge interrupts #define DWRFEDGEIRQ(edges) \ if (edges & DWRFI2CSCL) I2CnodeCkEdge (&node) //// BEGIN Framework #include "dwarfmain.h" //generic dwarf application "framework" //// END Framework //interrupt[NMI_VECTOR] void DEBUG_TRAP(void) /* To use this trap, first use the debugger to: Set or clear the WDTNMIES bit (6) in WDTCTL Set for low-going transition, clear for high-going Set WDTNMI in WDTCTL (5) Be sure to write 0x5A into the upper byte of WDTCTL Install a breakpoint at this routine */ /$ .iseg espdwarf_ivec_28,abs=0xfffc .data NMI_TRAP .pseg espdwarf_code .global NMI_TRAP NMI_TRAP: bic.b #0x10,&0x2 ;CLEAR NMIIFG bis.b #0x10,&0x0 ;SET NMIIE reti $/ interrupt[ADC_VECTOR] void ADC12_ISR(void) { allowINTinISR(); adcIsr(mc,&rv1,&h1); propagateEV(ADC); } static void runServo (struct timer *array, unsigned index, struct timer *timer) { //start ADC conversion ADC12CTL0|=ADC12SC; getNewPos(mc); if(mc[0].repeatRate) checkServoStatus(mc); if(mc[1].repeatRate) checkServoStatus(mc+1); if(debugFlag&debugEnc) { sendSerialData((int32)mc[0].position); sendSerialData((int32)mc[1].position); parserPutByte(&debugPort,0x0d); } MoveType0(mc+0); MoveType1(mc+1); } static void runValve (struct timer *array, unsigned index, struct timer *timer) { valveISR(rv,debugFlag); if(debugFlag&debugValve) { sendSerialData((int32)(rv[0].position)); parserPutByte(&debugPort,0x0d); } PulseAndHoldISR((SolStruct *)(&SS)); } static void runHeater(struct timer *array, unsigned index, struct timer *timer) { if(h1.state==heaterOn) HeaterServoLoop(&h1); if(debugFlag&debugTemp) { reportErr("T1 %d T2 %d T3 %d", h1.tempSense1,h1.tempSense2,h1.thermoCouple); } if(heaterCounter>8) { //reportErr("T1 %d T2 %d ", h1.tempSense1,h1.tempSense2); //reportErr("C1 %d C2 %d ", mc[0].motorCurrent,mc[1].motorCurrent); if(debugFlag&debugPressure) { reportErr("P1 %d P2 %d ", mc[1].pressure1,mc[1].pressure2);} // reportErr("T1 %d T2 %d ", h1.tempSense1,h1.tempSense2);} heaterCounter=0; }else heaterCounter++; } static void runGripper(struct timer *array, unsigned index, struct timer *timer) { gripperISR(&grp[1]); } void putDebugNewLine (void) { parserPutString (&debugPort, "\r\n"); } void putDebugString (char *string) { parserPutString (&debugPort, string); } void putDebugLine (char *line) { putDebugString(line); putDebugNewLine(); } void reportErr (const char *errTxt, ...) { char errbuf[80]; va_list printfArgs; va_start (printfArgs, errTxt); //suppress output if no one is listening if (!dwarfLostRTS() || debugFlag) { vsprintf (errbuf, errTxt, printfArgs); putDebugLine (errbuf); } // va_end (printfArgs); } //report on the type and content of I2C event received static void dumpI2Cev (unsigned evType, unsigned evLen) { char outbuf[40]; switch (evType) { case I2CnodeNonCRCmsg: sprintf (outbuf,"Rcv'd %d byte NON-CRC msg:",evLen); break; case I2CnodeCRCmsg: sprintf (outbuf, "Rcv'd %d byte CRC msg:", evLen); break; case I2CnodeGCmsg: sprintf(outbuf, "Rcv'd %d byte general call:", evLen); break; case I2CnodeGCCRCmsg: sprintf(outbuf, "Rcv'd %d byte CRC general call:",evLen); break; case I2CnodeReadRequest: putDebugString ("Master read us!"); return; default: sprintf(outbuf,"Unknown (type=%d) %d byte event:", evType, evLen); } putDebugString (outbuf); { byte *head = FIFOhead (node.events); unsigned offset = 0; while (evLen) { //output the message body to the debug port unsigned rcvd = FIFOreadByteAhead (node.events, &head); if (!(offset % 16)) putDebugNewLine (); sprintf (outbuf, "%02X ", rcvd); putDebugString (outbuf); offset++; evLen--; } putDebugNewLine (); } } static void reportI2Cerr (I2CnodeErr err,unsigned dst, byte *body,unsigned len) { char outbuf[60]; switch (err) { case I2CnodeOK: sprintf (outbuf, "Sent Node 0x%02x %d bytes:", dst, len); putDebugString (outbuf); { unsigned offset = 0; while (len) { //output the message body to the debug port if (!(offset % 16)) putDebugNewLine (); sprintf (outbuf, "%02X ", body[offset]); putDebugString (outbuf); offset++; len--; } putDebugNewLine (); } return; case I2CnodeNACK: { unsigned bytesSent = I2CnodeBytesXferred (&node); if (bytesSent) sprintf (outbuf, "Node 0x%02x NACK'd after %d bytes:\n\r", dst, bytesSent); else sprintf (outbuf, "Node 0x%02x is offline\r\n", dst); } break; case I2CnodeBusErr: case I2CnodeTimeOut: sprintf (outbuf, "Bus Error or Timeout accessing node 0x%02x\r\n", dst); break; case I2CnodeArgErr: sprintf (outbuf, "Can't access node 0x%x\r\n", dst); break; case I2CnodeInUse: //from setI2Cnode() sprintf (outbuf, "I2C address 0x%02x is in use!\r\n", dst); break; default: sprintf (outbuf, "Unknown error #%d accessing node 0x%x\r\n", err, dst); } putDebugString (outbuf); } //write a message to dstAdr & report errs to the debugPort static I2CnodeErr writeNode (unsigned dst, byte *body, unsigned len) { I2CnodeErr err = I2CnodeWrite (&node, dst, body, len); if (err || debugFlag & debugI2C) reportI2Cerr (err, dst, body, len); return err; } static void showOwnAdr (void) //factored out to avoid putting two outbuf copies on the stack { char outbuf[60]; sprintf (outbuf, "Assigned Node I2C address 0x%02x" " (with debugMask = 0x%02x)\r\n", current.ownAdr, debugFlag); putDebugString (outbuf); } //assign I2C address from dip switches or debug port static void assignI2Cadr (int newAdr) { I2CnodeErr err; if (newAdr) newAdr += baseI2Cadr; else { reassign: do { putDebugString ("\r\nEnter node's I2C address: 0x"); newAdr = parserGetHex(&debugPort, 0xff); } while (newAdr < 0); } putDebugNewLine (); err = setI2CnodeAdr (&node, newAdr); if (err) { reportI2Cerr (err, newAdr, NULL, 0); if (err == I2CnodeInUse) { byte dupMsg[2]; dupMsg[0] = ESPmsgDupAdr; dupMsg[1] = newAdr; writeNode (0, dupMsg, sizeof(dupMsg)); } goto reassign; } showOwnAdr (); } /************* local functions for msg handling *************/ static byte *addStatusReply (byte *cursor, MotorChannel *mcChan) // append a servo status reply block for mcChan at cursor { cursor = bufAddByte (cursor, updateStatus(mcChan)); cursor = bufAddServoPosition (cursor, getPosition(mcChan)); cursor = bufAdd16 (cursor, mcChan->cmdVel); cursor = bufAddByte (cursor, mcChan->motorCurrent); //don't understand how to get current pressures from mc1 struct cursor = bufAddByte (cursor, (byte)(((int32)79*(int32)mcChan->pressure1-(int32)37005)/(int32)2048)); return bufAddByte (cursor,(byte)(((int32)79*(int32)mcChan->pressure2-(int32)37005)/(int32)2048)); } static void reportShaft (unsigned errCode, unsigned src, unsigned tag, uint16 position, char *debugMsg) //report a shaft (err) reply { if validTag (tag) { beginReply (shaftRotate, tag, cursor); cursor = bufAddByte (cursor, shaftRotateCmd); cursor = bufAdd16 (cursor, position); if (errCode) cursor = bufAddByte (cursor, errCode); writeNode (src, shaftRotate, cursor-shaftRotate); reportErr(debugMsg); } } static void stopShaft (RotaryValve *shaft) { MASK_TIMER_INTERRUPTS(); idleShaft(shaft); UNMASK_TIMER_INTERRUPTS(); reportShaft(shaftAborted, shaft->source,shaft->tag,shaft->position, "Shaft command aborted"); shaft->tag = 0; } static void reportServo (unsigned errCode, unsigned src, unsigned tag, enum ESPmsgCmd cmdType, MotorChannel *mcChan, char *debugMsg) //report a shaft (err) reply { beginReply (servoMove, tag, cursor); cursor = bufAddByte (cursor, cmdType); if (errCode) { cursor = bufAddByte (cursor, errCode); if (mcChan) cursor = addStatusReply (cursor, mcChan); } writeNode (src, servoMove, cursor - servoMove); reportErr (debugMsg); } /********** I2C noReply command handlers *************/ //handle the ESPmsgEchoCmd static void doEchoCmd (unsigned cmdLen) { if (cmdLen) { unsigned source = FIFOreadByte(node.events); //consume source address byte echoBuf[80]; if (cmdLen <= sizeof(echoBuf)+1) { if (--cmdLen) FIFOreadBlockAhead (node.events, FIFOhead(node.events), echoBuf, cmdLen); writeNode (source, echoBuf, cmdLen); } }else reportErr ("Truncated echo command"); } static void doSolenoidConfigureMsg (unsigned cmdLen) { if (cmdLen < 1) { reportErr("Truncated Solenoid Configure Msg"); return; } SS.Duration = FIFOreadByte(node.events); reportErr("Solenoid Configure"); }//End of doSolenoidConfigureMsg static void doSolenoidCmd (const char *cmdName, unsigned cmdLen, uint16* mask, boolean *update) { if (!cmdLen){ reportErr("Truncated Solenoid %s Cmd", cmdName); *update = false; } else{ uint16 m = FIFOreadByte(node.events); if (cmdLen > 1) m |= FIFOreadByte(node.events) << 8; MASK_TIMER_INTERRUPTS(); *mask = m; *update = true; UNMASK_TIMER_INTERRUPTS(); reportErr("%s 0x%04X", cmdName, m); } } static void doHoldCommand (unsigned cmdLen) { doSolenoidCmd ("Hold", cmdLen, &SS.nHold,&SS.Hupdate); }//End of doHoldCommand static void doPulseCommand (unsigned cmdLen) { doSolenoidCmd ("Pulse", cmdLen, &SS.nPulse,&SS.Pupdate); }//End of doPulseCommand static void doThermalConfigMsg (unsigned cmdLen) { thermalCalibration tempTC; if (cmdLen >=6) { tempTC.heating.P = FIFOread16(node.events); tempTC.heating.I = FIFOread16(node.events); tempTC.heating.D = FIFOread16(node.events); if (cmdLen < 7) goto Process; tempTC.cooling.P = FIFOread16(node.events); tempTC.cooling.I = FIFOread16(node.events); tempTC.cooling.D = FIFOread16(node.events); Process: h1.tCalib=tempTC; reportErr("Thermal Calibration Configure"); return; } else { reportErr("Truncated Thermal Configure Msg"); return; } } static void doservoConfig (unsigned cmdLen) { MotorChannel tempMC; MotorChannel *mcChan; uint8 temp; if (cmdLen >= 2) //require channel # and option mask at a minimum { unsigned channel = FIFOreadByte(node.events);//consume channel if (channel >= NSERVO) { reportErr ("Bad channel in servo config"); return; } //valid channel so start getting data //but before updating this critical information //the servo needs to be shut down and the servo loop stopped. reportErr ("Servo config"); //first initilize tempMC to channels information tempMC = mc[channel]; //get the option mask temp = FIFOreadByte(node.events); tempMC.optionMask = temp; if (cmdLen < 5) goto Process; tempMC.thresholdOffset = FIFOreadServoPosition(node.events); if (cmdLen < 8) goto Process; tempMC.maxPositionErr = FIFOreadServoPosition(node.events); if (cmdLen < 16) goto Process; tempMC.gain.P = FIFOread16(node.events); tempMC.gain.I = FIFOread16(node.events); tempMC.gain.D = FIFOread16(node.events); if (cmdLen < 18) goto Process; tempMC.accel = FIFOread16(node.events); if (cmdLen < 20) goto Process; tempMC.maxSpeed = FIFOread16(node.events); if (cmdLen < 22) goto Process; tempMC.minSpeed = FIFOread16(node.events); if (cmdLen < 23) goto Process; tempMC.maxCurrent = (FIFOreadByte(node.events) *16); if (cmdLen < 24) goto Process; tempMC.maxPressureDelta =FIFOreadByte(node.events); if (cmdLen < 25) goto Process; tempMC.maxInPressure =(int16)(((int32)FIFOreadByte(node.events)*414+7485 )/16); if (cmdLen < 26) goto Process; tempMC.maxOutPressure = (int16)(((int32)FIFOreadByte(node.events)*414+7485 )/16); if (cmdLen < 27) goto Process; tempMC.minInPressure = (int16)(((int32)FIFOreadByte(node.events)*414+7485 )/16); if (cmdLen < 28) goto Process; tempMC.minOutPressure = (int16)(((int32)FIFOreadByte(node.events)*414+7485 )/16); Process: //now that message has been consumed all these values must be //verified before updating! if (tempMC.maxSpeed <= tempMC.minSpeed) { reportErr ("maxSpeed <= minSpeed"); return; } tempMC.defaultMaxSpeed=tempMC.maxSpeed; if (tempMC.maxPositionErr < 0 ) { reportErr ("maxPositionErr < 0"); return; } if (tempMC.maxOutPressure < tempMC.minOutPressure ) { reportErr ("maxOutPressure < minOutPressure"); return; } if (tempMC.maxInPressure < tempMC.minInPressure ) { reportErr ("maxInPressure < minInPressure"); return; } // reportErr ("Servo config cmd validated"); MASK_TIMER_INTERRUPTS(); mc[channel] = tempMC; mcChan=mc+channel; if ( mcChan->optionMask & servoOptionEnable) { //Turn encoder power on if (channel == 0) { set_cpld_bit(DCORENCPWR,DCOR0ENC5); // disableDWRFirqs (DCOR0HOMED); } else { set_cpld_bit(DCORENCPWR,DCOR1ENC5); // disableDWRFirqs (DCOR1HOMED); } } else { set8(mcChan->statusMask,servoLostHome); //Turn encoder power off if (channel == 0) { clear_cpld_bit(DCORENCPWR,DCOR0ENC5); MoveType0 = ServoDoNothing; if( MoveType1 == ServoDoNothing) clear_cpld_bit(DCOROUT,DCORMOTORPWR); } else { clear_cpld_bit(DCORENCPWR,DCOR1ENC5); MoveType1 = ServoDoNothing; if( MoveType0 == ServoDoNothing) clear_cpld_bit(DCOROUT,DCORMOTORPWR); } } if(mcChan->optionMask & servoOptionEncPolarity) { if (channel == 0) { set_cpld_bit(DCORENCCTL,DCOR0ENCDIR); } else { set_cpld_bit(DCORENCCTL,DCOR1ENCDIR); } } else { if (channel == 0) { clear_cpld_bit(DCORENCCTL,DCOR0ENCDIR); } else { clear_cpld_bit(DCORENCCTL,DCOR1ENCDIR); } } //disable latch int if (channel == 0) { disableDWRFirqs (DCOR0PASSED); } else { disableDWRFirqs (DCOR1PASSED); } //turn threshold on?? if ( mcChan->optionMask & servoOptionThreshold) { //and if so which channel, this can be done outside of interrupts if (channel == 0) set_cpld_bit(DCORENCPWR,DCOR0HOME3); else set_cpld_bit(DCORENCPWR,DCOR1HOME3);} else { if (channel == 0) clear_cpld_bit(DCORENCPWR,DCOR0HOME3); else clear_cpld_bit(DCORENCPWR,DCOR1HOME3);} //set threshold polarity if ( mcChan->optionMask & servoOptionThresPolarity){ //and if so which channel, this can be done outside of interrupts if (channel == 0) set_cpld_bit(DCORENCCTL,DCOR0PSEDGE); else set_cpld_bit(DCORENCCTL,DCOR1PSEDGE);} else { if (channel == 0) clear_cpld_bit(DCORENCCTL,DCOR0PSEDGE); else clear_cpld_bit(DCORENCCTL,DCOR1PSEDGE);} if (channel == 0) { disableDWRFirqs (DCOR0HOMED); } else { disableDWRFirqs (DCOR1HOMED); } //check if the home sensor is requested on if (mcChan->optionMask & servoOptionHomePwr) { if (channel == 0) set_cpld_bit(DCORENCPWR,DCOR0HOME5); else set_cpld_bit(DCORENCPWR,DCOR1HOME5);} else { if (channel == 0) clear_cpld_bit(DCORENCPWR,DCOR0HOME5); else clear_cpld_bit(DCORENCPWR,DCOR1HOME5);} if ( mcChan->optionMask & servoOptionHomePolarity){ if (channel == 0) set_cpld_bit(DCORENCCTL,DCOR0HMEDGE); else set_cpld_bit(DCORENCCTL,DCOR1HMEDGE);} else { if (channel == 0) clear_cpld_bit(DCORENCCTL,DCOR0HMEDGE); else clear_cpld_bit(DCORENCCTL,DCOR1HMEDGE);} UNMASK_TIMER_INTERRUPTS(); reportErr ("Servo %d, updated", channel); } else reportErr ("Truncated servo config command"); } static void doservoOff (unsigned cmdLen) { if (cmdLen >= (sizeof(struct servoOffMsg)-1)) //(sizeof(struct servoConfigMsg)-1)) { unsigned channel = FIFOreadByte(node.events);//consume channel cmdLen -= 1; if (channel >= NSERVO) { reportErr ("Bad channel in servo config"); return; } //valid channel so start getting data //but before updating this critical information //the servo needs to be shut down and the servo loop stopped. reportErr ("Servo %d off", channel); //first initilize tempMC to channels information servoOff(&mc[channel]); } else reportErr ("Truncated servo off command"); } static void doShaftConfigMsg (unsigned cmdLen) { if (cmdLen >= 1) { unsigned shaftNum = FIFOreadByte(node.events); if (shaftNum < NROTARY) { RotaryValve *shaft = rv + shaftNum; stopShaft(shaft); { uint16 maxError = shaft->maxError; uint16 revCounts = shaft->revCounts; uint16 lash = shaft->lash; byte maxCurrent = shaft->maxCurrent; byte brakeTics = shaft->brakeTics; uint16 maxErr; if (cmdLen >= 2) { brakeTics = FIFOreadByte(node.events); if (cmdLen >= 3) { maxCurrent = FIFOreadByte(node.events); if (cmdLen >= 5) { maxError = FIFOread16(node.events); if (cmdLen >= 7) { revCounts = FIFOread16(node.events); if (cmdLen >= 9) lash = FIFOread16(node.events); } } } } maxErr = maxError & 0x7fff; if (maxErr && revCounts + maxErr <= revCounts || lash && lash >= revCounts/4) reportErr ("Ignoring invalid config for shaft %d", shaftNum); else { shaft->maxError = maxError; shaft->revCounts = revCounts; shaft->lash = lash; shaft->maxCurrent = maxCurrent; shaft->brakeTics = brakeTics+1; } } } } } static inline void parseCmd (unsigned noReplyCmd, unsigned len) { switch (noReplyCmd) { case ESPmsgNOP: case ESPmsgDupAdr: break; case ESPmsgEchoCmd: doEchoCmd (len); break; case servoConfigCmd: doservoConfig(len); break; case servoOffCmd: doservoOff(len); break; case solenoidHoldCmd: doHoldCommand(len); break; case solenoidPulseCmd: doPulseCommand(len); break; case solenoidConfigCmd: doSolenoidConfigureMsg(len); break; case shaftConfigCmd: doShaftConfigMsg(len); break; case thermalConfigCmd: doThermalConfigMsg(len); break; default: reportErr("Bad %d byte I2C cmd (0x%02x)", len, noReplyCmd); } } /***************** I2C request handlers *****************/ static void doSolenoidStatusReq (unsigned src, unsigned tag, unsigned reqLen) { uint16 mask; beginReply (solenoidStatus, tag, cursor); cursor = bufAddByte (cursor, solenoidStatusCmd); MASK_TIMER_INTERRUPTS(); cursor = bufAddByte(cursor, SS.tic); mask = SS.cPulse | SS.cHold; UNMASK_TIMER_INTERRUPTS(); cursor = bufAddByte(cursor,mask); cursor = bufAddByte(cursor,mask>>8); writeNode (src, solenoidStatus, cursor - solenoidStatus); }//end of doSolenoidStatusReq static void doThermalConfigRequest(unsigned src,unsigned tag,unsigned reqLen) { beginReply (thermalConfig, tag, cursor); cursor = bufAddByte (cursor, thermalConfigCmd); { Heater *hTemp=&h1; cursor = bufAdd16 (cursor,hTemp->tCalib.heating.P); cursor = bufAdd16 (cursor,hTemp->tCalib.heating.I); cursor = bufAdd16 (cursor,hTemp->tCalib.heating.D); cursor = bufAdd16 (cursor,hTemp->tCalib.cooling.P); cursor = bufAdd16 (cursor,hTemp->tCalib.cooling.I); cursor = bufAdd16 (cursor,hTemp->tCalib.cooling.D); } writeNode (src, thermalConfig, cursor - thermalConfig); } static void makeThermalStatusReply(unsigned src, unsigned tag) { Heater *hTemp=&h1; beginReply (thermalStatus, tag, cursor); cursor = bufAddByte (cursor, thermalStatusCmd); cursor = bufAdd16 (cursor,(int16) ((160000UL*hTemp->tempSense1)>>16)); if(hTemp->state==heaterOn){ cursor = bufAdd16 (cursor, hTemp->elapsedTics); if( hTemp->heaterPWM>=0) cursor = bufAddByte (cursor, hTemp->heaterPWM); else cursor = bufAddByte (cursor, hTemp->coolerPWM);} writeNode (src, thermalStatus, cursor - thermalStatus); } static void doThermalStatusRequest (unsigned src, unsigned tag, unsigned reqLen) { if (reqLen) { unsigned rate = FIFOreadByte(node.events); } makeThermalStatusReply(src,tag); } static void makeServoStatusReply(unsigned src, unsigned tag,unsigned channel) { beginReply (servoStatus, tag, cursor); cursor = bufAddByte (cursor, servoStatusCmd); cursor = addStatusReply (cursor, mc+channel); writeNode (src, servoStatus, cursor - servoStatus); } //handle the servoStatusCmd request static void doServoStatusReq (unsigned src, unsigned tag, unsigned reqLen) { MotorChannel *mcChan; if (reqLen) { unsigned channel = FIFOreadByte(node.events); //consume servo channel # mcChan=mc+channel; if(reqLen>=2) { mcChan->repeatRate=FIFOread16(node.events); if(mcChan->repeatRate) { mcChan->servoTics=0; mcChan->sourceStatus=src; mcChan->tagStatus=tag; } else return; } if (channel < NSERVO) { //truncate status if channel # is invalid makeServoStatusReply(src, tag,channel); } } } static void doServoStatusRep (unsigned src, unsigned tag,unsigned channel) { MotorChannel *mcChan = mc+channel; makeServoStatusReply(src, tag,channel); } void checkServoStatus(MotorChannel *mcChan) { mcChan->servoTics--; if (mcChan->servoTics == 0){ doServoStatusRep (mcChan->sourceStatus, mcChan->tagStatus,0); mcChan->servoTics = mcChan->repeatRate+1;} } static void doShaftRotateRequest (unsigned src, unsigned tag, unsigned reqLen) { if (reqLen >= 1) { unsigned ShaftNum = FIFOreadByte(node.events); //consume servo channel # RotaryValve *shaft; byte previousState; if (ShaftNum >= NROTARY) goto badShaft; shaft = rv+ShaftNum; MASK_TIMER_INTERRUPTS(); previousState = shaft->State; stopShaft(shaft); //this unmasks timer interrupts shaft->tag = tag; shaft->source = src; if (reqLen >= 3) { uint16 goal = FIFOread16(node.events); uint16 r = shaft->revCounts; int8 direction = 0; if (reqLen >= 4) direction = FIFOreadByte(node.events); if (goal >= r) { reportShaft(invalidShaftAngle, src, tag, shaft->position, "Bad Shaft Angle"); return; } { const char *dirName = ""; if (direction) dirName = direction < 0 ? "down" : "up"; reportErr("Rotating Shaft %d %sto %d", ShaftNum, dirName, goal); } shaft->direction = direction; shaft->goal = goal; shaft->State = SSMstart; }else{ //missing goal -- just stop and report position if (previousState > SSMstart) shaft->brakeTimer = shaft->brakeTics; if (!shaft->brakeTimer) shaft->brakeTimer = 1; shaft->State = SSMread; } }else{ badShaft: reportShaft(invalidShaftNumber,src,tag,0, "Bad or Missing Shaft Number"); } }//end of doShaftRotateRequest static void doShaftConfigRequest (unsigned src, unsigned tag, unsigned reqLen) { beginReply (shaftConfig, tag, cursor); cursor = bufAddByte (cursor, shaftConfigCmd); if (reqLen) { unsigned shaftNum = FIFOreadByte(node.events); if (shaftNum < NROTARY) { //truncate config if shaft # is invalid RotaryValve *shaft = rv+shaftNum; cursor = bufAddByte (cursor, shaft->brakeTics); cursor = bufAddByte (cursor, shaft->maxCurrent); cursor = bufAdd16 (cursor, shaft->maxError); cursor = bufAdd16 (cursor, shaft->revCounts); } }//(reqLen >= 1) writeNode (src, shaftConfig, cursor - shaftConfig); }//End of doShaftConfigRequest //handle the shaftStatusCmd request static void doShaftStatusReq (unsigned src, unsigned tag, unsigned reqLen) { beginReply (shaftStatus, tag, cursor); cursor = bufAddByte (cursor, shaftStatusCmd); if (reqLen) { unsigned shaftNum = FIFOreadByte(node.events); //consume shaft # if (shaftNum < NROTARY) { //truncate status if shaft # is invalid RotaryValve *shaft = rv + shaftNum; cursor = bufAdd16 (cursor, shaft->position); if (shaft->State >= SSMstart) { int8 direction = 0; byte state = shaft->State; if (state == SSMrotatingUp) direction = 1; else if (state == SSMrotatingDown) direction = -1; cursor = bufAdd16 (cursor, shaft->goal); cursor = bufAddByte (cursor, direction); } } } writeNode (src, shaftStatus, cursor - shaftStatus); } static void doServoConfigRequest (unsigned src, unsigned tag, unsigned reqLen) { beginReply (servoConfig, tag, cursor); cursor = bufAddByte (cursor, servoConfigCmd); if (reqLen) { unsigned channel = FIFOreadByte(node.events); //consume servo channel # if (channel < NSERVO) { //truncate config if channel # is invalid MotorChannel *mcChan = mc+channel; cursor = bufAddByte (cursor, mcChan->optionMask); cursor = bufAddServoPosition (cursor, mcChan->thresholdOffset); cursor = bufAddServoPosition (cursor, mcChan->maxPositionErr); cursor = bufAdd16 (cursor, mcChan->gain.P); cursor = bufAdd16 (cursor, mcChan->gain.I); cursor = bufAdd16 (cursor, mcChan->gain.D); cursor = bufAdd16 (cursor, mcChan->maxSpeed); cursor = bufAdd16 (cursor, mcChan->minSpeed); cursor = bufAddByte (cursor, mcChan->maxCurrent); cursor = bufAddByte (cursor, mcChan->maxPressureDelta); cursor = bufAddByte (cursor, mcChan->maxInPressure); cursor = bufAddByte (cursor, mcChan->maxOutPressure); cursor = bufAddByte (cursor, mcChan->minInPressure); cursor = bufAddByte (cursor, mcChan->minOutPressure); }//(channel < NSERVO) }//(reqLen >= 1) writeNode (src, servoConfig, cursor - servoConfig); }//End of doServoConfigRequest void doServoOnRequest(unsigned src, unsigned tag, unsigned reqLen) { MotorChannel* mcChan; if (reqLen >= (sizeof(struct servoOnRequest)-1)) //(sizeof(struct servoConfigMsg)-1)) { unsigned channel = FIFOreadByte(node.events);//consume channel reqLen -= 1; if (channel >= NSERVO) { reportErr ("Bad channel in servo config"); return; } mcChan=mc+channel; //valid channel so start getting data reportErr ("Servo %d on", channel); //first initilize tempMC to channels information MASK_TIMER_INTERRUPTS(); //is servo enabled?? if ( mcChan->optionMask & servoOptionEnable) { if (!(mcChan->goalPosition==mcChan->intermediate)) { unsigned savesource; beginReply (servoMove, mcChan->tag, cursor); cursor = bufAddByte (cursor, mcChan->cmdType); cursor = bufAddByte (cursor, servoAborted); cursor = addStatusReply (cursor, mcChan); //now the buffer is built but don't send yet! //build up new position information to start to new position savesource = mcChan->source; //clear status bits reportErr ("Aborting move"); writeNode (savesource, servoMove, cursor - servoMove); } else { //clear out PWM regs if(mcChan->channel==0) TBCCR1 = 0; else TBCCR2 = 0; //turn power on set_cpld_bit(DCOROUT,DCORMOTORPWR); } mcChan->intermediate = mcChan->position; mcChan->goalPosition = mcChan->position; mcChan->cmdVel=0; // mcChan->sum=0; if(mcChan->channel==0) MoveType0 = trapServoLoop; else MoveType1 = trapServoLoop; //save tag and source mcChan->cmdType=servoOnCmd; mcChan->source=src; mcChan->tag=tag; mcChan->reason=0; set8(mcChan->statusMask, servoOn); UNMASK_TIMER_INTERRUPTS(); } else { mcChan->reason= servoNotReady; reportServo(servoNotReady, src, tag, servoOnCmd, mcChan, "ServoNotReady"); } }else reportErr("Bad Servo Cmd"); } static void moveServo (unsigned src, unsigned tag, enum ESPmsgCmd cmd, MotorChannel *mcChan, int32 sP) { //looks like we have a good message //if Tag != 0 then move in progress, need to abort first if (mcChan->statusMask & servoOn) { if (validTag(mcChan->tag)) {//abort current move //build up aborted move reply - ServoMoveReply //use the old saved tag of the current move unsigned savesource; beginReply (servoMove, mcChan->tag, cursor); cursor = bufAddByte (cursor, mcChan->cmdType); cursor = bufAddByte (cursor, servoAborted); cursor = addStatusReply (cursor, mcChan); //now the buffer is built but don't send yet! //build up new position information to start to new position savesource = mcChan->source; if(cmd==servoAbsMoveCmd) startAbsMove(mcChan, sP, tag, src); else startRelMove(mcChan, sP, tag, src); //clear status bits reportErr ("Aborting move"); writeNode (savesource, servoMove, cursor - servoMove); } //no move in progress else { if(cmd==servoAbsMoveCmd) startAbsMove(mcChan, sP, tag, src); else startRelMove(mcChan, sP, tag, src); } }else //servo is off!! reportServo(servoNotReady, src, tag, cmd, mcChan, "ServoNotReady"); } static const char *badServoChannel = "Bad or Missing Servo Channel"; static void doServoAbsMoveRequest (unsigned src, unsigned tag, unsigned reqLen) { int32 sP; if (reqLen < (sizeof(struct servoMoveRequest)-1)) goto badChannel; { unsigned channel = FIFOreadByte(node.events); //consume servo channel # sP = FIFOreadServoPosition(node.events); //consume new position if (channel >= NSERVO) { badChannel: // build up bad request reply reportServo (invalidServoChannel, src, tag, servoAbsMoveCmd,NULL, badServoChannel); } else moveServo (src, tag, servoAbsMoveCmd, mc+channel, sP); } } static void doServoRelMoveRequest (unsigned src, unsigned tag, unsigned reqLen) { int32 sP; if (reqLen < sizeof(struct servoMoveRequest)-1) goto badChannel; { unsigned channel = FIFOreadByte(node.events); //consume servo channel # sP = FIFOreadServoPosition(node.events); //consume new position if (channel >= NSERVO) { badChannel: // build up bad reuest reply reportServo (invalidServoChannel, src, tag, servoRelMoveCmd, NULL, badServoChannel); } else { MotorChannel *mcChan = mc+channel; moveServo (src, tag, servoRelMoveCmd, mcChan, sP); } } } static void doservoGripReq(unsigned src,unsigned tag,unsigned reqLen) { int32 sP; if (reqLen < sizeof(struct servoGripRequest)-1) goto badChannel; { unsigned channel = FIFOreadByte(node.events); //consume servo channel # sP = FIFOreadServoPosition(node.events); //consume new position if (channel >= NSERVO) { badChannel: // build up bad reuest reply reportServo (invalidServoChannel, src, tag, servoGripCmd, NULL, badServoChannel); } else { Gripper *gp = grp+channel; gp->goalPosition=1; gp->State=SWrite; gp->tag=tag; gp->source=src; gp->cmdType=servoGripCmd; } } } static void doservoReleaseReq(unsigned src,unsigned tag,unsigned reqLen) { int32 sP; if (reqLen < sizeof(struct servoGripRequest)-1) goto badChannel; { unsigned channel = FIFOreadByte(node.events); //consume servo channel # sP = FIFOreadServoPosition(node.events); //consume new position if (channel >= NSERVO) { badChannel: // build up bad reuest reply reportServo (invalidServoChannel, src, tag, servoReleaseCmd, NULL, badServoChannel); } else { Gripper *gp = grp+channel; gp->goalPosition=0; gp->State=SWrite; gp->tag=tag; gp->source=src; gp->cmdType=servoReleaseCmd; } } } static void dothermalRunRequest(unsigned src,unsigned tag,unsigned reqLen) { thermalStep tS; if (reqLen >= sizeof(tS)) { tS.dwell = FIFOread16(node.events); //1/4 sec tics tS.targetT = FIFOread16(node.events); //in hudredths of deg C tS.errHoldoff = FIFOread16(node.events); //ticks before starting to check temp tS.minT = FIFOread16(node.events); tS.maxT = FIFOread16(node.events); //acceptable temp band h1.elapsedTics=0; h1.tProgram = tS; h1.source = src; h1.tag = tag; if(!tS.dwell) { h1.source = src;//comment this out to recreate bug h1.tag = tag; turnHeaterOff(&h1); reportErr ("Heater Off"); return; } if(tS.targetT>=0) startHeater(&h1); } else{ h1.source = src;//comment this out to recreate bug h1.tag = tag; turnHeaterOff(&h1); reportErr ("Heater Off");} } static inline void parseRequest (unsigned request, unsigned src, unsigned tag, unsigned len) { // reportErr("Request Command Parsing"); switch (request) { case solenoidStatusCmd: doSolenoidStatusReq (src, tag, len); break; case servoStatusCmd: doServoStatusReq (src, tag, len); break; case servoConfigCmd: doServoConfigRequest (src, tag, len); break; case servoOnCmd: doServoOnRequest(src, tag, len); break; case servoAbsMoveCmd: doServoAbsMoveRequest(src, tag, len); break; case servoRelMoveCmd: doServoRelMoveRequest(src, tag, len); break; case servoGripCmd: doservoGripReq(src,tag,len); break; case servoReleaseCmd: doservoReleaseReq(src,tag,len); break; case shaftRotateCmd: doShaftRotateRequest(src, tag, len); break; case shaftConfigCmd: doShaftConfigRequest(src, tag, len); break; case shaftStatusCmd: doShaftStatusReq(src, tag, len); break; case thermalRunCmd: dothermalRunRequest(src, tag, len); break; case thermalStatusCmd: doThermalStatusRequest(src,tag,len); break; case thermalConfigCmd: doThermalConfigRequest(src,tag,len); break; default: reportErr("Bad %d byte I2C request (0x%02x) from 0x%02x",len,request,src); } } /***************** I2C reply handlers *******************/ static inline void parseReply (unsigned reply, unsigned src, unsigned tag, unsigned len) { switch (reply) { default: reportErr("Bad %d byte I2C reply (0x%02x) from 0x%02x", len, reply, src); } } // send a reply when the thermal controller completes a request // or if it was interrupted by a new one static void sendThermalDoneReply(void) { Heater *hTemp = &h1; beginReply (thermalRun, hTemp->tag, cursor); cursor = bufAddByte (cursor,thermalRunCmd); cursor = bufAddByte (cursor, hTemp->code); cursor = bufAdd16 (cursor, hTemp->elapsedTics); writeNode (hTemp->source, thermalRun, cursor - thermalRun); reportErr("Heater done"); } static void sendRotationDoneReply(unsigned channel) { RotaryValve *shaft = rv+channel; clear16(opComplete,rvalve0 << channel); // clear out so that we do not wake up any more // insert code to report overcurrent here!!! reportShaft (noShaftError, shaft->source, shaft->tag, shaft->position, "Rotate done"); shaft->tag=0; } static void sendServoDoneReply(unsigned channel) /* send a reply when the servo controller completes a request or if it was interrupted by a new one */ { MotorChannel *mcChan = mc+channel; beginReply (servoMove, mcChan->tag, cursor); cursor = bufAddByte (cursor, mcChan->cmdType); if (mcChan->reason) { cursor = bufAddByte (cursor, mcChan->reason); cursor = addStatusReply (cursor, mcChan); reportErr("Servo error = %d", mcChan->reason); } else reportErr("Servo done at %08lx", mcChan->position); clear16(opComplete,servo0 << channel); // clear out so that we do not wake up any more mcChan->tag |= 0x80; //set the MSB to signal that it has been sent writeNode (mcChan->source, servoMove, cursor - servoMove); } static void sendGripDoneReply(unsigned channel) { Gripper *gp=grp+channel; beginReply (servoGrip, gp->tag, cursor); cursor = bufAddByte (cursor, gp->cmdType); if (gp->reason) { cursor = bufAddByte (cursor, gp->reason); reportErr("Gripper error = %d", gp->reason); } else reportErr("Gripper done"); clear16(opComplete,grip<< channel); // clear out so that we do not wake up any more gp->tag |= 0x80; //set the MSB to signal that it has been sent gp->sendDoneReply=0; writeNode (gp->source, servoGrip, cursor - servoGrip); } static void sendReplies (void) //send reply messages for completed operations //called when opComplete has at least one bit set //should clear at least one bit of opComplete mask per invokation { uint16 opsDone = opComplete; uint16 mask; unsigned channel; if (opsDone >= (mask = 1<= (mask = 1<= (mask = 1<= 3) { unsigned tag = FIFOreadByte(node.events); unsigned cmdCode = FIFOreadByte(node.events); evLen -= 3; if (tag & 0x80) //it's a reply to our own outstanding request parseReply (cmdCode, srcOrCmd, tag & 0x7f, evLen); else parseRequest (cmdCode, srcOrCmd, tag, evLen); }else reportErr ("Truncated (%d byte) Request or Reply" " from node 0x%0x2", evLen, srcOrCmd); }else //no reply for this command parseCmd (srcOrCmd, evLen-1); } break; case I2CnodeReadRequest: //already serviced by filter fn break; default: //ignore all non-CRC messages reportErr ("Ignored %d byte Non-CRC message type %d", evLen,evType); } retireI2CnodeEvent(&node); } } RESET(); //just in case we somehow get here! }