![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
ObjectFollower
Dependencies: TPixy-Interface
Fork of PlayBack by
Drivers/PiController.cpp@26:e93890381e66, 2018-04-03 (annotated)
- Committer:
- asobhy
- Date:
- Tue Apr 03 16:52:38 2018 +0000
- Revision:
- 26:e93890381e66
- Parent:
- 16:58ec2b891a25
OBJECT FOLLOWER Latest
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
asobhy | 8:a0890fa79084 | 1 | /******************************************************************************/ |
asobhy | 8:a0890fa79084 | 2 | // ECE4333 |
asobhy | 9:fe56b888985c | 3 | // LAB Partner 1: Ahmed Sobhy - ID: 3594449 |
asobhy | 9:fe56b888985c | 4 | // LAB Partner 2: Brandon Kingman - ID: 3470444 |
asobhy | 9:fe56b888985c | 5 | // Project: Autonomous Robot Design |
asobhy | 9:fe56b888985c | 6 | // Instructor: Prof. Chris Diduch |
asobhy | 8:a0890fa79084 | 7 | /******************************************************************************/ |
asobhy | 8:a0890fa79084 | 8 | // filename: PiController.cpp |
asobhy | 8:a0890fa79084 | 9 | // file content description: |
asobhy | 8:a0890fa79084 | 10 | // * PiController initialization function |
asobhy | 8:a0890fa79084 | 11 | // * Picontroller function |
asobhy | 8:a0890fa79084 | 12 | // * Value Saturation functions |
asobhy | 8:a0890fa79084 | 13 | /******************************************************************************/ |
asobhy | 8:a0890fa79084 | 14 | |
asobhy | 7:73fd05fe556a | 15 | #include "mbed.h" |
asobhy | 7:73fd05fe556a | 16 | #include "PiController.h" |
asobhy | 15:cf67f83d5409 | 17 | #include "ui.h" |
asobhy | 7:73fd05fe556a | 18 | |
asobhy | 7:73fd05fe556a | 19 | // global speed variable; |
asobhy | 7:73fd05fe556a | 20 | float Ki; |
asobhy | 7:73fd05fe556a | 21 | float Kp; |
asobhy | 9:fe56b888985c | 22 | int32_t scale; |
asobhy | 7:73fd05fe556a | 23 | |
asobhy | 7:73fd05fe556a | 24 | /******************************************************************************/ |
asobhy | 7:73fd05fe556a | 25 | int SaturatingSubtract(int x, int y) |
asobhy | 7:73fd05fe556a | 26 | { |
asobhy | 7:73fd05fe556a | 27 | int z; |
asobhy | 7:73fd05fe556a | 28 | z = x - y; // 32-bit overflow detection and saturating arithmetic |
asobhy | 7:73fd05fe556a | 29 | if((x > 0) && (y < 0) && (z < 0)) z = 0x7FFFFFFF; |
asobhy | 7:73fd05fe556a | 30 | else if((x < 0) && (y > 0) && (z > 0)) z = 0x80000000; |
asobhy | 7:73fd05fe556a | 31 | return z; |
asobhy | 7:73fd05fe556a | 32 | } |
asobhy | 7:73fd05fe556a | 33 | |
asobhy | 7:73fd05fe556a | 34 | /******************************************************************************/ |
asobhy | 7:73fd05fe556a | 35 | int SaturatingAdd(int x, int y) |
asobhy | 7:73fd05fe556a | 36 | { |
asobhy | 7:73fd05fe556a | 37 | int z; |
asobhy | 7:73fd05fe556a | 38 | z = x + y; // 32-bit overflow detection and saturating arithmetic |
asobhy | 7:73fd05fe556a | 39 | if((x > 0) && (y > 0) && (z < 0)) z = 0x7FFFFFFF; |
asobhy | 7:73fd05fe556a | 40 | else if((x < 0) && (y < 0) && (z > 0)) z = 0x80000000; |
asobhy | 7:73fd05fe556a | 41 | return z; |
asobhy | 7:73fd05fe556a | 42 | } |
asobhy | 7:73fd05fe556a | 43 | |
asobhy | 7:73fd05fe556a | 44 | /******************************************************************************/ |
asobhy | 7:73fd05fe556a | 45 | int SaturateValue(int x, int Limit) |
asobhy | 7:73fd05fe556a | 46 | { |
asobhy | 7:73fd05fe556a | 47 | if(x > Limit) return(Limit); // Impose maximum limit on x |
asobhy | 7:73fd05fe556a | 48 | else if(x < -Limit) return(-Limit); |
asobhy | 7:73fd05fe556a | 49 | else return(x); |
asobhy | 7:73fd05fe556a | 50 | } |
asobhy | 7:73fd05fe556a | 51 | |
asobhy | 7:73fd05fe556a | 52 | |
asobhy | 7:73fd05fe556a | 53 | /*****************************************************************************/ |
asobhy | 7:73fd05fe556a | 54 | void PiController_init(float Kp_given, float Ki_given) |
asobhy | 7:73fd05fe556a | 55 | { |
asobhy | 7:73fd05fe556a | 56 | Kp = Kp_given; |
asobhy | 7:73fd05fe556a | 57 | Ki = Ki_given; |
asobhy | 7:73fd05fe556a | 58 | |
asobhy | 7:73fd05fe556a | 59 | // initialization |
asobhy | 7:73fd05fe556a | 60 | scale = 40; |
asobhy | 9:fe56b888985c | 61 | //xState = 0; |
asobhy | 7:73fd05fe556a | 62 | |
asobhy | 7:73fd05fe556a | 63 | } |
asobhy | 7:73fd05fe556a | 64 | |
asobhy | 7:73fd05fe556a | 65 | |
asobhy | 7:73fd05fe556a | 66 | /******************************************************************************* |
asobhy | 7:73fd05fe556a | 67 | * @brief PI Controller function |
asobhy | 7:73fd05fe556a | 68 | * @param setp is the setpoint we would like the system to get to |
asobhy | 7:73fd05fe556a | 69 | * @param dP is the current speed of the system |
asobhy | 7:73fd05fe556a | 70 | * @return u is the control signal out of the controller to make the system |
asobhy | 7:73fd05fe556a | 71 | * reach the desired setpoint |
asobhy | 7:73fd05fe556a | 72 | *******************************************************************************/ |
asobhy | 9:fe56b888985c | 73 | uint32_t PiControllerR(int setp, int dP) |
asobhy | 7:73fd05fe556a | 74 | { |
asobhy | 9:fe56b888985c | 75 | static int32_t e, u; |
asobhy | 9:fe56b888985c | 76 | static int32_t xState = 0; |
asobhy | 7:73fd05fe556a | 77 | int32_t xTemp; |
asobhy | 7:73fd05fe556a | 78 | int32_t uProportional; |
asobhy | 7:73fd05fe556a | 79 | int32_t uIntegral; |
asobhy | 7:73fd05fe556a | 80 | int32_t uS; |
asobhy | 7:73fd05fe556a | 81 | |
asobhy | 7:73fd05fe556a | 82 | e = SaturatingSubtract(setp, dP); // e is the velocity error |
asobhy | 7:73fd05fe556a | 83 | |
asobhy | 7:73fd05fe556a | 84 | xTemp = SaturatingAdd(xState, e); |
asobhy | 7:73fd05fe556a | 85 | |
asobhy | 7:73fd05fe556a | 86 | // the maximum value that 'u' can get to is 20 |
asobhy | 7:73fd05fe556a | 87 | // the maximum value that dPosition can get to 560 |
asobhy | 7:73fd05fe556a | 88 | // scaling factor is 560/20 = 28 |
asobhy | 7:73fd05fe556a | 89 | // scaling factor used is 40 |
asobhy | 7:73fd05fe556a | 90 | |
asobhy | 7:73fd05fe556a | 91 | uProportional = (float)(Kp*e/scale); |
asobhy | 7:73fd05fe556a | 92 | uIntegral = (float)(Ki*xState/scale); |
asobhy | 7:73fd05fe556a | 93 | |
asobhy | 7:73fd05fe556a | 94 | uS = SaturatingAdd(uProportional, uIntegral); |
asobhy | 7:73fd05fe556a | 95 | |
asobhy | 7:73fd05fe556a | 96 | u = SaturateValue(uS, U_LIMIT); |
asobhy | 7:73fd05fe556a | 97 | if(u==uS) xState=xTemp; // if limit has not been reached then update xState |
asobhy | 7:73fd05fe556a | 98 | |
asobhy | 7:73fd05fe556a | 99 | return u; |
asobhy | 7:73fd05fe556a | 100 | |
asobhy | 7:73fd05fe556a | 101 | } |
asobhy | 9:fe56b888985c | 102 | |
asobhy | 9:fe56b888985c | 103 | |
asobhy | 9:fe56b888985c | 104 | /******************************************************************************* |
asobhy | 9:fe56b888985c | 105 | * @brief PI Controller function |
asobhy | 9:fe56b888985c | 106 | * @param setp is the setpoint we would like the system to get to |
asobhy | 9:fe56b888985c | 107 | * @param dP is the current speed of the system |
asobhy | 9:fe56b888985c | 108 | * @return u is the control signal out of the controller to make the system |
asobhy | 9:fe56b888985c | 109 | * reach the desired setpoint |
asobhy | 9:fe56b888985c | 110 | *******************************************************************************/ |
asobhy | 9:fe56b888985c | 111 | uint32_t PiControllerL(int setp, int dP) |
asobhy | 9:fe56b888985c | 112 | { |
asobhy | 9:fe56b888985c | 113 | static int32_t e, u; |
asobhy | 9:fe56b888985c | 114 | static int32_t xState = 0; |
asobhy | 9:fe56b888985c | 115 | int32_t xTemp; |
asobhy | 9:fe56b888985c | 116 | int32_t uProportional; |
asobhy | 9:fe56b888985c | 117 | int32_t uIntegral; |
asobhy | 9:fe56b888985c | 118 | int32_t uS; |
asobhy | 9:fe56b888985c | 119 | |
asobhy | 9:fe56b888985c | 120 | e = SaturatingSubtract(setp, dP); // e is the velocity error |
asobhy | 9:fe56b888985c | 121 | |
asobhy | 9:fe56b888985c | 122 | xTemp = SaturatingAdd(xState, e); |
asobhy | 9:fe56b888985c | 123 | |
asobhy | 9:fe56b888985c | 124 | // the maximum value that 'u' can get to is 20 |
asobhy | 9:fe56b888985c | 125 | // the maximum value that dPosition can get to 560 |
asobhy | 9:fe56b888985c | 126 | // scaling factor is 560/20 = 28 |
asobhy | 9:fe56b888985c | 127 | // scaling factor used is 40 |
asobhy | 9:fe56b888985c | 128 | |
asobhy | 9:fe56b888985c | 129 | uProportional = (float)(Kp*e/scale); |
asobhy | 9:fe56b888985c | 130 | uIntegral = (float)(Ki*xState/scale); |
asobhy | 9:fe56b888985c | 131 | |
asobhy | 9:fe56b888985c | 132 | uS = SaturatingAdd(uProportional, uIntegral); |
asobhy | 9:fe56b888985c | 133 | |
asobhy | 9:fe56b888985c | 134 | u = SaturateValue(uS, U_LIMIT); |
asobhy | 9:fe56b888985c | 135 | if(u==uS) xState=xTemp; // if limit has not been reached then update xState |
asobhy | 9:fe56b888985c | 136 | |
asobhy | 9:fe56b888985c | 137 | return u; |
asobhy | 9:fe56b888985c | 138 | |
asobhy | 9:fe56b888985c | 139 | } |