ECE4333 - 2018 - Ahmed & Brandon / Mbed OS PlayBack

PlayBack

Dependencies:   TPixy-Interface

Fork of ManualControlFinal by ECE4333 - 2018 - Ahmed & Brandon

Drivers/PiController.cpp@8:a0890fa79084, 2018-02-19 (annotated)

Committer:
asobhy
Date:
Mon Feb 19 17:42:33 2018 +0000
Revision:
8:a0890fa79084
Parent:
7:73fd05fe556a
Child:
9:fe56b888985c
added more comments

Who changed what in which revision?

UserRevisionLine numberNew contents of line
asobhy 8:a0890fa79084 1 /******************************************************************************/
asobhy 8:a0890fa79084 2 // ECE4333
asobhy 8:a0890fa79084 3 // LAB Partner 1: Ahmed Sobhy - ID: 3594449
asobhy 8:a0890fa79084 4 // LAB Partner 2: Brandon Kingman - ID: 3470444
asobhy 8:a0890fa79084 5 // Project: Autonomous Robot Design
asobhy 8:a0890fa79084 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 7:73fd05fe556a 17
asobhy 7:73fd05fe556a 18 // global speed variable;
asobhy 7:73fd05fe556a 19 extern Mutex setpoint_mutex;
asobhy 7:73fd05fe556a 20 float Ki;
asobhy 7:73fd05fe556a 21 float Kp;
asobhy 7:73fd05fe556a 22 int32_t e, u, xState, 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 7:73fd05fe556a 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 7:73fd05fe556a 73 uint32_t PiController(int setp, int dP)
asobhy 7:73fd05fe556a 74 {
asobhy 7:73fd05fe556a 75
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 setpoint_mutex.lock();
asobhy 7:73fd05fe556a 82 e = SaturatingSubtract(setp, dP); // e is the velocity error
asobhy 7:73fd05fe556a 83 setpoint_mutex.unlock();
asobhy 7:73fd05fe556a 84
asobhy 7:73fd05fe556a 85 xTemp = SaturatingAdd(xState, e);
asobhy 7:73fd05fe556a 86
asobhy 7:73fd05fe556a 87 // the maximum value that 'u' can get to is 20
asobhy 7:73fd05fe556a 88 // the maximum value that dPosition can get to 560
asobhy 7:73fd05fe556a 89 // scaling factor is 560/20 = 28
asobhy 7:73fd05fe556a 90 // scaling factor used is 40
asobhy 7:73fd05fe556a 91
asobhy 7:73fd05fe556a 92 uProportional = (float)(Kp*e/scale);
asobhy 7:73fd05fe556a 93 uIntegral = (float)(Ki*xState/scale);
asobhy 7:73fd05fe556a 94
asobhy 7:73fd05fe556a 95 uS = SaturatingAdd(uProportional, uIntegral);
asobhy 7:73fd05fe556a 96
asobhy 7:73fd05fe556a 97 u = SaturateValue(uS, U_LIMIT);
asobhy 7:73fd05fe556a 98 if(u==uS) xState=xTemp; // if limit has not been reached then update xState
asobhy 7:73fd05fe556a 99
asobhy 7:73fd05fe556a 100 return u;
asobhy 7:73fd05fe556a 101
asobhy 7:73fd05fe556a 102 }