/* TrainControlRCX V1.4 ----------------------------------------------------------------------------------------------- RobotC code for LEGO trains with an RCX PBrick present. This code runs on RCX PBricks (1.0, 1.5, 2.0) with the firmware "fast0788.lgo" present. This firmware ships with the programming environment RobotC 2.01 which is available as trial version for free at www.robotc.net. The PBricks are embedded in LEGO train platforms equipped with at least one 9V train motor (9V DC or RC train varieties). In combination with suitable control soft/hardware, several trains may be controlled simultaneously on the same track layout. The trains may be equipped with custom IR/RF transceivers for remote operation without line-of-sight. This is optional; the direct IR link works as well. With transceiver present, the RCX IR power level is set to low to avoid saturation of the transceiver IR electronics. Without IR/RF transceiver present, the IR power level is set to high. The program implements train motor power setting (forward/backward), PID speed control on/off, sound, handshake on/off, and one separate on/off output, e.g., for switching train head lights. A system byte containing relevant PBrick data (firmware version, #motors, etc.) is returned upon request ("polling"). A simple communication message protocol is implemented: bFlagLongMessages = false (default): Two consecutive individual messages (1 byte payload each) are transmitted from the host control site. The first message received by the PBrick = ID, next message = databyte (power change, sound, light on/off, etc.). bFlagLongMessages = true: One long message (payload = 1 byte (1 - 256) + 1 int (-32767 ... 32767) + 1 byte (-127 - + 127) is transmitted. This is for all-RobotC operation (i.e., RCX PBricks running host control programs). If the second message arrival times out, then either an error occurred or the one-byte message received is of type broadcast. Maximum time delay between Message 1 and 2 (bFlagLongMessages = false): cnMessageTimeOut [ms]. See include file TrainMessageDefs.h for details. |---------------------------------------------------------||----------------------------| | Sent to PBrick || Returned by PBrick | | Message 1 | Message 2 || Message | |=========================|===============================||============================| | Broadcast | n.a. || n.a. | |-------------------------|-------------------------------||----------------------------| | | || | | 1) cnHandshakeOn (240) | n.a. || n.a. | | 2) cnHandshakeOff (241) | n.a. || n.a. | | | || | |=========================|===============================||============================| | | || a) Acknowledgement | | ID | databyte || (=incremented databyte) | | | || b) Error message | | | || c) System databyte | |-------------------------|-------------------------------||----------------------------| | | RawData (range 1 - 95) || | | | || | | | Train speed ( 1 - 15) || a) or b) | | | PID derivative time (16 - 39) || a) or b) | | | PID reset rate (40 - 63) || a) or b) | | | PID gain (64 - 87) || a) or b) | | | PID loop wait time (88 - 95) || a) or b) | | (range 192 - 223) |-------------------------------||----------------------------| | | Command (range 96 - 127) || | | | || | | | TrainStop (96) || a) or b) | | | TrainLightOff (97) || a) or b) | | | TrainLightOn (98) || a) or b) | | | TrainSound (99) || a) or b) | | | TrainSoundT (100) || a) or b) | | | PollPBrickData (101) || c) or b) | | | TrainPIDControlOn (103) || a) or b) | | | TrainPIDControlOff (104) || a) or b) | | | || | |-------------------------|-------------------------------||----------------------------| Current error messages returned by PBricks are cnBusy (224) and cnInvalidDataByte (225). Note that the value passed to SendReplyMessage is always incremented before the message is transmitted to the host control program. Any software/hardware combination capable of generating the above message sequences can be used to control a number of trains running on the same track/track layout. RCX PBricks running a suitable control program and VisualBasic 6.0 SP5 in combination with the USB tower have been successfully tested as controllers. The old RS232 tower does not work directly with VB6/RobotC. However, with a suitable USB to RS232 converter, the old tower works with all Windows based programs tested (RobotC, NQC, BricxCC, VB6). Default communication is bi-directional; Train PBricks acknowledge received messages. The replies may be optionally decoded by the host control program(s). If the communication protocol is set to "no handshake" at the host control side, then the controller may send repeated messages. Already processed incoming messages are executed only once by the PBrick, even if multiple identical transmissions from the controller are received. Single commands (e.g., bell) require a "toggle bit" set/reset to work properly. Implementation here is simply command acknowledgement = command + 1. This is analogy to the LEGO(R) PowerFunctions protocol, but without a dedicated bit. Train motor power (nTargetPower) is transmitted within the range 1 ... 8 ... 15 with: 1 = full speed bwd; 8 = motors off; 15 = full speed fwd. Values 0 and 16 are not implemented. Trains run too fast and easily derail. The RobotC motor output power range is -100% ... 0% ... +100%. The PID setpoint range is -56% ... 0% ... +56% in increments of 4%. Direct motor power setting (without PID control) range is -65% ... 0% ... +65%. See function process_databyte`for details. Manual mode: nRampedPower slowly ramps to nTargetPower in task ramp_power; PID loop in manual mode then simply sets nOutputPower = nRampedPower. PID mode : nRampedPower ramps slowly to nTargetPower in task ramp_power and nOutputPower responds PID-controlled to changes in nRampedPower by monitoring nActualSpeed (PID input) and nPIDSpeedSetpoint (PID setpoint = nRampedPower). Currently both speed and power use the %scale. The speed measurement is experimentally calibrated to match the %scale using SPEED_CAL_FACTOR in task PID_speed_control. (There are two versions of the IR RF transceivers: An "old" TTL based and a "new" PIC based version. The old tranceivers draw more than 30 mA current and need to be powered from one RCX output (log. OUT_3 on RCX). The new transceivers draw less than 10 mA and may be powered from an active RCX sensor input.) Pre-processing macros (see individual train configuration files and default list below): Configurations: MOTORS 1 : 1 motor present on logical output 1. 2 : 2 motors present on logical outputs 1 and 2. RCX_OUT_3: ALWAYS_ON : Logical output 3 is always on (e.g., as permanent power source). SWITCH : Logical output 3 is switchable (e.g, for head lights). Logical-to-hardware mapping: OUT_1, OUT_2, OUT_3 motorA, -B, -C : Logical program output OUT_1, 2, 3 to RCX hardware motorA, B, C mapping. DIR_1, DIR_2 : Logical motor directions to real motor direction mapping (electric connector brick FWD, BWD orientation may be fixed due to spatial constraints). FWD: cmd forward = motor runs forward; BWD: cmd forward = motor runs backward. Note: bMotorReflected[motorX] = true does not work on RCX. ROT_SENSOR_IN_1, PWR_IN_2, PWR_IN_3 : Mapping of logical RCX inputs to actual train hardware setup. S1, S2, S3 Log. IN_1: Rotation sensor; log. IN_2: Power source; log. IN_3: Power source. PID_ROT_SENSOR_DIR : Logical to hardware mapping of the rotation sensor direction when the train moves forward. FWD, BWD Depending on the model layout, the rotation sensor may count up when moving forward (FWD) or count down (BWD). Other: IRRF_TRANSCEIVER_PRESENT TRUE, FALSE : TRUE: The IR TX power of the RCX is set to low. This avoids saturation of the IRRF transceiver's IR electronis. FALSE: IR TX power is set to high. ROT_SENSOR_PRESENT : TRUE: Switching between automatic (PID) and manual speed control is enabled. TRUE, FALSE FALSE: PID loop runs always in manual mode. ---------------------------------------------------------------------------------------------------------------------*/ #include "./include/TrainMacroDefs.h" //Train macro definitions for preprocessing. #include "./include/TrainMessageDefs.h" //Message protocol constants. #include "./include/TrainLCDErrorDefs.h" //LCD panel error code constants. #define FIRMWARE SWAN_ROC //For calculation of system databyte, see below. //Firmware types are defined in TrainControlDefs.h. #define TMR_MSG_TOUT T1 //Timer definitions for convenience: #define TMR_SPEED T2 //T1 = 2nd message timeout, T2 = speed measurement timer, #define TMR_REPLY_DLY T3 //T3 = reply message delay. //--------------------------------------------------------------------------------------------------------------------- //Train configuration section; MODIFY TO MATCH PRESENT HARDWARE SETUP //Select one of the alternative train configurations from the list (file location: in "./TrainConfigurationFiles") or //comment all list entries; in this case the default set below is used. //Current train list: //#define GP40_RCX //#define SANTA_FE //#define BR101 //#define BR23 #ifdef GP40_RCX #include "./TrainConfigurationFiles/GP40_RCX.h" #endif #ifdef SANTA_FE #include "./TrainConfigurationFiles/SANTA_FE.h" #endif #ifdef BR101 #include "./TrainConfigurationFiles/BR101.h" #endif #ifdef BR23 #include "./TrainConfigurationFiles/BR23.h" #endif //Default configuration set #ifndef CONFIGURATION_READ //Defined in train configuration files. If not defined, no train //was selected from the above list. //This set is (currently) identical to the GP40_RCX file. //Motor 1 on output A; motor 2 on output C; head light on output B. //Motor 1: real = logical directions; motor 2: inverted. //The new IR RF transceiver is present and powered from active input 2. //The rotation sensor is attached to input 3. #define MY_ID 192 //ID of train (incoming messages). #define RCX_OUT_3 SWITCH //Head light may be turned on/off. #define MOTORS 2 //2 motors present. #define OUT_1 motorA //Logical output 1 drives motor 1 on output A. #define OUT_2 motorC //Logical output 2 drives motor 2 on output C. #define OUT_3 motorB //Logical Output 3 drives on/off device (light) on output B. #define DIR_1 FWD //When output A is set to forward, motor runs forward. #define DIR_2 FWD //When output C is set to forward, motor runs forward. #define ROT_SENSOR_IN_1 S3 //Logical sensor 1 is on input 3. #define PWR_IN_2 S1 //Logical sensor 2 is on input 1. Configured as power source. #define PWR_IN_3 S2 //Logical sensor 3 is on input 2. Configured as power source. #define ROT_SENSOR_PRESENT TRUE //Enable/disable automatic/manual speed control. #define PID_ROT_SENSOR_DIR FWD //Rotation sensor counts up when train moves forward. #define IRRF_TRANSCEIVER_PRESENT TRUE //Set IR TX power to low. #endif //End of train configuration section ---------------------------------------------------------------------------------- /* Calculation of system data byte which is returned upon receiving a poll command. Used for remote train identification. Structure (bit 7 = high bit, bit 0 = low bit). C_PBRICK_IDENTIFICATION_DATABYTE should not be larger than PBrickErrorMessagesLBound (224) see VB6 control program code (mdiIDControl). SendReplyMessage increments nReply by one, thus 1 is subtracted in the calculation. (SWC = switch controllers separately used for multiple swittch point operation) Dec.: |--128--|---64--|---32--|---16--|---8---|---4---|---2---|---1---| Bit#: |---7---|---6---|---5---|---4---|---3---|---2---|---1---|---0---| Trains: | n.a. | n.a. | n.a. | n.a. |#motors| Firmware version | |-------|-------|-------|-------|-------|-----------------------| SWC: | n.a. | #Outputs | Firmware version | |-------|-------------------------------|-----------------------| For all PBricks: bit 0 - 2 : firmware version (1 ... 7). Need to start at 1 => return message is always > 0. SCOUT = 1, RCX2 = 2, SWAN_NQC = 3, SWAN_RoC = 4, SPY = 5, other = 6. Train PBricks: bit 3 : Number of motors; transmitted: 0 = 1 motor, 1 = 2 motors. SWC PBricks: bit 3 - 6 : Number of outputs (1 ... 16); transmitted: 0 = 1 output, ... , 15 = 16 outputs. bit 8 : Generally not used to avoid confusion with error messages etc. */ #define C_PBRICK_IDENTIFICATION_DATABYTE (FIRMWARE + (8*(MOTORS - 1)) - 1) //Program code ******************************************************************************************************** // //Global variables ---------------------------------------------------------------------------------------------------- int nTargetPower = 0; //Target speed as received by controller. int nRampedPower = 0; //PID setpoint/current speed setting. int nDisplay = 0; //Reset LCD display on startup. int nPIDSpeedSetpoint = 0; //Initialized for bumpless PID startup. Declared globally //since used in ramp_power and PID_speed_control. bool bFlagHandshake = false; //Handshake is off by default. bool bFlagPIDSpeedControl = false; //PID control is off by default. bool bFlagBusy = false; //Status of PBrick; if busy, new messages are rejected. bool bFlagLongMessages = false; //If true, protocol is one long message instead of two //consecutive one-byte messages. int nPIDLoopWaitTime = 60; //Minimum wait time before PID loop is executed [ms]. float fPIDGain = 1.3; //Experimental settings as defaults. Host control program may float fPIDResetRate = 0.4; //request changes of loop parameters. float fPIDDerivTime = 0.9; //Functions ----------------------------------------------------------------------------------------------------------- // //--------------------------------------------------------------------------------------------------------------------- void initialize() { #if IRRF_TRANSCEIVER_PRESENT == TRUE bLongRangeTransmitter = false; //For compatibility with all IR RF transceivers - they are #else //usually mounted close to the RCX/SCOUT IR window. bLongRangeTransmitter = true; //For direct IR communication. #endif SensorType[ROT_SENSOR_IN_1] = sensorRotation; //Speed measurement for PID control on logical input 1. SensorMode[ROT_SENSOR_IN_1] = modeRotation; SensorType[PWR_IN_2] = sensorReflection; //Power delivery to logical input 2 and 3 (e.g., as power SensorType[PWR_IN_3] = sensorReflection; //supply for the IR RF transceivers). SetUserDisplay(nDisplay); //Show program selected data in LCD display. bNoPowerDownOnACAdaptor = true; //If AC/DC is present, turn sleep timer off. bFloatDuringInactiveMotorPWM = true; //Set motors to coast when power is turned off. /* #if DIR_1 == -1 //Set-up motor direction as wired on model. bMotorReflected[OUT_1] = true; //bMotorReflected not recognized by RobotC in RCX mode. #endif //Control by software required. #if DIR_2 == -1 bMotorReflected[OUT_2] = true; #endif */ #if MOTORS == 1 //Log output 2 is powered up as source for other devices. motor[OUT_2] = 100; #endif #if RCX_OUT_3 == ALWAYS_ON //Log output 3 is powered up as source for other devices. motor[OUT_3] = 100; #endif #if RCX_OUT_3 == SWITCH //Switch log OUT_3 on/off at runtime. Default = off. motor[OUT_3] = 0; #endif } //--------------------------------------------------------------------------------------------------------------------- void SendReplyMessage(int nReply) //Acknowledge data byte or send error message. { const int cnReplyWaitTime = 300; //Wait time for message reply [ms]. if (bFlagHandshake) //If flag handshake is true, acknowledge messages. { //If false do nothing. //PlaySound(soundBlip); //Play sound for debugging purposes. ++nReply; //Increment data_byte. See comments in receive_message. while(time1[TMR_REPLY_DLY] < cnReplyWaitTime); //Required for Visual Basic's vpb.Monitor event generation to //catch up. This is a critical value. 300 ms delay appears to //be close to the the minimum wait time. Static wait 250 ms //works as well; timer operation results in reply time //optimization: If processing of databyte elsewhere takes longer, //the wait time here is "shortened" since timer4 is cleared //after message arrival is detected. sendMessageWithParm(nReply); //Send error code/acknowledgement. } } //--------------------------------------------------------------------------------------------------------------------- void ProcessDatabyte(int nDataByte) { nDisplay = nDataByte; //Show nDataByte in LCD display = default; may be overwritten //below. //-- Process commands ------------------------------ if (nDataByte >= cnLowerCmdRange) //Check lower command limit; if true, nDataByte = new command. { switch (nDataByte) //Browse through command space. { case cnTrainStop: //Immediate stop without ramping. nTargetPower = 0; //If nTargetPower = nRampedPower, the ramp_power loop is not nRampedPower = 1; //executed. nRampedPower = 1 causes an almost immediate stop. break; #if ROT_SENSOR_PRESENT == TRUE //If sensor is present, turn PID speed control on/off. case cnTrainPIDControlOn: PlaySound(soundUpwardTones); bFlagPIDSpeedControl = true; break; case cnTrainPIDControlOff: PlaySound(soundDownwardTones); bFlagPIDSpeedControl = false; break; #else //If sensor is not present, do nothing but generate case cnTrainPIDControlOn: //appropriate acknowledgement and sound feedback. PlaySound(soundLowBuzz); break; case cnTrainPIDControlOff: PlaySound(soundLowBuzz); break; #endif #if RCX_OUT_3 == SWITCH //Logical output 3 is not used as permanent power source. case cnTrainLightOff: //On/off switching is enabled. motor[OUT_3] = 0; //0 = off. break; case cnTrainLightOn: motor[OUT_3] = 100; //100 = full power level. break; #endif #if RCX_OUT_3 == ALWAYS_ON //Logical output 3 is used as permanent power source. case cnTrainLightOff: //Do nothing but generate appropriate acknowledgement. PlaySound(soundLowBuzz); break; case cnTrainLightOn: PlaySound(soundLowBuzz); break; #endif case cnTrainSound: //Sound generation. case cnTrainSoundT: //Same as above with toggle "bit" set. See comments in header PlayTone(1400,50); //section. PlayTone(1300,5); PlayTone(1200,20); PlayTone(1500,5); PlayTone(1400,100); break; default: nDataByte = cnInvalidDataByte; //Command byte was not recognized: Set error code. PlaySound(soundException); nDisplay = cnLCDErrorInvalidDatabyte; break; }//switch (nDataByte) goto SendReply; //Acknowledge and exit. }//if(nDataByte >= cnLowerCmdRange) //-- Process raw data ------------------------------ //PID parameters //The PID parameters are transmitted as rawdata. There is no if (nDataByte >= cnLowerSetPIDLoopWaitTimeRange) //dedicated bit-structure for each entry (PID loop delay, gain, { //deriv. time, reset rate). nPIDLoopWaitTime = (nDataByte - cnLowerSetPIDLoopWaitTimeRange) * 30; //nPIDLoopWaitTime range 8 * 30 ms slices = 240 ms. nDisplay = nPIDLoopWaitTime; //88 = 0, 89 = 30, 90 = 60, ..., 95 = 240 ms. PlaySound(soundUpwardTones); goto SendReply; } if (nDataByte >= cnLowerSetPIDGainRange) //Range: 64 = 0.0, 65 = 0.1, 66 = 0.2, ..., 87 = 2.3 { nDisplay = (nDataByte - cnLowerSetPIDGainRange); //0, 1, 2, ..., 23 fPIDGain = (float) nDisplay / 10; //0.0 ... 2.3; cast command required, otherwise integer PlaySound(soundUpwardTones); //math is used. goto SendReply; } if (nDataByte >= cnLowerSetPIDResetRateRange) //Range: 40 = 0.0, 41 = 0.1, 42 = 0.2, ..., 63 = 2.3 { nDisplay = (nDataByte - cnLowerSetPIDResetRateRange); fPIDResetRate = (float) nDisplay / 10; PlaySound(soundUpwardTones); goto SendReply; } if (nDataByte >= cnLowerSetPIDDerivTimeRange) //Range: 16 = 0.0, 17 = 0.1, 18 = 0.2, ..., 39 = 2.3 { nDisplay = (nDataByte - cnLowerSetPIDDerivTimeRange); fPIDDerivTime = (float) nDisplay / 10; PlaySound(soundUpwardTones); goto SendReply; } if (nDataByte > cnTrainPwrRange) //Check train control range (power = 1 ... 8 ... 15). { //Note: Checking not required, if cnLowerSetPIDDerivTimeRange //is identical with cnLowerSetPIDDerivTimeRange. nDataByte = cnInvalidDataByte; //Set error code. nDisplay = cnLCDErrorInvalidDatabyte; PlaySound(soundException); goto SendReply; } // Speed data ------------------------------------ nTargetPower = nDataByte - 8; //Shift target power range to -7 ... -1, 0, 1 ... + scale. if (bFlagPIDSpeedControl) //The firmware allows PWM power settings in steps of 4%: //0 ... 3 = 0%; 4 ... 7 = 4%; ... 99 = 96%; 100 = 100%. nTargetPower *= 8; //Settings below approx. 30% result in virtually no torque on //the LEGO 9V train motor; a train with some load does not move //at all. nTargetPower range: -56 ... -16, -8, 0, 8, 16 ... 56. else //No PID control. { nTargetPower *= 5; //Range without PID control: -35 ... -10, -5, 0, 5, 10 ... 35. if (nTargetPower < 0) nTargetPower -=30; //Add offset of 30 ("compensate" the torque issue manually). if (nTargetPower > 0) nTargetPower +=30; //Range in manual mode: -65 ... -40, -35, 0 , 35, 40 ... 65. } nDisplay = nTargetPower; //-- Acknowledge message --------------------------- SendReply: SendReplyMessage(nDataByte); //SendReplyMessage increments passed nDataByte value. bFlagBusy = false; }//void process_databyte //Tasks --------------------------------------------------------------------------------------------------------------- // //--------------------------------------------------------------------------------------------------------------------- task RampPower() { //Delay the power setting of the motors, e.g., upon setting the the power directly from idle to full. //Constants for calculation of non-linear time addition to the constant 200 ms wait time: const int cnLinearStartingPoint = 16; //[%] power at which simple linear increase starts. const int cnCorrectionFactor = 8; //Decrease calculated non-linear addition by this factor. const int cnLinearTimeSlice = 200; //Linear (minimum) wait time [ms]. int nSign; //RCX firmware has no built-in abs(x) or sgn(x) function. while (true) { if (nRampedPower == nTargetPower) //Nothing to do. continue; if (bFlagPIDSpeedControl) { if (nRampedPower < 0) nSign = -1; else nSign = 1; //Non-linear response at low power settings. nRampedPower rise //is less steep, which decreases slipping of the train motor //wheels when the train slowly starts to move. wait1Msec(cnLinearTimeSlice*(cnLinearStartingPoint-(nRampedPower*nSign))/cnCorrectionFactor+cnLinearTimeSlice); //wait1Msec(cnLinearTimeSlice); //Alternative linear function. } else //Manual speed control { if ((nRampedPower >= 28)||nRampedPower <= -28) //Don't wait with ramping in manual control if nRampedPower is wait1Msec(cnLinearTimeSlice); //< +/-28 (=> ramp to +/-28 as fast as possible; reason is the } //low torque delivered below 30% motor power). if (nRampedPower < nTargetPower) ++nRampedPower; //Range of ramped power is: -56 ... -1, 0, +1, ... +56. else --nRampedPower; //Small step size for "realistic" PID set point adjustment. //When the PID loop is in manual mode, nOutputPower is set to //nRampedPower within the PID loop. //nDisplay = nRampedPower; //Uncomment for watching nRampedPower in LCD display. This //constantly overwrites all other LCD data. nPIDSpeedSetpoint = nRampedPower; //Update PID setpoint for bumpless transfer from manual to PID } //mode. } //task ramp_power() //--------------------------------------------------------------------------------------------------------------------- task PIDSpeedControl() { /* PID loop. The algorithm is adapted from the eBook "The PID Control Algorithm - How it works, how to tune it, and how to use it" by John A. Shaw (2nd edition, 2005). Some data on the LEGO RCX rotoation sensor used for train speed measurement: - 16 ticks per 360 deg rotation. - 5.5 cm travel path per 360 deg rotation using the RC trainwheels = 3.4 mm per tick. */ //Local constants const int cnSpeedCalFactor = 320; //Calibration factor for rotation sensor reading/time[T2] => //speed conversion [% scale]. //Local variables //Variables used in referenced BASIC code. //--------------------------------------------------//--------------------------------------------------------------- float fActualSpeed; //Input [%]. float fActualSpeedD; //InputD. float fActualSpeedLast = 0; //InputLast; needs to be initialized for first PID loop. float fPIDError; //Err. float fPIDFeedBack = 0; //Feedback; needs to be initialized for first PID loop. float fOutputPower; //OutP. //PID_ROT_SENSOR_DIR //Action. //bFlagPIDSpeedControl //Mode. //not required (?) //OutPutTemp. //nPIDSpeedSetpoint //SetP. ClearTimer(TMR_SPEED); //Reset speed measurement timer 2 for initial PID loop access. SensorValue[ROT_SENSOR_IN_1] = 0; //Reset any accumulated rotation sensor ticks on log. input 1 //for initial PID loop access. //--- PID loop ------------------------------------------------------------------------------------------------------ while (true) { while (time1[TMR_SPEED] < nPIDLoopWaitTime); //Note: 0 ms loop time delay is too short to accumulate //meaningful rotation sensor tick readings at low speeds. fActualSpeed = SensorValue[ROT_SENSOR_IN_1] * cnSpeedCalFactor * PID_ROT_SENSOR_DIR / time1[TMR_SPEED]; //Speed calculation. ClearTimer(TMR_SPEED); //Reset speed measurent timer. SensorValue[ROT_SENSOR_IN_1] = 0; //Reset accumulated rotation sensor data. if (bFlagPIDSpeedControl) //PID algorithm ------------------------------------------------------------------------- { fActualSpeedD = fActualSpeed + (fActualSpeed - fActualSpeedLast) * fPIDDerivTime; fActualSpeedLast = fActualSpeed; //Store current speed value. fPIDError = nPIDSpeedSetpoint - fActualSpeedD; //nPIDSpeedSetpoint is calculated from nRampedPower. fOutputPower = (fPIDError * fPIDGain) + fPIDFeedBack; if (fOutputPower > 100) fOutputPower = 100; //Restrict nOutputPower to firmware power range. if (fOutputPower < -100) fOutputPower = -100; fPIDFeedBack += (fOutputPower - fPIDFeedBack) * fPIDResetRate; if ((nRampedPower == 0) && (fActualSpeed == 0)){//Required to fully remove power from the motors; at low output fOutputPower = 0; //power levels the train stops with zero PID loop error but fPIDFeedBack = 0; //power is still around 20 with no torque and speed = 0. fActualSpeedLast = 0; } } else //Manual speed control --------------------------------------------------------------------------------------- { fOutputPower = nRampedPower; //Set output power to ramped power as calcualted in task //ramp_power [% scale]. fPIDFeedBack = fOutputPower; //Store PIDFeedback and current speed for bumpless transfer from fActualSpeedLast = fActualSpeed; //manual into PID mode. } motor[OUT_1] = fOutputPower * DIR_1; //Set motor power, recognize motor wiring direction. #if MOTORS == 2 //motor[] accepts floats (rounded >down< to integers). motor[OUT_2] = fOutputPower * DIR_2; #endif }//while (true) }//task PID_speed_control //--------------------------------------------------------------------------------------------------------------------- task main() { /* Main program loop; task main directly processes and distributes all relevant information to other tasks depending on incoming messages. Message protocol: Incoming messages: Either two 1-byte messages (for compatibility with SCOUT, NQC and VB6) or one 2-byte message for all-RobotC operation, depending on bFlagLongMessages (false = two consecutive 1-byte messages). Outgoing (reply/acknowledgement) messages are always 1-byte long and of type: PBrick error, incremented system databyte, or incremented databyte for processed rawdata or commands. ID and databyte structure for bi-directional communication: Structure consist of upper 3 bits = data type and lower 5 bits = value. This way each type has its own unique range and provides more protocol saftey on the control program side. --------------------------------------------------------------------------------------------------------------- | Bit structure | Type | Base | Range | Comment | --------------------------------------------------------------------------------------------------------------- | 111 1 XXXX | Broadcast messages | 240 | 16 (240 - 255) | (No PBrick reply) | | 111 0 XXXX | Error messages | 224 | 16 (224 - 239) | | | 110 X XXXX | ID address range | 192 | 32 (192 - 223) | | | 101 X XXXX | (Not defined) | 160 | 32 (160 - 191) | | | 100 X XXXX | (Not defined) | 128 | 32 (128 - 159) | | | 011 X XXXX | Command | 96 | 31 ( 96 - 127) | | | 010 X XXXX | Rawdata | 64 | 31 ( 64 - 95) | | | 001 X XXXX | Rawdata | 32 | 32 ( 32 - 63) | | | 000 X XXXX | Rawdata | 1 | 31 ( 1 - 31) | | --------------------------------------------------------------------------------------------------------------- nDataByte is incremented in SendReplyMessage to avoid confusion with the tower echo upon error checking in the control program. Currently, en/decoded errors are: 223 + 1 = timeout (switch de-multiplexers only ); 224 + 1 = busy; 225 + 1 = invalid data_byte. See include file TrainMessageDefs.h */ //local variables int nIDorBroadcast; //Stores either PBrick ID or broadcast message. int nDataByte; //Second message byte received if <> 0. int nOldDataByte = 0; //Last second message byte received. initialize(); //Set program parameters depending on include file. StartTask(RampPower); //Delays speed settings (avoid speed "jumps"). StartTask(PIDSpeedControl); //Controls motor power settings. //-- Main loop ------------------------------------------------------------------------------------------------------ while (true) { ClearMessage(); //Flush message buffer. while (message == 0); //Wait for message. ClearTimer(TMR_REPLY_DLY); //Message arrived, reset TMR_REPLY_DLY. SendReplyMessage waits //until TMR_REPLY_DLY > cnReplyWaitTime ms before issuing a //reply message. //Store messages --------------------------------- if (bFlagLongMessages == true) //Currently always false. For all-RobotC operation only, all { //other PBrick types but NXT don't support this setting. nIDorBroadcast = messageParm[0]; //Store message data. nDataByte = messageParm[1]; } else //Default operation. { nIDorBroadcast = message; //Store first message byte. ClearMessage(); ClearTimer(TMR_MSG_TOUT); //Reset message timeout timer. while ((message == 0) && (time1[TMR_MSG_TOUT] < cnMessageTimeOut)); //Wait max. cnMessageTimeOut ms for second message. nDataByte = message; //Store (optional) second message byte. } //Process broadcasts ----------------------------- if ((nDataByte == 0) && (nIDorBroadcast >= cnLowerBroadcastRange)) { //Message sent is only 1 byte long within broadcast range. nDisplay = nIDorBroadcast; //Show message content in LCD display. switch (nIDorBroadcast) { case cnHandshakeOn: { bFlagHandshake = true; //Enable handshake (all PBricks). PlaySound(soundBlip); break; } case cnHandshakeOff: { bFlagHandshake = false; //Disable handshake (all PBricks). PlaySound(soundBlip); break; } default: //Error: Broadcast message not recognized. { nDisplay = cnLCDErrorInvalidBroadcast; //Show error code in LCD display. PlaySound(soundException); break; } } //There is no reply; the congested responses from several continue; //PBricks cannot be decoded. Start over. } else //Message sent is 2 bytes (ID + databyte) //Process ID and databyte -------------------------- { if (nIDorBroadcast != MY_ID) //Check message content with ID of this PBrick. continue; //If not true start over. if (bFlagBusy) //PBrick is busy. This occurs mainly on demultiplexers and { //hardly on trains. SendReplyMessage(cnBusy); PlaySound(soundBeepBeep); PlaySound(soundBeepBeep); nDisplay = cnLCDErrorPBrickBusy; continue; } if (nDataByte >= cnUpperCmdRange) //Out of command range. { SendReplyMessage(cnInvalidDataByte); PlaySound(soundException); nDisplay = cnLCDErrorInvalidDatabyte; continue; } if (nDataByte == cnPollPBrickData) //Handle poll command directly. { //Always return ID databyte. This eliminates problems with SendReplyMessage(C_PBRICK_IDENTIFICATION_DATABYTE); PlaySound(soundFastUpwardTones); //correct determination of the toggle "bit" for repeated nDisplay = nDataByte; //command transmission. continue; } //Discard repeated messages -------------------- if (nOldDataByte == nDataByte) //Disable multiple processing of power/command data. { //Note: Some commands (e.g., cmdBell) are always identical. SendReplyMessage(nDataByte); //In this case a toggle "bit" is set on the host side. continue; //With handshake operation enabled, repeated transmission } //of identical data means that either an error occurred locally //or the host controller missed the acknowledgement. The latter //is mostly the case. The message is not processed, just the //acknowledgement is sent. If handshake is disabled, repeated //messages are ignorded. //Process nDataByte ---------------------------- nOldDataByte = nDataByte; //Store new data_byte. bFlagBusy = true; //Signal busy condition to control program, if necessary. ProcessDatabyte(nDataByte); //bFlagBusy is reset after processing nDataByte. //No acknowledgement here; SendReplyMessage is called in //process_databyte. }//Processing of 2-byte message }//Mainloop }