ECE4333 - 2018 - Ahmed & Brandon / Mbed OS ManualControlFinal

ManualControl

Dependencies:   TPixy-Interface

Fork of MbedOS_Robot_Team by ECE4333 - 2018 - Ahmed & Brandon

Drivers/PiController.cpp@9:fe56b888985c, 2018-02-23 (annotated)

Committer:
asobhy
Date:
Fri Feb 23 20:58:34 2018 +0000
Revision:
9:fe56b888985c
Parent:
8:a0890fa79084
Child:
15:cf67f83d5409
right after the two motors are running

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 7:73fd05fe556a 17
asobhy 7:73fd05fe556a 18 // global speed variable;
asobhy 9:fe56b888985c 19 extern Mutex setpointR_mutex;
asobhy 9:fe56b888985c 20 extern Mutex setpointL_mutex;
asobhy 7:73fd05fe556a 21 float Ki;
asobhy 7:73fd05fe556a 22 float Kp;
asobhy 9:fe56b888985c 23 int32_t scale;
asobhy 7:73fd05fe556a 24
asobhy 7:73fd05fe556a 25 /******************************************************************************/
asobhy 7:73fd05fe556a 26 int SaturatingSubtract(int x, int y)
asobhy 7:73fd05fe556a 27 {
asobhy 7:73fd05fe556a 28 int z;
asobhy 7:73fd05fe556a 29 z = x - y; // 32-bit overflow detection and saturating arithmetic
asobhy 7:73fd05fe556a 30 if((x > 0) && (y < 0) && (z < 0)) z = 0x7FFFFFFF;
asobhy 7:73fd05fe556a 31 else if((x < 0) && (y > 0) && (z > 0)) z = 0x80000000;
asobhy 7:73fd05fe556a 32 return z;
asobhy 7:73fd05fe556a 33 }
asobhy 7:73fd05fe556a 34
asobhy 7:73fd05fe556a 35 /******************************************************************************/
asobhy 7:73fd05fe556a 36 int SaturatingAdd(int x, int y)
asobhy 7:73fd05fe556a 37 {
asobhy 7:73fd05fe556a 38 int z;
asobhy 7:73fd05fe556a 39 z = x + y; // 32-bit overflow detection and saturating arithmetic
asobhy 7:73fd05fe556a 40 if((x > 0) && (y > 0) && (z < 0)) z = 0x7FFFFFFF;
asobhy 7:73fd05fe556a 41 else if((x < 0) && (y < 0) && (z > 0)) z = 0x80000000;
asobhy 7:73fd05fe556a 42 return z;
asobhy 7:73fd05fe556a 43 }
asobhy 7:73fd05fe556a 44
asobhy 7:73fd05fe556a 45 /******************************************************************************/
asobhy 7:73fd05fe556a 46 int SaturateValue(int x, int Limit)
asobhy 7:73fd05fe556a 47 {
asobhy 7:73fd05fe556a 48 if(x > Limit) return(Limit); // Impose maximum limit on x
asobhy 7:73fd05fe556a 49 else if(x < -Limit) return(-Limit);
asobhy 7:73fd05fe556a 50 else return(x);
asobhy 7:73fd05fe556a 51 }
asobhy 7:73fd05fe556a 52
asobhy 7:73fd05fe556a 53
asobhy 7:73fd05fe556a 54 /*****************************************************************************/
asobhy 7:73fd05fe556a 55 void PiController_init(float Kp_given, float Ki_given)
asobhy 7:73fd05fe556a 56 {
asobhy 7:73fd05fe556a 57 Kp = Kp_given;
asobhy 7:73fd05fe556a 58 Ki = Ki_given;
asobhy 7:73fd05fe556a 59
asobhy 7:73fd05fe556a 60 // initialization
asobhy 7:73fd05fe556a 61 scale = 40;
asobhy 9:fe56b888985c 62 //xState = 0;
asobhy 7:73fd05fe556a 63
asobhy 7:73fd05fe556a 64 }
asobhy 7:73fd05fe556a 65
asobhy 7:73fd05fe556a 66
asobhy 7:73fd05fe556a 67 /*******************************************************************************
asobhy 7:73fd05fe556a 68 * @brief PI Controller function
asobhy 7:73fd05fe556a 69 * @param setp is the setpoint we would like the system to get to
asobhy 7:73fd05fe556a 70 * @param dP is the current speed of the system
asobhy 7:73fd05fe556a 71 * @return u is the control signal out of the controller to make the system
asobhy 7:73fd05fe556a 72 * reach the desired setpoint
asobhy 7:73fd05fe556a 73 *******************************************************************************/
asobhy 9:fe56b888985c 74 uint32_t PiControllerR(int setp, int dP)
asobhy 7:73fd05fe556a 75 {
asobhy 9:fe56b888985c 76 static int32_t e, u;
asobhy 9:fe56b888985c 77 static int32_t xState = 0;
asobhy 7:73fd05fe556a 78 int32_t xTemp;
asobhy 7:73fd05fe556a 79 int32_t uProportional;
asobhy 7:73fd05fe556a 80 int32_t uIntegral;
asobhy 7:73fd05fe556a 81 int32_t uS;
asobhy 7:73fd05fe556a 82
asobhy 9:fe56b888985c 83 setpointR_mutex.lock();
asobhy 9:fe56b888985c 84 setpointL_mutex.lock();
asobhy 7:73fd05fe556a 85 e = SaturatingSubtract(setp, dP); // e is the velocity error
asobhy 9:fe56b888985c 86 setpointL_mutex.unlock();
asobhy 9:fe56b888985c 87 setpointR_mutex.unlock();
asobhy 7:73fd05fe556a 88
asobhy 7:73fd05fe556a 89 xTemp = SaturatingAdd(xState, e);
asobhy 7:73fd05fe556a 90
asobhy 7:73fd05fe556a 91 // the maximum value that 'u' can get to is 20
asobhy 7:73fd05fe556a 92 // the maximum value that dPosition can get to 560
asobhy 7:73fd05fe556a 93 // scaling factor is 560/20 = 28
asobhy 7:73fd05fe556a 94 // scaling factor used is 40
asobhy 7:73fd05fe556a 95
asobhy 7:73fd05fe556a 96 uProportional = (float)(Kp*e/scale);
asobhy 7:73fd05fe556a 97 uIntegral = (float)(Ki*xState/scale);
asobhy 7:73fd05fe556a 98
asobhy 7:73fd05fe556a 99 uS = SaturatingAdd(uProportional, uIntegral);
asobhy 7:73fd05fe556a 100
asobhy 7:73fd05fe556a 101 u = SaturateValue(uS, U_LIMIT);
asobhy 7:73fd05fe556a 102 if(u==uS) xState=xTemp; // if limit has not been reached then update xState
asobhy 7:73fd05fe556a 103
asobhy 7:73fd05fe556a 104 return u;
asobhy 7:73fd05fe556a 105
asobhy 7:73fd05fe556a 106 }
asobhy 9:fe56b888985c 107
asobhy 9:fe56b888985c 108
asobhy 9:fe56b888985c 109 /*******************************************************************************
asobhy 9:fe56b888985c 110 * @brief PI Controller function
asobhy 9:fe56b888985c 111 * @param setp is the setpoint we would like the system to get to
asobhy 9:fe56b888985c 112 * @param dP is the current speed of the system
asobhy 9:fe56b888985c 113 * @return u is the control signal out of the controller to make the system
asobhy 9:fe56b888985c 114 * reach the desired setpoint
asobhy 9:fe56b888985c 115 *******************************************************************************/
asobhy 9:fe56b888985c 116 uint32_t PiControllerL(int setp, int dP)
asobhy 9:fe56b888985c 117 {
asobhy 9:fe56b888985c 118 static int32_t e, u;
asobhy 9:fe56b888985c 119 static int32_t xState = 0;
asobhy 9:fe56b888985c 120 int32_t xTemp;
asobhy 9:fe56b888985c 121 int32_t uProportional;
asobhy 9:fe56b888985c 122 int32_t uIntegral;
asobhy 9:fe56b888985c 123 int32_t uS;
asobhy 9:fe56b888985c 124
asobhy 9:fe56b888985c 125 setpointR_mutex.lock();
asobhy 9:fe56b888985c 126 setpointL_mutex.lock();
asobhy 9:fe56b888985c 127 e = SaturatingSubtract(setp, dP); // e is the velocity error
asobhy 9:fe56b888985c 128 setpointL_mutex.unlock();
asobhy 9:fe56b888985c 129 setpointR_mutex.unlock();
asobhy 9:fe56b888985c 130
asobhy 9:fe56b888985c 131 xTemp = SaturatingAdd(xState, e);
asobhy 9:fe56b888985c 132
asobhy 9:fe56b888985c 133 // the maximum value that 'u' can get to is 20
asobhy 9:fe56b888985c 134 // the maximum value that dPosition can get to 560
asobhy 9:fe56b888985c 135 // scaling factor is 560/20 = 28
asobhy 9:fe56b888985c 136 // scaling factor used is 40
asobhy 9:fe56b888985c 137
asobhy 9:fe56b888985c 138 uProportional = (float)(Kp*e/scale);
asobhy 9:fe56b888985c 139 uIntegral = (float)(Ki*xState/scale);
asobhy 9:fe56b888985c 140
asobhy 9:fe56b888985c 141 uS = SaturatingAdd(uProportional, uIntegral);
asobhy 9:fe56b888985c 142
asobhy 9:fe56b888985c 143 u = SaturateValue(uS, U_LIMIT);
asobhy 9:fe56b888985c 144 if(u==uS) xState=xTemp; // if limit has not been reached then update xState
asobhy 9:fe56b888985c 145
asobhy 9:fe56b888985c 146 return u;
asobhy 9:fe56b888985c 147
asobhy 9:fe56b888985c 148 }