ECE4333 - 2018 - Ahmed & Brandon / Mbed OS MbedOS_Robot_Team

ManualControl

Dependencies:   TPixy-Interface

Fork of MbedOS_Robot by ECE4333 - 2018 - Ahmed & Brandon

Drivers/PiController.cpp@29:83c103d12078, 2018-04-10 (annotated)

Committer:
asobhy
Date:
Tue Apr 10 20:54:37 2018 +0000
Branch:
ManualControl
Revision:
29:83c103d12078
Parent:
22:c09acff62e6a 
ManualControl

Who changed what in which revision?

UserRevisionLine numberNew 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 20:9118203f7c9c 60 scale = 40;
asobhy 7:73fd05fe556a 61
asobhy 7:73fd05fe556a 62 }
asobhy 7:73fd05fe556a 63
asobhy 7:73fd05fe556a 64
asobhy 7:73fd05fe556a 65 /*******************************************************************************
asobhy 7:73fd05fe556a 66 * @brief PI Controller function
asobhy 7:73fd05fe556a 67 * @param setp is the setpoint we would like the system to get to
asobhy 7:73fd05fe556a 68 * @param dP is the current speed of the system
asobhy 7:73fd05fe556a 69 * @return u is the control signal out of the controller to make the system
asobhy 7:73fd05fe556a 70 * reach the desired setpoint
asobhy 7:73fd05fe556a 71 *******************************************************************************/
asobhy 9:fe56b888985c 72 uint32_t PiControllerR(int setp, int dP)
asobhy 7:73fd05fe556a 73 {
asobhy 9:fe56b888985c 74 static int32_t e, u;
asobhy 9:fe56b888985c 75 static int32_t xState = 0;
asobhy 7:73fd05fe556a 76 int32_t xTemp;
asobhy 7:73fd05fe556a 77 int32_t uProportional;
asobhy 7:73fd05fe556a 78 int32_t uIntegral;
asobhy 7:73fd05fe556a 79 int32_t uS;
asobhy 7:73fd05fe556a 80
asobhy 7:73fd05fe556a 81 e = SaturatingSubtract(setp, dP); // e is the velocity error
asobhy 7:73fd05fe556a 82
asobhy 7:73fd05fe556a 83 xTemp = SaturatingAdd(xState, e);
asobhy 7:73fd05fe556a 84
asobhy 7:73fd05fe556a 85 // the maximum value that 'u' can get to is 20
asobhy 7:73fd05fe556a 86 // the maximum value that dPosition can get to 560
asobhy 7:73fd05fe556a 87 // scaling factor is 560/20 = 28
asobhy 7:73fd05fe556a 88 // scaling factor used is 40
asobhy 7:73fd05fe556a 89
asobhy 7:73fd05fe556a 90 uProportional = (float)(Kp*e/scale);
asobhy 7:73fd05fe556a 91 uIntegral = (float)(Ki*xState/scale);
asobhy 7:73fd05fe556a 92
asobhy 7:73fd05fe556a 93 uS = SaturatingAdd(uProportional, uIntegral);
asobhy 7:73fd05fe556a 94
asobhy 7:73fd05fe556a 95 u = SaturateValue(uS, U_LIMIT);
asobhy 7:73fd05fe556a 96 if(u==uS) xState=xTemp; // if limit has not been reached then update xState
asobhy 7:73fd05fe556a 97
asobhy 7:73fd05fe556a 98 return u;
asobhy 7:73fd05fe556a 99
asobhy 7:73fd05fe556a 100 }
asobhy 9:fe56b888985c 101
asobhy 9:fe56b888985c 102
asobhy 9:fe56b888985c 103 /*******************************************************************************
asobhy 9:fe56b888985c 104 * @brief PI Controller function
asobhy 9:fe56b888985c 105 * @param setp is the setpoint we would like the system to get to
asobhy 9:fe56b888985c 106 * @param dP is the current speed of the system
asobhy 9:fe56b888985c 107 * @return u is the control signal out of the controller to make the system
asobhy 9:fe56b888985c 108 * reach the desired setpoint
asobhy 9:fe56b888985c 109 *******************************************************************************/
asobhy 9:fe56b888985c 110 uint32_t PiControllerL(int setp, int dP)
asobhy 9:fe56b888985c 111 {
asobhy 9:fe56b888985c 112 static int32_t e, u;
asobhy 9:fe56b888985c 113 static int32_t xState = 0;
asobhy 9:fe56b888985c 114 int32_t xTemp;
asobhy 9:fe56b888985c 115 int32_t uProportional;
asobhy 9:fe56b888985c 116 int32_t uIntegral;
asobhy 9:fe56b888985c 117 int32_t uS;
asobhy 9:fe56b888985c 118
asobhy 9:fe56b888985c 119 e = SaturatingSubtract(setp, dP); // e is the velocity error
asobhy 9:fe56b888985c 120
asobhy 9:fe56b888985c 121 xTemp = SaturatingAdd(xState, e);
asobhy 9:fe56b888985c 122
asobhy 9:fe56b888985c 123 // the maximum value that 'u' can get to is 20
asobhy 9:fe56b888985c 124 // the maximum value that dPosition can get to 560
asobhy 9:fe56b888985c 125 // scaling factor is 560/20 = 28
asobhy 9:fe56b888985c 126 // scaling factor used is 40
asobhy 9:fe56b888985c 127
asobhy 9:fe56b888985c 128 uProportional = (float)(Kp*e/scale);
asobhy 9:fe56b888985c 129 uIntegral = (float)(Ki*xState/scale);
asobhy 9:fe56b888985c 130
asobhy 9:fe56b888985c 131 uS = SaturatingAdd(uProportional, uIntegral);
asobhy 9:fe56b888985c 132
asobhy 9:fe56b888985c 133 u = SaturateValue(uS, U_LIMIT);
asobhy 9:fe56b888985c 134 if(u==uS) xState=xTemp; // if limit has not been reached then update xState
asobhy 9:fe56b888985c 135
asobhy 9:fe56b888985c 136 return u;
asobhy 9:fe56b888985c 137
asobhy 9:fe56b888985c 138 }