MorphGI / Mbed 2 deprecated LoCoMoTE_PeristalticPumpControl_v4

Control code for the peristaltic pump. Includes current control.

Dependencies:   mbed QEI FastAnalogIn mbed-rtos FastPWM

main.cpp@5:59dd1467762e, 2021-06-22 (annotated)

Committer:
dofydoink
Date:
Tue Jun 22 14:23:21 2021 +0000
Revision:
5:59dd1467762e
Parent:
4:3bab17dfae4e 
Final version of peristaltic pump control

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dofydoink 0:20018747657d 1 #include "mbed.h"
dofydoink 0:20018747657d 2 #include "math.h"
dofydoink 0:20018747657d 3
dofydoink 0:20018747657d 4 #include "rtos.h"
dofydoink 0:20018747657d 5 #include "FastPWM.h"
dofydoink 0:20018747657d 6
dofydoink 5:59dd1467762e 7 #define REV_TIME_S 0.7f
dofydoink 5:59dd1467762e 8 #define BREAK_TIME 0.2f
dofydoink 5:59dd1467762e 9
dofydoink 5:59dd1467762e 10 DigitalOut led(D13);
dofydoink 0:20018747657d 11
dofydoink 5:59dd1467762e 12 Thread MotorControlThread;
dofydoink 0:20018747657d 13
dofydoink 5:59dd1467762e 14 //sample time variables
dofydoink 5:59dd1467762e 15 double dblSampleTime_s = 0.0001;
dofydoink 0:20018747657d 16
dofydoink 0:20018747657d 17
dofydoink 0:20018747657d 18
dofydoink 5:59dd1467762e 19 int intCurrentFilOrder = 10;
dofydoink 0:20018747657d 20
dofydoink 5:59dd1467762e 21 InterruptIn pinActivation(PC_13);//user button
dofydoink 5:59dd1467762e 22
dofydoink 5:59dd1467762e 23 AnalogIn pinCurSense(A0);
dofydoink 0:20018747657d 24
dofydoink 5:59dd1467762e 25 FastPWM pinPwmOutput(D6);
dofydoink 5:59dd1467762e 26 DigitalOut pinDirectionFwd(D7);//INB, Positive motor terminal
dofydoink 5:59dd1467762e 27 DigitalOut pinDirectionRev(D4);//INA, negative motor terminal
dofydoink 3:9bd35e5b05ba 28
dofydoink 5:59dd1467762e 29 DigitalOut pinEnA(D3);
dofydoink 5:59dd1467762e 30 DigitalOut pinEnB(D2);
dofydoink 3:9bd35e5b05ba 31
dofydoink 5:59dd1467762e 32 //define all variables
dofydoink 5:59dd1467762e 33 DigitalOut Mntr(D13);
dofydoink 0:20018747657d 34
dofydoink 5:59dd1467762e 35 Semaphore semMotCtrl;
dofydoink 5:59dd1467762e 36 Semaphore semPrint;
dofydoink 0:20018747657d 37
dofydoink 0:20018747657d 38 Serial pc(USBTX, USBRX); // tx, rx
dofydoink 3:9bd35e5b05ba 39 int ii,jj,kk,nn, spiTick; //counting variables
dofydoink 3:9bd35e5b05ba 40
dofydoink 5:59dd1467762e 41 bool isFwdDone;
dofydoink 5:59dd1467762e 42 bool isRev;
dofydoink 5:59dd1467762e 43 bool isBreakDone;
dofydoink 5:59dd1467762e 44 bool isBreakNeeded;
dofydoink 3:9bd35e5b05ba 45
dofydoink 5:59dd1467762e 46 int intLastDir;
dofydoink 3:9bd35e5b05ba 47
dofydoink 5:59dd1467762e 48 double dblCurrentAmps;
dofydoink 5:59dd1467762e 49 double dblMotorVoltage = 12.0;
dofydoink 5:59dd1467762e 50 double dblError;
dofydoink 5:59dd1467762e 51 double dblSumError;
dofydoink 5:59dd1467762e 52 double dblITerm;
dofydoink 5:59dd1467762e 53 double dblKi = 1000.0;
dofydoink 5:59dd1467762e 54 double dblKp = 2.00;
dofydoink 3:9bd35e5b05ba 55
dofydoink 3:9bd35e5b05ba 56
dofydoink 5:59dd1467762e 57 double output;//controller output
dofydoink 3:9bd35e5b05ba 58
dofydoink 5:59dd1467762e 59 double dblDemand;
dofydoink 5:59dd1467762e 60 double dblFilOutput;
dofydoink 5:59dd1467762e 61 //current limit variables
dofydoink 5:59dd1467762e 62 double CurrentLimitSet;
dofydoink 5:59dd1467762e 63 double dblCurrentLimitAmps = 2.5;
dofydoink 5:59dd1467762e 64 double currentBuck;
dofydoink 5:59dd1467762e 65 double currentBuckGain = 10.0;
dofydoink 3:9bd35e5b05ba 66
dofydoink 5:59dd1467762e 67 double analogReadingCurSens;
dofydoink 3:9bd35e5b05ba 68
dofydoink 5:59dd1467762e 69 int intDirection;
dofydoink 5:59dd1467762e 70
dofydoink 5:59dd1467762e 71 Timeout tickRunFwd;
dofydoink 5:59dd1467762e 72 Timeout tickRunRev;
dofydoink 5:59dd1467762e 73 Timeout tickBreak;
dofydoink 3:9bd35e5b05ba 74
dofydoink 5:59dd1467762e 75 Ticker tickPrint;
dofydoink 3:9bd35e5b05ba 76
dofydoink 5:59dd1467762e 77 void StopPump()
dofydoink 5:59dd1467762e 78 {
dofydoink 5:59dd1467762e 79 intDirection = 0;
dofydoink 5:59dd1467762e 80
dofydoink 5:59dd1467762e 81 }
dofydoink 3:9bd35e5b05ba 82
dofydoink 3:9bd35e5b05ba 83
dofydoink 3:9bd35e5b05ba 84
dofydoink 5:59dd1467762e 85 void StartPumpFwd()
dofydoink 5:59dd1467762e 86 {
dofydoink 5:59dd1467762e 87 intDirection = 1;
dofydoink 5:59dd1467762e 88 }
dofydoink 3:9bd35e5b05ba 89
dofydoink 5:59dd1467762e 90 void MotorPIControl()
dofydoink 5:59dd1467762e 91 {
dofydoink 5:59dd1467762e 92
dofydoink 3:9bd35e5b05ba 93 }
dofydoink 3:9bd35e5b05ba 94
dofydoink 3:9bd35e5b05ba 95
dofydoink 5:59dd1467762e 96 void MotorControl()
dofydoink 3:9bd35e5b05ba 97 {
dofydoink 5:59dd1467762e 98 while(1)
dofydoink 0:20018747657d 99 {
dofydoink 5:59dd1467762e 100 semMotCtrl.wait();
dofydoink 5:59dd1467762e 101 Mntr = !Mntr;
dofydoink 0:20018747657d 102
dofydoink 5:59dd1467762e 103
dofydoink 5:59dd1467762e 104
dofydoink 5:59dd1467762e 105 analogReadingCurSens = pinCurSense.read();
dofydoink 5:59dd1467762e 106
dofydoink 5:59dd1467762e 107 dblCurrentAmps = analogReadingCurSens*3.3/0.15;
dofydoink 5:59dd1467762e 108
dofydoink 5:59dd1467762e 109 //calculate output
dofydoink 5:59dd1467762e 110 switch (intDirection)//if asked to run forward
dofydoink 5:59dd1467762e 111 {
dofydoink 5:59dd1467762e 112 case 1 :
dofydoink 5:59dd1467762e 113 dblDemand = 2.5;
dofydoink 5:59dd1467762e 114 pinDirectionFwd = 0;
dofydoink 5:59dd1467762e 115 pinDirectionRev = 1;
dofydoink 5:59dd1467762e 116
dofydoink 5:59dd1467762e 117 break;
dofydoink 5:59dd1467762e 118
dofydoink 5:59dd1467762e 119 case 0 :
dofydoink 5:59dd1467762e 120 dblDemand = 0.0;
dofydoink 5:59dd1467762e 121 dblSumError = 0.0;
dofydoink 5:59dd1467762e 122 pinDirectionFwd = 1;
dofydoink 5:59dd1467762e 123 pinDirectionRev = 1;
dofydoink 5:59dd1467762e 124 output = 0;
dofydoink 5:59dd1467762e 125 break;
dofydoink 5:59dd1467762e 126
dofydoink 5:59dd1467762e 127
dofydoink 5:59dd1467762e 128 default :
dofydoink 5:59dd1467762e 129 dblDemand = 0;
dofydoink 5:59dd1467762e 130 pinDirectionFwd = 1;
dofydoink 5:59dd1467762e 131 pinDirectionRev = 1;
dofydoink 0:20018747657d 132
dofydoink 5:59dd1467762e 133
dofydoink 5:59dd1467762e 134 }
dofydoink 5:59dd1467762e 135
dofydoink 5:59dd1467762e 136 dblError = dblDemand - dblCurrentAmps;
dofydoink 5:59dd1467762e 137 dblSumError = dblSumError + dblError;
dofydoink 5:59dd1467762e 138 dblITerm = dblKi*dblKp*dblSumError;
dofydoink 3:9bd35e5b05ba 139
dofydoink 3:9bd35e5b05ba 140
dofydoink 5:59dd1467762e 141 if(dblITerm > 0.95)
dofydoink 3:9bd35e5b05ba 142 {
dofydoink 5:59dd1467762e 143 dblITerm = 0.95;
dofydoink 3:9bd35e5b05ba 144 }
dofydoink 5:59dd1467762e 145 if(dblITerm < 0.0)
dofydoink 3:9bd35e5b05ba 146 {
dofydoink 5:59dd1467762e 147 dblITerm = 0.0;
dofydoink 3:9bd35e5b05ba 148 }
dofydoink 0:20018747657d 149
dofydoink 5:59dd1467762e 150 output = dblKp*dblError + dblITerm;
dofydoink 5:59dd1467762e 151
dofydoink 5:59dd1467762e 152 if(output > 0.95)
dofydoink 3:9bd35e5b05ba 153 {
dofydoink 5:59dd1467762e 154 output = 0.95;
dofydoink 0:20018747657d 155
dofydoink 0:20018747657d 156
dofydoink 3:9bd35e5b05ba 157 }
dofydoink 5:59dd1467762e 158 if(output < 0.0)
dofydoink 3:9bd35e5b05ba 159 {
dofydoink 5:59dd1467762e 160 output = 0.0;
dofydoink 3:9bd35e5b05ba 161
dofydoink 3:9bd35e5b05ba 162 }
dofydoink 5:59dd1467762e 163 //printf("Er: %f, I: %f Out: %f\r\n",dblError, dblITerm, output);
dofydoink 5:59dd1467762e 164
dofydoink 5:59dd1467762e 165 //write output to motoroutput
dofydoink 5:59dd1467762e 166
dofydoink 5:59dd1467762e 167 pinPwmOutput.write(output);
dofydoink 5:59dd1467762e 168 intLastDir = intDirection;
dofydoink 0:20018747657d 169
dofydoink 5:59dd1467762e 170 //printf("Dir: %d\r\n",intDirection);
dofydoink 0:20018747657d 171 }
dofydoink 5:59dd1467762e 172 }
dofydoink 5:59dd1467762e 173
dofydoink 5:59dd1467762e 174 void startMotorControl()
dofydoink 3:9bd35e5b05ba 175 {
dofydoink 5:59dd1467762e 176 semMotCtrl.release();
dofydoink 3:9bd35e5b05ba 177 }
dofydoink 0:20018747657d 178
dofydoink 5:59dd1467762e 179 void startPrint()
dofydoink 0:20018747657d 180 {
dofydoink 5:59dd1467762e 181 semPrint.release();
dofydoink 0:20018747657d 182 }
dofydoink 0:20018747657d 183
dofydoink 0:20018747657d 184
dofydoink 5:59dd1467762e 185 Ticker motorCtrlTicker;
dofydoink 0:20018747657d 186
dofydoink 0:20018747657d 187 int main() {
dofydoink 5:59dd1467762e 188 pinActivation.mode(PullDown);
dofydoink 5:59dd1467762e 189 pinEnA = 1;
dofydoink 5:59dd1467762e 190 pinEnB = 1;
dofydoink 5:59dd1467762e 191 Mntr = 1;
dofydoink 0:20018747657d 192 pinPwmOutput.period_us(50);
dofydoink 5:59dd1467762e 193 isFwdDone = 1;
dofydoink 5:59dd1467762e 194 isBreakDone = 0;
dofydoink 5:59dd1467762e 195 dblKi = dblKi * dblSampleTime_s / dblMotorVoltage;
dofydoink 0:20018747657d 196
dofydoink 5:59dd1467762e 197 dblKp = dblKp / dblMotorVoltage;
dofydoink 2:aee7d4724915 198
dofydoink 5:59dd1467762e 199 printf("\r\nPump Controller Online\r\n\n\n");
dofydoink 5:59dd1467762e 200 pinPwmOutput.write(0.5);
dofydoink 5:59dd1467762e 201 wait(2.0);
dofydoink 5:59dd1467762e 202 pinActivation.rise(&StopPump);
dofydoink 5:59dd1467762e 203 pinActivation.fall(&StartPumpFwd);
dofydoink 5:59dd1467762e 204 dblDemand = 0.0;
dofydoink 5:59dd1467762e 205
dofydoink 0:20018747657d 206
dofydoink 0:20018747657d 207 //calculate/convert variables
dofydoink 0:20018747657d 208
dofydoink 0:20018747657d 209 CurrentLimitSet = dblCurrentLimitAmps *0.14/3.3;
dofydoink 5:59dd1467762e 210 //pinPwmOutput = 0.8;
dofydoink 5:59dd1467762e 211 MotorControlThread.start(MotorControl);
dofydoink 3:9bd35e5b05ba 212
dofydoink 5:59dd1467762e 213 motorCtrlTicker.attach(&startMotorControl, dblSampleTime_s);
dofydoink 5:59dd1467762e 214 //tickPrint.attach(&startPrint,1.0f);
dofydoink 0:20018747657d 215
dofydoink 0:20018747657d 216 while(1)
dofydoink 0:20018747657d 217 {
dofydoink 0:20018747657d 218 Thread::wait(osWaitForever);
dofydoink 0:20018747657d 219 }
dofydoink 3:9bd35e5b05ba 220 }