ECE4333 - 2018 - Ahmed & Brandon / Mbed OS ManualControlFinal

ManualControl

Dependencies:   TPixy-Interface

Fork of MbedOS_Robot_Team by ECE4333 - 2018 - Ahmed & Brandon

Drivers/PiController.cpp@15:cf67f83d5409, 2018-03-03 (annotated)

Committer:
asobhy
Date:
Sat Mar 03 04:47:26 2018 +0000
Revision:
15:cf67f83d5409
Parent:
9:fe56b888985c
Child:
16:58ec2b891a25
following robot almost done. Need to get a better object to follow. Need to use a torch to light up the object. Need to adjust width of object in camera settings to match new object then test.

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 9:fe56b888985c 82 setpointR_mutex.lock();
asobhy 9:fe56b888985c 83 setpointL_mutex.lock();
asobhy 7:73fd05fe556a 84 e = SaturatingSubtract(setp, dP); // e is the velocity error
asobhy 9:fe56b888985c 85 setpointL_mutex.unlock();
asobhy 9:fe56b888985c 86 setpointR_mutex.unlock();
asobhy 7:73fd05fe556a 87
asobhy 7:73fd05fe556a 88 xTemp = SaturatingAdd(xState, e);
asobhy 7:73fd05fe556a 89
asobhy 7:73fd05fe556a 90 // the maximum value that 'u' can get to is 20
asobhy 7:73fd05fe556a 91 // the maximum value that dPosition can get to 560
asobhy 7:73fd05fe556a 92 // scaling factor is 560/20 = 28
asobhy 7:73fd05fe556a 93 // scaling factor used is 40
asobhy 7:73fd05fe556a 94
asobhy 7:73fd05fe556a 95 uProportional = (float)(Kp*e/scale);
asobhy 7:73fd05fe556a 96 uIntegral = (float)(Ki*xState/scale);
asobhy 7:73fd05fe556a 97
asobhy 7:73fd05fe556a 98 uS = SaturatingAdd(uProportional, uIntegral);
asobhy 7:73fd05fe556a 99
asobhy 7:73fd05fe556a 100 u = SaturateValue(uS, U_LIMIT);
asobhy 7:73fd05fe556a 101 if(u==uS) xState=xTemp; // if limit has not been reached then update xState
asobhy 7:73fd05fe556a 102
asobhy 7:73fd05fe556a 103 return u;
asobhy 7:73fd05fe556a 104
asobhy 7:73fd05fe556a 105 }
asobhy 9:fe56b888985c 106
asobhy 9:fe56b888985c 107
asobhy 9:fe56b888985c 108 /*******************************************************************************
asobhy 9:fe56b888985c 109 * @brief PI Controller function
asobhy 9:fe56b888985c 110 * @param setp is the setpoint we would like the system to get to
asobhy 9:fe56b888985c 111 * @param dP is the current speed of the system
asobhy 9:fe56b888985c 112 * @return u is the control signal out of the controller to make the system
asobhy 9:fe56b888985c 113 * reach the desired setpoint
asobhy 9:fe56b888985c 114 *******************************************************************************/
asobhy 9:fe56b888985c 115 uint32_t PiControllerL(int setp, int dP)
asobhy 9:fe56b888985c 116 {
asobhy 9:fe56b888985c 117 static int32_t e, u;
asobhy 9:fe56b888985c 118 static int32_t xState = 0;
asobhy 9:fe56b888985c 119 int32_t xTemp;
asobhy 9:fe56b888985c 120 int32_t uProportional;
asobhy 9:fe56b888985c 121 int32_t uIntegral;
asobhy 9:fe56b888985c 122 int32_t uS;
asobhy 9:fe56b888985c 123
asobhy 9:fe56b888985c 124 setpointR_mutex.lock();
asobhy 9:fe56b888985c 125 setpointL_mutex.lock();
asobhy 9:fe56b888985c 126 e = SaturatingSubtract(setp, dP); // e is the velocity error
asobhy 9:fe56b888985c 127 setpointL_mutex.unlock();
asobhy 9:fe56b888985c 128 setpointR_mutex.unlock();
asobhy 9:fe56b888985c 129
asobhy 9:fe56b888985c 130 xTemp = SaturatingAdd(xState, e);
asobhy 9:fe56b888985c 131
asobhy 9:fe56b888985c 132 // the maximum value that 'u' can get to is 20
asobhy 9:fe56b888985c 133 // the maximum value that dPosition can get to 560
asobhy 9:fe56b888985c 134 // scaling factor is 560/20 = 28
asobhy 9:fe56b888985c 135 // scaling factor used is 40
asobhy 9:fe56b888985c 136
asobhy 9:fe56b888985c 137 uProportional = (float)(Kp*e/scale);
asobhy 9:fe56b888985c 138 uIntegral = (float)(Ki*xState/scale);
asobhy 9:fe56b888985c 139
asobhy 9:fe56b888985c 140 uS = SaturatingAdd(uProportional, uIntegral);
asobhy 9:fe56b888985c 141
asobhy 9:fe56b888985c 142 u = SaturateValue(uS, U_LIMIT);
asobhy 9:fe56b888985c 143 if(u==uS) xState=xTemp; // if limit has not been reached then update xState
asobhy 9:fe56b888985c 144
asobhy 9:fe56b888985c 145 return u;
asobhy 9:fe56b888985c 146
asobhy 9:fe56b888985c 147 }