//thread entry function void WorkerGroup::Run() { bool doExit = false; static thread_rank_t counter = 0; thread_rank_t rank; thread_internal_cmd_t cmd; //determine rank of this thread workDispatch.Lock(); rank = counter; counter++; workDispatch.Unlock(); //worker thread main loop while(!doExit) { //wait until work has been assigned cmd = RecvCmd(); switch(cmd) { case THREADS_IDLE: { WorkerGroupException e; throw e; break; } case THREADS_SHUTDOWN: doExit = true; break; default: //do work cmds[cmd]->Exec((thread_cmd_t)cmd, rank); break; } //confirm completion of command AckCmd(); } }
void Idle() { unsigned int addr = 0; unsigned char MsgAddr = 0; unsigned char PresetNumber = 0; unsigned char *ConfigPtr; ConfigStruct TempConfig; char idx; MULTI temp; PRESET tempPset; if (ExtModeActive) { idx = 255; while (idx--); return; } if (!RX_MsgComplete) { idx = 255; while (idx--); return; } ChkSum = 0; ReadIdx = 1; RX_MsgComplete = 0; MsgAddr = MessageStream_ReadByte(); if (MsgAddr == 0xFF) { TX_Idx = 0; TXBuffer[TX_Idx++] = '$'; MessageStream_WriteByte(MyID); MessageStream_WriteByte('O'); MessageStream_WriteByte('R'); MessageStream_WriteByte('B'); MessageStream_WriteByte('I'); MessageStream_WriteByte('T'); TXBuffer[TX_Idx++] = '#'; idx = TX_Idx; TX_Idx = 0; TX_bCount = idx; } else if (MsgAddr == MyID) { unsigned char CmdID = MessageStream_ReadByte(); switch (CmdID) { case CMD_GET_COMPLETE_STATUS: TX_Idx = 0; TXBuffer[TX_Idx++] = '$'; MessageStream_WriteByte(CmdID); for (idx = 0; idx < 20; idx++) { MessageStream_WriteByte(LCD_Line2[idx]); ChkSum += LCD_Line2[idx]; } for (idx = 0; idx < 20; idx++) { MessageStream_WriteByte(LCD_Line1[idx]); ChkSum += LCD_Line1[idx]; } bLock_Motor_Position = 1; temp.dbl = Motor_Position; bLock_Motor_Position = 0; temp.dbl *= Config.Degrees_Per_Count; MessageStream_WriteByte(temp.ub[0]); MessageStream_WriteByte(temp.ub[1]); MessageStream_WriteByte(temp.ub[2]); MessageStream_WriteByte(temp.ub[3]); ChkSum += temp.ub[0]; ChkSum += temp.ub[1]; ChkSum += temp.ub[2]; ChkSum += temp.ub[3]; temp.dbl = 0; while (!(temp.dbl == Move_speedQ24)) temp.dbl = Move_speedQ24; temp.dbl *= INV_Conversion_Q24_500Hz; temp.dbl *= Config.Degrees_Per_Count; MessageStream_WriteByte(temp.ub[0]); MessageStream_WriteByte(temp.ub[1]); MessageStream_WriteByte(temp.ub[2]); MessageStream_WriteByte(temp.ub[3]); ChkSum += temp.ub[0]; ChkSum += temp.ub[1]; ChkSum += temp.ub[2]; ChkSum += temp.ub[3]; temp.dbl = 0; bLock_BatteryVoltage = 1; temp.dbl = BatteryVoltage; bLock_BatteryVoltage = 0; temp.dbl *= Config.Volts_per_Count; MessageStream_WriteByte(temp.ub[0]); MessageStream_WriteByte(temp.ub[1]); MessageStream_WriteByte(temp.ub[2]); MessageStream_WriteByte(temp.ub[3]); ChkSum += temp.ub[0]; ChkSum += temp.ub[1]; ChkSum += temp.ub[2]; ChkSum += temp.ub[3]; MessageStream_WriteByte(ChkSum); TXBuffer[TX_Idx++] = '#'; idx = TX_Idx; TX_Idx = 0; TX_bCount = idx; break; case CMD_GETDISPLAY: TX_Idx = 0; TXBuffer[TX_Idx++] = '$'; MessageStream_WriteByte(CmdID); for (idx = 0; idx < 20; idx++) { MessageStream_WriteByte(LCD_Line2[idx]); ChkSum += LCD_Line2[idx]; } for (idx = 0; idx < 20; idx++) { MessageStream_WriteByte(LCD_Line1[idx]); ChkSum += LCD_Line1[idx]; } MessageStream_WriteByte(ChkSum); TXBuffer[TX_Idx++] = '#'; idx = TX_Idx; TX_Idx = 0; TX_bCount = idx; break; case CMD_UI_CLICK: isPendingExternalUIEvent = 0; PendingExternalUIEvent = USER_INPUT_CLICK; isPendingExternalUIEvent = 1; AckCmd(CmdID); break; case CMD_UI_CANCEL: isPendingExternalUIEvent = 0; PendingExternalUIEvent = USER_INPUT_CANCEL; isPendingExternalUIEvent = 1; AckCmd(CmdID); break; case CMD_UI_BACK: isPendingExternalUIEvent = 0; PendingExternalUIEvent = USER_INPUT_BACK; isPendingExternalUIEvent = 1; AckCmd(CmdID); break; case CMD_UI_INC: isPendingExternalUIEvent = 0; PendingExternalUIEvent = USER_INPUT_INC; isPendingExternalUIEvent = 1; AckCmd(CmdID); break; case CMD_UI_DEC: isPendingExternalUIEvent = 0; PendingExternalUIEvent = USER_INPUT_DEC; isPendingExternalUIEvent = 1; AckCmd(CmdID); break; case CMD_UI_MACRO: isPendingExternalUIEvent = 0; PendingExternalUIEvent = -MessageStream_ReadByte(); isPendingExternalUIEvent = 1; AckCmd(CmdID); break; case CMD_GET_UI_LOC: TX_Idx = 0; TXBuffer[TX_Idx++] = '$'; MessageStream_WriteByte(CmdID); MessageStream_WriteByte(UI_Location); MessageStream_WriteByte(UI_Location); TXBuffer[TX_Idx++] = '#'; idx = TX_Idx; TX_Idx = 0; TX_bCount = idx; break; case CMD_GET_POS: bLock_Motor_Position = 1; temp.dbl = Motor_Position; bLock_Motor_Position = 0; temp.dbl *= Config.Degrees_Per_Count; MessageStream_WriteMULTI(temp, CmdID); break; case CMD_GET_SPEED: temp.dbl = 0; while (!(temp.dbl == Move_speedQ24)) temp.dbl = Move_speedQ24; temp.dbl *= INV_Conversion_Q24_500Hz; temp.dbl *= Config.Degrees_Per_Count; MessageStream_WriteMULTI(temp, CmdID); break; case CMD_GET_BATTERY: temp.dbl = 0; bLock_BatteryVoltage = 1; temp.dbl = BatteryVoltage; bLock_BatteryVoltage = 0; temp.dbl *= Config.Volts_per_Count; MessageStream_WriteMULTI(temp, CmdID); break; case CMD_GET_PRESET: PresetNumber = MessageStream_ReadByte(); if (PresetNumber > 5) { NackCmd(CmdID, 1); break; } if (PresetNumber == 0) { NackCmd(CmdID, 1); break; } LoadPreset(PresetNumber, (unsigned char *) &tempPset); TX_Idx = 0; TXBuffer[TX_Idx++] = '$'; MessageStream_WriteByte(CmdID); for (idx = 0; idx<sizeof (tempPset); idx++) { MessageStream_WriteByte(tempPset.ub[idx]); ChkSum += tempPset.ub[idx]; } MessageStream_WriteByte(ChkSum); TXBuffer[TX_Idx++] = '#'; idx = TX_Idx; TX_Idx = 0; TX_bCount = idx; break; case CMD_SET_PRESET: PresetNumber = MessageStream_ReadByte(); if (PresetNumber > 5) { NackCmd(CmdID, 1); break; } if (PresetNumber == 0) { NackCmd(CmdID, 1); break; } ChkSum += PresetNumber; for (idx = 0; idx < 120; idx++) { tempPset.ub[idx] = MessageStream_ReadByte(); ChkSum += tempPset.ub[idx]; } if (ChkSum == MessageStream_ReadByte()) { SavePreset(PresetNumber, (unsigned char *) &tempPset); AckCmd(CmdID); } else { NackCmd(CmdID, ChkSum); } break; case CMD_GET_CONFIG: ConfigPtr = (unsigned char *) &Config; TX_Idx = 0; TXBuffer[TX_Idx++] = '$'; MessageStream_WriteByte(CmdID); for (idx = 0; idx<sizeof (Config); idx++) { MessageStream_WriteByte((*ConfigPtr)); ChkSum += (*ConfigPtr); ConfigPtr++; } MessageStream_WriteByte(ChkSum); TXBuffer[TX_Idx++] = '#'; idx = TX_Idx; TX_Idx = 0; TX_bCount = idx; break; case CMD_SET_CONFIG: ConfigPtr = (unsigned char *) &TempConfig; for (idx = 0; idx < sizeof (Config); idx++) { (*ConfigPtr) = MessageStream_ReadByte(); ChkSum += (*ConfigPtr); ConfigPtr++; } if (ChkSum == MessageStream_ReadByte()) { addr=0; EEprom_write(&addr, (unsigned char *) &TempConfig, sizeof (TempConfig)); AckCmd(CmdID); } else { NackCmd(CmdID, ChkSum); } break; default: NackCmd(CmdID, 0xFF); } } return; }
unsigned char ExtMode() { UI_Location = UI_LOC_EXTMODE; unsigned char PathIdx = 0; unsigned char PathCount = 0; signed int PathDistances[100]; unsigned int PathTravelTimes[100]; unsigned int PathDwellTimes[100]; ExtModeActive = 1; signed char ret; unsigned char CmdID; unsigned char idx; unsigned char State = 0; MULTI mTemp; char PrepMove_Ready = 0; double PrepMove_Distance, PrepMove_Speed, PrepMove_Acceleration; double S, D, T; LCD_ClearDisplay(); LCD_PrintString("EXTERNAL MODE"); while (1) { ret = GetInput_nonblocking(); if (ret == USER_INPUT_CANCEL) { goto labelShutdown; } switch (State) { case 0: //Idle Mode - Nothing is happening... bMove_InProgress = 0; bFollowMode = 0; bSpeedMode = 0; break; case 1: //Stopping bMove_InProgress = 0; bFollowMode = 0; bSpeedMode = 1; Speed_SetToThis = 0; Speed_SetPending = 1; while(Speed_SetPending); if (!Speed_IsAccelerating) State = 0; break; case 2: //Single Move in Progress if (!bMove_InProgress) State = 1; break; case 3: //Moving to a Point along a Path; if (!bMove_InProgress) { Wait_seconds_nonblocking(PathDwellTimes[PathIdx]); PathIdx += 1; State = 4; } break; case 4: //Waiting for the Dwell time to expire for this waypoint... if (!bWaiting) { if (PathIdx == PathCount) { State = 1; } else { D = PathDistances[PathIdx]; T = PathTravelTimes[PathIdx]; S = SpeedRequiredToMoveInTime(D, T); Move(D, S); State = 3; } } break; } if (RX_MsgComplete) { ChkSum = 0; ReadIdx = 1; RX_MsgComplete = 0; if (MessageStream_ReadByte() == MyID) { CmdID = MessageStream_ReadByte(); switch (CmdID) { case CMD_PREP_MOVE: mTemp.ub[0] = MessageStream_ReadByte(); mTemp.ub[1] = MessageStream_ReadByte(); mTemp.ub[2] = MessageStream_ReadByte(); mTemp.ub[3] = MessageStream_ReadByte(); PrepMove_Distance = mTemp.dbl; ChkSum += mTemp.ub[3]; ChkSum += mTemp.ub[2]; ChkSum += mTemp.ub[1]; ChkSum += mTemp.ub[0]; mTemp.ub[0] = MessageStream_ReadByte(); mTemp.ub[1] = MessageStream_ReadByte(); mTemp.ub[2] = MessageStream_ReadByte(); mTemp.ub[3] = MessageStream_ReadByte(); PrepMove_Speed = mTemp.dbl; ChkSum += mTemp.ub[3]; ChkSum += mTemp.ub[2]; ChkSum += mTemp.ub[1]; ChkSum += mTemp.ub[0]; mTemp.ub[0] = MessageStream_ReadByte(); mTemp.ub[1] = MessageStream_ReadByte(); mTemp.ub[2] = MessageStream_ReadByte(); mTemp.ub[3] = MessageStream_ReadByte(); PrepMove_Acceleration = mTemp.dbl; ChkSum += mTemp.ub[3]; ChkSum += mTemp.ub[2]; ChkSum += mTemp.ub[1]; ChkSum += mTemp.ub[0]; if (ChkSum == MessageStream_ReadByte()) { PrepMove_Ready = 1; AckCmd(CmdID); } else { PrepMove_Ready = 0; NackCmd(CmdID, 0xFA); } break; case CMD_EXEC_MOVE: if (!PrepMove_Ready) { NackCmd(CmdID, 1); break; } if (State) { NackCmd(CmdID, 2); break; } MoveAdvanced(PrepMove_Distance, PrepMove_Speed, PrepMove_Acceleration); PrepMove_Ready = 0; State = 2; AckCmd(CmdID); break; case CMD_STOP: if (!State == 0) State = 1; AckCmd(CmdID); break; case CMD_STATUS: TX_Idx = 0; TXBuffer[TX_Idx++] = '$'; MessageStream_WriteByte(CmdID); MessageStream_WriteByte(State); ChkSum += State; MessageStream_WriteByte(PrepMove_Ready); ChkSum += PrepMove_Ready; bLock_Motor_Position = 1; mTemp.dbl = Motor_Position; bLock_Motor_Position = 0; mTemp.dbl *= Config.Degrees_Per_Count; MessageStream_WriteByte(mTemp.ub[0]); MessageStream_WriteByte(mTemp.ub[1]); MessageStream_WriteByte(mTemp.ub[2]); MessageStream_WriteByte(mTemp.ub[3]); ChkSum += mTemp.ub[0]; ChkSum += mTemp.ub[1]; ChkSum += mTemp.ub[2]; ChkSum += mTemp.ub[3]; mTemp.dbl = 0; while (!(mTemp.dbl == Move_speedQ24)) mTemp.dbl = Move_speedQ24; mTemp.dbl *= INV_Conversion_Q24_500Hz; mTemp.dbl *= Config.Degrees_Per_Count; MessageStream_WriteByte(mTemp.ub[0]); MessageStream_WriteByte(mTemp.ub[1]); MessageStream_WriteByte(mTemp.ub[2]); MessageStream_WriteByte(mTemp.ub[3]); ChkSum += mTemp.ub[0]; ChkSum += mTemp.ub[1]; ChkSum += mTemp.ub[2]; ChkSum += mTemp.ub[3]; bLock_Tick500Hz = 1; mTemp.dbl = Tick500Hz; bLock_Tick500Hz = 0; mTemp.dbl *= 0.002; MessageStream_WriteByte(mTemp.ub[0]); MessageStream_WriteByte(mTemp.ub[1]); MessageStream_WriteByte(mTemp.ub[2]); MessageStream_WriteByte(mTemp.ub[3]); ChkSum += mTemp.ub[0]; ChkSum += mTemp.ub[1]; ChkSum += mTemp.ub[2]; ChkSum += mTemp.ub[3]; bLock_BatteryVoltage = 1; mTemp.dbl = BatteryVoltage; bLock_BatteryVoltage = 0; mTemp.dbl *= Config.Volts_per_Count; MessageStream_WriteByte(mTemp.ub[0]); MessageStream_WriteByte(mTemp.ub[1]); MessageStream_WriteByte(mTemp.ub[2]); MessageStream_WriteByte(mTemp.ub[3]); ChkSum += mTemp.ub[0]; ChkSum += mTemp.ub[1]; ChkSum += mTemp.ub[2]; ChkSum += mTemp.ub[3]; MessageStream_WriteByte(ChkSum); TXBuffer[TX_Idx++] = '#'; idx = TX_Idx; TX_Idx = 0; TX_bCount = idx; break; case CMD_PATH_INIT: if ((State == 3) || (State == 4)) { NackCmd(CmdID, 1); break; } PathIdx = 0; PathCount = 0; AckCmd(CmdID); break; case CMD_PATH_ADD: if ((State == 3) || (State == 4)) { NackCmd(CmdID, 1); break; } if (PathCount > 99) { NackCmd(CmdID, 2); break; } mTemp.ub[0] = MessageStream_ReadByte(); mTemp.ub[1] = MessageStream_ReadByte(); ChkSum += mTemp.ub[0]; ChkSum += mTemp.ub[1]; PathDistances[PathCount] = mTemp.i[0]; mTemp.ub[0] = MessageStream_ReadByte(); mTemp.ub[1] = MessageStream_ReadByte(); ChkSum += mTemp.ub[0]; ChkSum += mTemp.ub[1]; PathTravelTimes[PathCount] = mTemp.ui[0]; mTemp.ub[0] = MessageStream_ReadByte(); mTemp.ub[1] = MessageStream_ReadByte(); ChkSum += mTemp.ub[0]; ChkSum += mTemp.ub[1]; PathDwellTimes[PathCount] = mTemp.ui[0]; if (ChkSum == MessageStream_ReadByte()) { AckCmd(CmdID); PathCount++; } else { PrepMove_Ready = 0; NackCmd(CmdID, 0xFA); } break; case CMD_PATH_RUN: if (!(State == 0)) { NackCmd(CmdID, 1); break; } PathIdx = 0; D = PathDistances[PathIdx]; T = PathTravelTimes[PathIdx]; S = SpeedRequiredToMoveInTime(D, T); Move(D, S); State = 3; AckCmd(CmdID); break; case CMD_EXIT: AckCmd(CmdID); goto labelShutdown; default: NackCmd(CmdID, 0xFE); break; } } } } labelShutdown: bMove_InProgress = 0; bFollowMode = 0; bSpeedMode = 1; Speed_SetToThis = 0; Speed_SetPending = 1; while (Speed_IsAccelerating) { idx = 0xff; while (idx--); } ExtModeActive = 0; return 0; }