ECE4333 - 2018 - Ahmed & Brandon / Mbed OS PlayBack

PlayBack

Dependencies:   TPixy-Interface

Fork of ManualControlFinal by ECE4333 - 2018 - Ahmed & Brandon

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?

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 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 }