Controllers: PID, Sliding Mode, Root Locus, Ect...
th3jester
Posts: 81
Has there been any work on creating an object for different control algorithms? I've searched the OBEX and found a few PID objects, some I don't exactly trust. Is anyone interested in working on this with me? The need for accurate control in embedded systems is in high demand and should be used. A few, stated in the subject line, PID, Sliding Mode, Root Locus, and various others could be included. Being able to use Laplace transforms would be beneficial as it makes the PID and Root Locus very easy.
Any thoughts? suggestions?
Any thoughts? suggestions?
Comments
Controller.PID(Kp, Ki, Kd, @output)
The sliding mode and root locus will be similar if I can figure out how to code it. Others will be added once I discuss this with an expert in control systems.
EDIT: oopps.... I forget the set points and process variable, so it will look like this.
Controller.PID (SetPoint, ProcessVariable, Kp, Ki, Kd, @output)
I could have an Init method to set the Kp, Ki, and Kd gains yet this would limit the loop to only one process. Making it versatile on the fly is a big advantage.
This will be a very useful object once it's complete.
I have to do a little more research on sliding mode and root locus as it was a year ago that I took the class.
CON CLOCKS_PER_MICROSECOND = 5*16 ' simple xin*pll / 1_000_000 CLOCKS_PER_MILLISECOND = 5000*16 ' simple xin*pll / 1_000 HzToSec = 1.0 / 80_000_000.0 VAR long f32_Cmd byte cog PUB Stop if cog cogstop(cog~ - 1) PUB Start stop f32_Cmd := 0 return cog := cognew(@f32_entry, @f32_Cmd) + 1 PUB PID (Setpoint, ProcessVariable, Kp, Ki, Kd, output) | dt, dte, cur_dt, pre_dt, SP, PV, e, P, I, I_pre, D, ep cur_dt := cnt dte := cur_dt - pre_dt dte := FMul(FFloat(dte), HzToSec) e := FSub(SP, PV) 'CALCULATE ERROR P := FMul(Kp, e) 'PID I := FAdd(FMul(FMul(dte, FDiv(FAdd(e, ep), 2.0)), Ki), I_pre) I_pre := I D := FMul(Kd, FDiv(FSub(e, ep), dte)) long[output] := FRound(FAdd(P, FAdd(I, D))) ep := e pre_dt := cur_dt return PUB SMC 'Sliding Mode Control PUB RLC 'Root Locus Contrl PUB Laplace 'math for Laplace transform.....if possible PUB Complex_Num 'for Laplace transform PUB Polar_to_Rec 'for Laplace transform PUB Rec_to_Polar 'for Laplace transform PUB Cmd_ptr {{ return the Hub address of f32_Cmd, so other code can call it directly }} return @f32_Cmd PUB Call_ptr {{ return the Hub address of the dispatch table, so other code can call it directly }} return @cmdCallTable PUB FAdd(a, b) {{ Addition: result = a + b Parameters: a 32-bit floating point value b 32-bit floating point value Returns: 32-bit floating point value }} result := cmdFAdd f32_Cmd := @result repeat while f32_Cmd PUB FSub(a, b) {{ Subtraction: result = a - b Parameters: a 32-bit floating point value b 32-bit floating point value Returns: 32-bit floating point value }} result := cmdFSub f32_Cmd := @result repeat while f32_Cmd PUB FMul(a, b) {{ Multiplication: result = a * b Parameters: a 32-bit floating point value b 32-bit floating point value Returns: 32-bit floating point value }} result := cmdFMul f32_Cmd := @result repeat while f32_Cmd PUB FDiv(a, b) {{ Division: result = a / b Parameters: a 32-bit floating point value b 32-bit floating point value Returns: 32-bit floating point value }} result := cmdFDiv f32_Cmd := @result repeat while f32_Cmd PUB FFloat(n) {{ Convert integer to floating point. Parameters: n 32-bit integer value Returns: 32-bit floating point value }} result := cmdFFloat f32_Cmd := @result repeat while f32_Cmd PUB FRound(a) | b {{ Convert floating point to integer (with rounding). Parameters: a 32-bit floating point value b flag: 1 signifies rounding to the nearest integer Returns: 32-bit integer value }} b := %001 result := cmdFTruncRound f32_Cmd := @result repeat while f32_Cmd PUB Delay_MS( time ) {{ Delays "time" milliseconds and returns. }} waitcnt (cnt + time*CLOCKS_PER_MILLISECOND) ' // end Delay_MS ' ///////////////////////////////////////////////////////////////////////////// PUB Delay_US( time ) {{ Delays "time" microseconds and returns. }} waitcnt (cnt + time*CLOCKS_PER_MICROSECOND) ' // end Delay_US '////////////////////////////////////////////////////////////////////////////// CON SignFlag = $1 ZeroFlag = $2 NaNFlag = $8 DAT '---------------------------- ' Assembly language routines '---------------------------- '---------------------------- ' Main control loop '---------------------------- org 0 ' (try to keep 2 or fewer instructions between rd/wrlong) f32_entry rdlong ret_ptr, par wz ' wait for command to be non-zero, and store it in the call location if_z jmp #f32_entry rdlong :execCmd, ret_ptr ' get the pointer to the return value ("@result") add ret_ptr, #4 rdlong fNumA, ret_ptr ' fnumA is the long after "result" add ret_ptr, #4 rdlong fNumB, ret_ptr ' fnumB is the long after fnumA sub ret_ptr, #8 :execCmd nop ' execute command, which was replaced by getCommand :finishCmd wrlong fnumA, ret_ptr ' store the result (2 longs before fnumB) mov t1, #0 ' zero out the command register wrlong t1, par ' clear command status jmp #f32_entry ' wait for next command '---------------------------- ' addition and subtraction ' fnumA = fnumA +- fnumB '---------------------------- _FSub xor fnumB, Bit31 ' negate B jmp #_FAdd ' add values _FAdd call #_Unpack2 ' unpack two variables if_c_or_z jmp #_FAdd_ret ' check for NaN or B = 0 test flagA, #SignFlag wz ' negate A mantissa if negative if_nz neg manA, manA test flagB, #SignFlag wz ' negate B mantissa if negative if_nz neg manB, manB mov t1, expA ' align mantissas sub t1, expB abs t1, t1 wc max t1, #31 if_nc sar manB, t1 if_c sar manA, t1 if_c mov expA, expB add manA, manB ' add the two mantissas abs manA, manA wc ' store the absolte value, muxc flagA, #SignFlag ' and flag if it was negative call #_Pack ' pack result and exit _FSub_ret _FAdd_ret ret '---------------------------- ' multiplication ' fnumA *= fnumB '---------------------------- _FMul call #_Unpack2 ' unpack two variables if_c jmp #_FMul_ret ' check for NaN xor flagA, flagB ' get sign of result add expA, expB ' add exponents ' standard method: 404 counts for this block mov t1, #0 ' t1 is my accumulator mov t2, #24 ' loop counter for multiply (only do the bits needed...23 + implied 1) shr manB, #6 ' start by right aligning the B mantissa :multiply shr t1, #1 ' shift the previous accumulation down by 1 shr manB, #1 wc ' get multiplier bit if_c add t1, manA ' if the bit was set, add in the multiplicand djnz t2, #:multiply ' go back for more mov manA, t1 ' yes, that's my final answer. call #_Pack _FMul_ret ret '---------------------------- ' division ' fnumA /= fnumB '---------------------------- _FDiv call #_Unpack2 ' unpack two variables if_c_or_z mov fnumA, NaN ' check for NaN or divide by 0 if_c_or_z jmp #_FDiv_ret xor flagA, flagB ' get sign of result sub expA, expB ' subtract exponents ' slightly faster division, using 26 passes instead of 30 mov t1, #0 ' clear quotient mov t2, #26 ' loop counter for divide (need 24, plus 2 for rounding) :divide ' divide the mantissas cmpsub manA, manB wc rcl t1, #1 shl manA, #1 djnz t2, #:divide shl t1, #4 ' align the result (we did 26 instead of 30 iterations) mov manA, t1 ' get result and exit call #_Pack _FDiv_ret ret '------------------------------------------------------------------------------ ' fnumA = float(fnumA) '------------------------------------------------------------------------------ _FFloat abs manA, fnumA wc,wz ' get |integer value| if_z jmp #_FFloat_ret ' if zero, exit mov flagA, #0 ' set the sign flag muxc flagA, #SignFlag ' depending on the integer's sign mov expA, #29 ' set my exponent call #_Pack ' pack and exit _FFloat_ret ret '------------------------------------------------------------------------------ ' rounding and truncation ' fnumB controls the output format: ' %00 = integer, truncate ' %01 = integer, round ' %10 = float, truncate ' %11 = float, round '------------------------------------------------------------------------------ _FTruncRound mov t1, fnumA ' grab a copy of the input call #_Unpack ' unpack floating point value ' Are we going for float or integer? cmpsub fnumB, #%10 wc ' clear bit 1 and set the C flag if it was a 1 rcl t2, #1 and t2, #1 wz ' Z now signified integer output shl manA, #2 ' left justify mantissa sub expA, #30 ' our target exponent is 30 abs expA, expA wc ' adjust for exponent sign, and track if it was negative if_z_and_nc mov manA, NaN ' integer output, and it's too large for us to handle if_z_and_nc jmp #:check_sign if_nz_and_nc mov fnumA, t1 ' float output, and we're already all integer if_nz_and_nc jmp #_FTruncRound_ret ' well, I need to kill off some bits, so let's do it max expA, #31 wc shr manA, expA if_c add manA, fnumB ' round up 1/2 lsb if desired, and if it isn't supposed to be 0! (if expA was > 31) shr manA, #1 if_z jmp #:check_sign ' integer output? mov expA, #29 call #_Pack jmp #_FTruncRound_ret :check_sign test flagA, #signFlag wz ' check sign and exit negnz fnumA, manA _FTruncRound_ret ret '------------------------------------------------------------------------------ ' truncation to unsigned integer ' fnumA = unsigned int(fnumA), clamped to 0 '------------------------------------------------------------------------------ _UintTrunc call #_Unpack mov fnumA, #0 test flagA, #SignFlag wc if_c_or_z jmp #_UintTrunc_ret ' if the input number was negative or zero, we're done shl manA, #2 ' left justify mantissa sub expA, #31 ' our target exponent is 31 abs expA, expA wc,wz if_a neg fnumA, #1 ' if we needed to shift left, we're already maxed out if_be cmp expA, #32 wc ' otherwise, if we need to shift right by more than 31, the answer is 0 if_c shr manA, expA ' OK, shift it down if_c mov fnumA, manA _UintTrunc_ret ret '------------------------------------------------------------------------------ ' input: fnumA 32-bit floating point value ' fnumB 32-bit floating point value ' output: flagA fnumA flag bits (Nan, Infinity, Zero, Sign) ' expA fnumA exponent (no bias) ' manA fnumA mantissa (aligned to bit 29) ' flagB fnumB flag bits (Nan, Infinity, Zero, Sign) ' expB fnumB exponent (no bias) ' manB fnumB mantissa (aligned to bit 29) ' C flag set if fnumA or fnumB is NaN ' Z flag set if fnumB is zero ' changes: fnumA, flagA, expA, manA, fnumB, flagB, expB, manB, t1 '------------------------------------------------------------------------------ _Unpack2 mov t1, fnumA ' save A mov fnumA, fnumB ' unpack B to A call #_Unpack if_c jmp #_Unpack2_ret ' check for NaN mov fnumB, fnumA ' save B variables mov flagB, flagA mov expB, expA mov manB, manA mov fnumA, t1 ' unpack A call #_Unpack cmp manB, #0 wz ' set Z flag _Unpack2_ret ret '------------------------------------------------------------------------------ ' input: fnumA 32-bit floating point value ' output: flagA fnumA flag bits (Nan, Infinity, Zero, Sign) ' expA fnumA exponent (no bias) ' manA fnumA mantissa (aligned to bit 29) ' C flag set if fnumA is NaN ' Z flag set if fnumA is zero ' changes: fnumA, flagA, expA, manA '------------------------------------------------------------------------------ _Unpack mov flagA, fnumA ' get sign shr flagA, #31 mov manA, fnumA ' get mantissa and manA, Mask23 mov expA, fnumA ' get exponent shl expA, #1 shr expA, #24 wz if_z jmp #:zeroSubnormal ' check for zero or subnormal cmp expA, #255 wz ' check if finite if_nz jmp #:finite mov fnumA, NaN ' no, then return NaN mov flagA, #NaNFlag jmp #:exit2 :zeroSubnormal or manA, expA wz,nr ' check for zero if_nz jmp #:subnorm or flagA, #ZeroFlag ' yes, then set zero flag neg expA, #150 ' set exponent and exit jmp #:exit2 :subnorm shl manA, #7 ' fix justification for subnormals :subnorm2 test manA, Bit29 wz if_nz jmp #:exit1 shl manA, #1 sub expA, #1 jmp #:subnorm2 :finite shl manA, #6 ' justify mantissa to bit 29 or manA, Bit29 ' add leading one bit :exit1 sub expA, #127 ' remove bias from exponent :exit2 test flagA, #NaNFlag wc ' set C flag cmp manA, #0 wz ' set Z flag _Unpack_ret ret '------------------------------------------------------------------------------ ' input: flagA fnumA flag bits (Nan, Infinity, Zero, Sign) ' expA fnumA exponent (no bias) ' manA fnumA mantissa (aligned to bit 29) ' output: fnumA 32-bit floating point value ' changes: fnumA, flagA, expA, manA '------------------------------------------------------------------------------ _Pack cmp manA, #0 wz ' check for zero if_z mov expA, #0 if_z jmp #:exit1 sub expA, #380 ' take us out of the danger range for djnz :normalize shl manA, #1 wc ' normalize the mantissa if_nc djnz expA, #:normalize ' adjust exponent and jump add manA, #$100 wc ' round up by 1/2 lsb addx expA, #(380 + 127 + 2) ' add bias to exponent, account for rounding (in flag C, above) mins expA, Minus23 maxs expA, #255 abs expA, expA wc,wz ' check for subnormals, and get the abs in case it is if_a jmp #:exit1 :subnormal or manA, #1 ' adjust mantissa ror manA, #1 shr manA, expA mov expA, #0 ' biased exponent = 0 :exit1 mov fnumA, manA ' bits 22:0 mantissa shr fnumA, #9 movi fnumA, expA ' bits 23:30 exponent shl flagA, #31 or fnumA, flagA ' bit 31 sign _Pack_ret ret '-------------------- constant values ----------------------------------------- One long 1.0 NaN long $7FFF_FFFF Minus23 long -23 Mask23 long $007F_FFFF Mask29 long $1FFF_FFFF Bit29 long $2000_0000 Bit30 long $4000_0000 Bit31 long $8000_0000 LogTable long $C000 ALogTable long $D000 SineTable long $E000 '-------------------- initialized variables ----------------------------------- '-------------------- local variables ----------------------------------------- ret_ptr res 1 t1 res 1 t2 res 1 t3 res 1 t4 res 1 t5 res 1 t6 res 1 t7 res 1 t8 res 1 fnumA res 1 ' floating point A value flagA res 1 expA res 1 manA res 1 fnumB res 1 ' floating point B value flagB res 1 expB res 1 manB res 1 fit $1F0 ' A cog has 496 longs available, the last 16 (to make it up to 512) are register shadows. ' command dispatch table: must be compiled along with PASM code in ' Cog RAM to know the addresses, but does not neet to fit in it. cmdCallTable cmdFAdd call #_FAdd cmdFSub call #_FSub cmdFMul call #_FMul cmdFDiv call #_FDiv cmdFFloat call #_FFloat cmdFTruncRound call #_FTruncRound cmdUintTrunc call #_UintTrunc {cmdFSqr call #_FSqr cmdFCmp call #_FCmp cmdFSin call #_Sin cmdFCos call #_Cos cmdFTan call #_Tan cmdFLog2 call #_Log2 cmdFExp2 call #_Exp2 cmdFPow call #_Pow cmdFFrac call #_Frac cmdFMod call #_FMod cmdASinCos call #_ASinCos cmdATan2 call #_ATan2 cmdCeil call #_Ceil cmdFloor call #_Floor }EDIT: I have almost immediately regretted taking on Sliding Mode Control. After reading a few papers on it and realized how advanced the control may get, I believe this will be the last implementation of the object. The PID will be completely optimized and accurate as well as the Root Locus control. Other simpler controls will be added upon suggestion and discovery.
I have realized that the need for the process function is a high priority. So a method will need to be created to identify a pseudo-code equation with given inputs and variables i.e :
Controllers.SMC(x^2 + ax + a, alpha1, alpha2, alpha3)
where the process is x^2 + ax + a
and the gains are alpha1, alpha2, alpha3.
This is just an example of what need to be done, not the actual code required. Because for a system you have a process and a controller. The function of a system is the process * the controller ( in real simplified terms). TF = Gp * Gc