Julius Bernth
/
Cuvette_Centrifuge_Test_v1-2_ParallelMode
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
main.cpp@18:5e9b288793bb, 2021-11-04 (annotated)
- Committer:
- juliusbernth
- Date:
- Thu Nov 04 09:17:40 2021 +0000
- Revision:
- 18:5e9b288793bb
- Parent:
- 17:eb74805e8f9b
- Child:
- 19:a71ef54d9e94
Working for parallel mode
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| juliusbernth | 0:64442857169c | 1 | /* mbed Microcontroller Library |
| juliusbernth | 0:64442857169c | 2 | * Copyright (c) 2019 ARM Limited |
| juliusbernth | 0:64442857169c | 3 | * SPDX-License-Identifier: Apache-2.0 |
| juliusbernth | 0:64442857169c | 4 | */ |
| juliusbernth | 0:64442857169c | 5 | |
| juliusbernth | 0:64442857169c | 6 | #include "mbed.h" |
| juliusbernth | 0:64442857169c | 7 | #include "platform/mbed_thread.h" |
| juliusbernth | 0:64442857169c | 8 | #include "Settings.h" |
| juliusbernth | 0:64442857169c | 9 | #include "QEI.h" |
| juliusbernth | 7:e36f61608c10 | 10 | #include "math.h" |
| juliusbernth | 0:64442857169c | 11 | //#include "LIS3DH.h" |
| juliusbernth | 0:64442857169c | 12 | |
| juliusbernth | 0:64442857169c | 13 | /* |
| juliusbernth | 0:64442857169c | 14 | Open issues: |
| juliusbernth | 5:e9bb800a3742 | 15 | - need to figure out how to handle transitions to error states |
| juliusbernth | 5:e9bb800a3742 | 16 | - Need to test how breaking is performed. |
| juliusbernth | 5:e9bb800a3742 | 17 | -Test onboard current limitation |
| juliusbernth | 0:64442857169c | 18 | */ |
| juliusbernth | 0:64442857169c | 19 | |
| juliusbernth | 0:64442857169c | 20 | Timer encoderTimer; |
| juliusbernth | 0:64442857169c | 21 | Timer buttonTimer; |
| juliusbernth | 0:64442857169c | 22 | Timer testTimer; |
| juliusbernth | 0:64442857169c | 23 | |
| juliusbernth | 0:64442857169c | 24 | Timeout buttonTimeout; |
| juliusbernth | 0:64442857169c | 25 | |
| juliusbernth | 0:64442857169c | 26 | Thread PrintThread(osPriorityAboveNormal); |
| juliusbernth | 0:64442857169c | 27 | Thread CentrifugeTestThread(osPriorityHigh); |
| juliusbernth | 0:64442857169c | 28 | Thread ReadButtonThread(osPriorityNormal); |
| juliusbernth | 0:64442857169c | 29 | Thread MotorControlThread(osPriorityRealtime); |
| juliusbernth | 0:64442857169c | 30 | |
| juliusbernth | 0:64442857169c | 31 | double SPIN_T[6]; //times for spin cycle |
| juliusbernth | 0:64442857169c | 32 | |
| juliusbernth | 18:5e9b288793bb | 33 | //QEI encoder(PA_14, PA_13, PA_15, 256, QEI::X4_ENCODING); |
| juliusbernth | 18:5e9b288793bb | 34 | QEI encoder(PA_15, PB_7, PC_13, 256, QEI::X4_ENCODING); |
| juliusbernth | 0:64442857169c | 35 | |
| juliusbernth | 0:64442857169c | 36 | //LIS3DH accel = LIS3DH(SPI_MOSI, SPI_MISO, SPI_SCLK, SPI_CS,LIS3DH_DR_NR_LP_400HZ, LIS3DH_FS_2G); |
| juliusbernth | 0:64442857169c | 37 | |
| juliusbernth | 0:64442857169c | 38 | DigitalOut EN_FAULTA (D2); |
| juliusbernth | 0:64442857169c | 39 | DigitalOut STBY (D8); |
| juliusbernth | 0:64442857169c | 40 | DigitalOut PHA (D3); |
| juliusbernth | 9:b1f53e4eb453 | 41 | //DigitalOut PHB (D7); |
| juliusbernth | 0:64442857169c | 42 | PwmOut PWMA (D5); |
| juliusbernth | 9:b1f53e4eb453 | 43 | //PwmOut PWMB (D4); |
| juliusbernth | 0:64442857169c | 44 | PwmOut REF_PWM_A (D15); |
| juliusbernth | 0:64442857169c | 45 | PwmOut REF_PWM_B (D14); |
| juliusbernth | 7:e36f61608c10 | 46 | AnalogIn pinAx(A0); |
| juliusbernth | 7:e36f61608c10 | 47 | AnalogIn pinAy(A1); |
| juliusbernth | 7:e36f61608c10 | 48 | AnalogIn pinAz(A2); |
| juliusbernth | 7:e36f61608c10 | 49 | |
| juliusbernth | 9:b1f53e4eb453 | 50 | DigitalOut PHB (D7); |
| juliusbernth | 9:b1f53e4eb453 | 51 | DigitalOut EN_FAULTB (D11); |
| juliusbernth | 9:b1f53e4eb453 | 52 | PwmOut PWMB (D4); |
| juliusbernth | 9:b1f53e4eb453 | 53 | PwmOut REF_PWMB (D14); |
| juliusbernth | 9:b1f53e4eb453 | 54 | |
| juliusbernth | 7:e36f61608c10 | 55 | DigitalOut pinSt(A3); |
| juliusbernth | 0:64442857169c | 56 | |
| juliusbernth | 18:5e9b288793bb | 57 | Serial pc(USBTX,USBRX); |
| juliusbernth | 0:64442857169c | 58 | |
| juliusbernth | 0:64442857169c | 59 | DigitalOut pinStatusLED (D13); |
| juliusbernth | 8:41cd1fc8cbbe | 60 | |
| juliusbernth | 18:5e9b288793bb | 61 | DigitalIn pinButton (PC_11);//(PC_8); |
| juliusbernth | 18:5e9b288793bb | 62 | DigitalOut pinLedWhite (PC_2);//(PC_6); |
| juliusbernth | 18:5e9b288793bb | 63 | DigitalOut pinLedRed (PC_3);//(PC_5); |
| juliusbernth | 0:64442857169c | 64 | |
| juliusbernth | 0:64442857169c | 65 | /* Still to allocate |
| juliusbernth | 0:64442857169c | 66 | |
| juliusbernth | 0:64442857169c | 67 | DigitalOut pinBuzzer |
| juliusbernth | 0:64442857169c | 68 | DigitalOut pinPowerLED |
| juliusbernth | 0:64442857169c | 69 | |
| juliusbernth | 0:64442857169c | 70 | */ |
| juliusbernth | 0:64442857169c | 71 | |
| juliusbernth | 0:64442857169c | 72 | //global variables |
| juliusbernth | 0:64442857169c | 73 | double speedFilterConstant; |
| juliusbernth | 5:e9bb800a3742 | 74 | char state; |
| juliusbernth | 0:64442857169c | 75 | double currentPulses; |
| juliusbernth | 0:64442857169c | 76 | double lastPulses; |
| juliusbernth | 0:64442857169c | 77 | double lastSpeedRPM; |
| juliusbernth | 0:64442857169c | 78 | double Tnow; |
| juliusbernth | 5:e9bb800a3742 | 79 | double demandSpeed_RPM = 0.0; |
| juliusbernth | 5:e9bb800a3742 | 80 | double currentSpeedRPM; |
| juliusbernth | 0:64442857169c | 81 | double deltaError; |
| juliusbernth | 0:64442857169c | 82 | double lastError; |
| juliusbernth | 0:64442857169c | 83 | double integralTerm; |
| juliusbernth | 0:64442857169c | 84 | double output; |
| juliusbernth | 0:64442857169c | 85 | |
| juliusbernth | 9:b1f53e4eb453 | 86 | double _MagFil; |
| juliusbernth | 9:b1f53e4eb453 | 87 | |
| juliusbernth | 0:64442857169c | 88 | Ticker tickerMotorControl; |
| juliusbernth | 0:64442857169c | 89 | Ticker tickerPrint; |
| juliusbernth | 0:64442857169c | 90 | Ticker tickerReleaseButton; |
| juliusbernth | 0:64442857169c | 91 | |
| juliusbernth | 0:64442857169c | 92 | Semaphore semMotorControl(0); |
| juliusbernth | 0:64442857169c | 93 | Semaphore semPrint(0); |
| juliusbernth | 0:64442857169c | 94 | Semaphore semStartTest(0); |
| juliusbernth | 0:64442857169c | 95 | Semaphore semButton(0); |
| juliusbernth | 0:64442857169c | 96 | |
| juliusbernth | 3:4f215646a42b | 97 | Mutex mut1; |
| juliusbernth | 0:64442857169c | 98 | |
| juliusbernth | 0:64442857169c | 99 | double LimitDouble(double input, double min, double max){ |
| juliusbernth | 0:64442857169c | 100 | double output; |
| juliusbernth | 0:64442857169c | 101 | output = input; |
| juliusbernth | 0:64442857169c | 102 | if (input > max){ |
| juliusbernth | 0:64442857169c | 103 | output = max; |
| juliusbernth | 0:64442857169c | 104 | } |
| juliusbernth | 0:64442857169c | 105 | if (input < min){ |
| juliusbernth | 0:64442857169c | 106 | output = min; |
| juliusbernth | 0:64442857169c | 107 | } |
| juliusbernth | 0:64442857169c | 108 | return output; |
| juliusbernth | 0:64442857169c | 109 | } |
| juliusbernth | 0:64442857169c | 110 | |
| juliusbernth | 0:64442857169c | 111 | |
| juliusbernth | 0:64442857169c | 112 | void TestCompleteNotification(){ |
| juliusbernth | 0:64442857169c | 113 | //send message |
| juliusbernth | 17:eb74805e8f9b | 114 | //printf("\r\nTest complete\r\n"); |
| juliusbernth | 0:64442857169c | 115 | //sound buzzer and blink 3 times |
| juliusbernth | 0:64442857169c | 116 | } |
| juliusbernth | 0:64442857169c | 117 | |
| juliusbernth | 0:64442857169c | 118 | void MotorControlRelease(){ |
| juliusbernth | 0:64442857169c | 119 | semMotorControl.release(); |
| juliusbernth | 0:64442857169c | 120 | } |
| juliusbernth | 0:64442857169c | 121 | void PrintRelease(){ |
| juliusbernth | 0:64442857169c | 122 | semPrint.release(); |
| juliusbernth | 0:64442857169c | 123 | } |
| juliusbernth | 0:64442857169c | 124 | |
| juliusbernth | 5:e9bb800a3742 | 125 | void PrintTimeRemaining(){ |
| juliusbernth | 0:64442857169c | 126 | while(1){ |
| juliusbernth | 0:64442857169c | 127 | semPrint.wait(); |
| juliusbernth | 5:e9bb800a3742 | 128 | if(state == STATE_RUNNING){ |
| juliusbernth | 0:64442857169c | 129 | double timeElapsed = testTimer.read(); |
| juliusbernth | 0:64442857169c | 130 | double timeRemaining = SPIN_T[5] - timeElapsed; |
| juliusbernth | 0:64442857169c | 131 | int displayTime = int(timeRemaining)+1; |
| juliusbernth | 16:cefb22c49c2f | 132 | |
| juliusbernth | 7:e36f61608c10 | 133 | //mut1.lock(); |
| juliusbernth | 5:e9bb800a3742 | 134 | double printDemandSpeed = demandSpeed_RPM; |
| juliusbernth | 5:e9bb800a3742 | 135 | double printCurrentSpeed = currentSpeedRPM; |
| juliusbernth | 16:cefb22c49c2f | 136 | int intCurrentSpeed = int(printCurrentSpeed); |
| juliusbernth | 17:eb74805e8f9b | 137 | //printf("Current Speed %d RPM.\t Time remaining %d s\r\n",intCurrentSpeed, displayTime); |
| juliusbernth | 7:e36f61608c10 | 138 | //mut1.unlock(); |
| juliusbernth | 15:f86310960aab | 139 | //printf("%f\t%f\r\n", currentSpeedRPM,_MagFil); |
| juliusbernth | 17:eb74805e8f9b | 140 | printf("%f\t%f\r\n", printDemandSpeed,printCurrentSpeed); |
| juliusbernth | 12:bc34f264e2f2 | 141 | //printf("%f\r\n",_MagFil); |
| juliusbernth | 12:bc34f264e2f2 | 142 | } |
| juliusbernth | 12:bc34f264e2f2 | 143 | if(state == STATE_ERROR){ |
| juliusbernth | 12:bc34f264e2f2 | 144 | double timeElapsed = testTimer.read(); |
| juliusbernth | 12:bc34f264e2f2 | 145 | double timeRemaining = SPIN_T[5] - timeElapsed; |
| juliusbernth | 12:bc34f264e2f2 | 146 | int displayTime = int(timeRemaining)+1; |
| juliusbernth | 12:bc34f264e2f2 | 147 | //printf("Time remaining %d s\r\n", displayTime); |
| juliusbernth | 12:bc34f264e2f2 | 148 | //mut1.lock(); |
| juliusbernth | 12:bc34f264e2f2 | 149 | double printDemandSpeed = demandSpeed_RPM; |
| juliusbernth | 12:bc34f264e2f2 | 150 | double printCurrentSpeed = currentSpeedRPM; |
| juliusbernth | 12:bc34f264e2f2 | 151 | //mut1.unlock(); |
| juliusbernth | 12:bc34f264e2f2 | 152 | //printf("%f\t%f\r\n", currentSpeedRPM,_MagFil); |
| juliusbernth | 14:81d390496d4e | 153 | //printf("%f\t%f\r\n", printDemandSpeed,printCurrentSpeed); |
| juliusbernth | 9:b1f53e4eb453 | 154 | //printf("%f\r\n",_MagFil); |
| juliusbernth | 3:4f215646a42b | 155 | } |
| juliusbernth | 3:4f215646a42b | 156 | } |
| juliusbernth | 3:4f215646a42b | 157 | } |
| juliusbernth | 3:4f215646a42b | 158 | |
| juliusbernth | 16:cefb22c49c2f | 159 | bool isSpinning(){ |
| juliusbernth | 16:cefb22c49c2f | 160 | int pulses1 = encoder.getPulses(); |
| juliusbernth | 16:cefb22c49c2f | 161 | wait(0.1); |
| juliusbernth | 16:cefb22c49c2f | 162 | int pulses2 = encoder.getPulses(); |
| juliusbernth | 16:cefb22c49c2f | 163 | int difference = pulses2 - pulses1; |
| juliusbernth | 16:cefb22c49c2f | 164 | if (abs(difference) > 10){ |
| juliusbernth | 16:cefb22c49c2f | 165 | return 1; |
| juliusbernth | 16:cefb22c49c2f | 166 | }else{ |
| juliusbernth | 16:cefb22c49c2f | 167 | return 0; |
| juliusbernth | 16:cefb22c49c2f | 168 | } |
| juliusbernth | 16:cefb22c49c2f | 169 | } |
| juliusbernth | 16:cefb22c49c2f | 170 | |
| juliusbernth | 0:64442857169c | 171 | void MotorControl(){ |
| juliusbernth | 9:b1f53e4eb453 | 172 | double Ax, Ay, Az, Mag; |
| juliusbernth | 9:b1f53e4eb453 | 173 | static double MagFil, lastAx, lastAy, lastAz; |
| juliusbernth | 9:b1f53e4eb453 | 174 | static double lastErrorDot; |
| juliusbernth | 0:64442857169c | 175 | while(1){ |
| juliusbernth | 0:64442857169c | 176 | semMotorControl.wait();//wait for a signal |
| juliusbernth | 12:bc34f264e2f2 | 177 | |
| juliusbernth | 17:eb74805e8f9b | 178 | |
| juliusbernth | 12:bc34f264e2f2 | 179 | Tnow = testTimer.read(); |
| juliusbernth | 12:bc34f264e2f2 | 180 | //check accel. If problem, change state to ERROR |
| juliusbernth | 12:bc34f264e2f2 | 181 | //check accel here |
| juliusbernth | 12:bc34f264e2f2 | 182 | |
| juliusbernth | 12:bc34f264e2f2 | 183 | /* |
| juliusbernth | 12:bc34f264e2f2 | 184 | Ax = 0.2*pinAx.read() + 0.8*Ax - 0.5; |
| juliusbernth | 12:bc34f264e2f2 | 185 | Ay = 0.2*pinAy.read() + 0.8*Ay - 0.5; |
| juliusbernth | 12:bc34f264e2f2 | 186 | Az = 0.2*pinAz.read() + 0.8*Az - 0.5; |
| juliusbernth | 12:bc34f264e2f2 | 187 | */ |
| juliusbernth | 12:bc34f264e2f2 | 188 | |
| juliusbernth | 12:bc34f264e2f2 | 189 | Ax = pinAx.read() - 0.5; |
| juliusbernth | 12:bc34f264e2f2 | 190 | Ay = pinAy.read() - 0.5; |
| juliusbernth | 12:bc34f264e2f2 | 191 | Az = pinAz.read() - 0.5; |
| juliusbernth | 12:bc34f264e2f2 | 192 | |
| juliusbernth | 12:bc34f264e2f2 | 193 | |
| juliusbernth | 12:bc34f264e2f2 | 194 | //read accel |
| juliusbernth | 12:bc34f264e2f2 | 195 | Mag = Ax*Ax + Ay*Ay; |
| juliusbernth | 12:bc34f264e2f2 | 196 | Mag = sqrt(Mag); |
| juliusbernth | 12:bc34f264e2f2 | 197 | //Mag *=ACCEL_SCALE; |
| juliusbernth | 12:bc34f264e2f2 | 198 | MagFil = 0.1*Mag + 0.9*MagFil; |
| juliusbernth | 12:bc34f264e2f2 | 199 | _MagFil = MagFil; |
| juliusbernth | 12:bc34f264e2f2 | 200 | if (MagFil > VIBRATION_THRESHOLD){ |
| juliusbernth | 12:bc34f264e2f2 | 201 | //mut1.lock(); |
| juliusbernth | 15:f86310960aab | 202 | state = STATE_ERROR; |
| juliusbernth | 17:eb74805e8f9b | 203 | //printf("Excess vibration detected\r\n"); |
| juliusbernth | 12:bc34f264e2f2 | 204 | //mut1.unlock(); |
| juliusbernth | 12:bc34f264e2f2 | 205 | } |
| juliusbernth | 12:bc34f264e2f2 | 206 | //int deltaT = encoderTimer.read();//read current time in seconds |
| juliusbernth | 7:e36f61608c10 | 207 | if(state == STATE_RUNNING){//need to check if this is the best condition to look for. |
| juliusbernth | 17:eb74805e8f9b | 208 | |
| juliusbernth | 0:64442857169c | 209 | //calculate current demand |
| juliusbernth | 5:e9bb800a3742 | 210 | if ( (SPIN_T[0] <= Tnow) && (Tnow < SPIN_T[1]) ) {//test is in warmup |
| juliusbernth | 0:64442857169c | 211 | demandSpeed_RPM = 0.0; |
| juliusbernth | 5:e9bb800a3742 | 212 | //printf("warm up %f\r\n", demandSpeed_RPM); |
| juliusbernth | 5:e9bb800a3742 | 213 | //deactivate motor? |
| juliusbernth | 0:64442857169c | 214 | } |
| juliusbernth | 5:e9bb800a3742 | 215 | if ((SPIN_T[1] <= Tnow)&&(Tnow < SPIN_T[2])){//test is in ramp up |
| juliusbernth | 5:e9bb800a3742 | 216 | |
| juliusbernth | 5:e9bb800a3742 | 217 | double a = Tnow - SPIN_T[1]; |
| juliusbernth | 5:e9bb800a3742 | 218 | demandSpeed_RPM = a * targetSpeed_RPM / T_RAMP; |
| juliusbernth | 5:e9bb800a3742 | 219 | //printf("ramp up %f\r\n", demandSpeed_RPM); |
| juliusbernth | 5:e9bb800a3742 | 220 | } |
| juliusbernth | 5:e9bb800a3742 | 221 | if ((SPIN_T[2] <= Tnow)&&(Tnow < SPIN_T[3])){//test is in coast |
| juliusbernth | 5:e9bb800a3742 | 222 | demandSpeed_RPM = targetSpeed_RPM; |
| juliusbernth | 5:e9bb800a3742 | 223 | //printf("coast %f\r\n", demandSpeed_RPM); |
| juliusbernth | 4:d04afc466198 | 224 | } |
| juliusbernth | 5:e9bb800a3742 | 225 | if ((SPIN_T[3] <= Tnow)&&(Tnow < SPIN_T[4])){//test is in ramp down |
| juliusbernth | 5:e9bb800a3742 | 226 | double a = Tnow - SPIN_T[3]; |
| juliusbernth | 5:e9bb800a3742 | 227 | demandSpeed_RPM = targetSpeed_RPM - (a/T_RAMP*targetSpeed_RPM); |
| juliusbernth | 5:e9bb800a3742 | 228 | //printf("ramp down %f\r\n", demandSpeed_RPM); |
| juliusbernth | 5:e9bb800a3742 | 229 | } |
| juliusbernth | 5:e9bb800a3742 | 230 | if ((SPIN_T[4] <= Tnow)&&(Tnow < SPIN_T[5])){//test is in cooldown |
| juliusbernth | 5:e9bb800a3742 | 231 | demandSpeed_RPM = 0.0; |
| juliusbernth | 5:e9bb800a3742 | 232 | //printf("cool down %f\r\n", demandSpeed_RPM); |
| juliusbernth | 5:e9bb800a3742 | 233 | //deactivate motor? |
| juliusbernth | 4:d04afc466198 | 234 | } |
| juliusbernth | 12:bc34f264e2f2 | 235 | } |
| juliusbernth | 7:e36f61608c10 | 236 | |
| juliusbernth | 18:5e9b288793bb | 237 | if(state == STATE_ERROR){ |
| juliusbernth | 18:5e9b288793bb | 238 | demandSpeed_RPM -=66.7*SAMPLE_TIME_US/1000000; |
| juliusbernth | 18:5e9b288793bb | 239 | } |
| juliusbernth | 18:5e9b288793bb | 240 | |
| juliusbernth | 18:5e9b288793bb | 241 | |
| juliusbernth | 18:5e9b288793bb | 242 | demandSpeed_RPM = LimitDouble(demandSpeed_RPM,0.0,MAX_SPEED_RPM);//limit demand |
| juliusbernth | 18:5e9b288793bb | 243 | currentPulses = encoder.getPulses();//calculate current speed |
| juliusbernth | 18:5e9b288793bb | 244 | double deltaPulses = currentPulses - lastPulses; |
| juliusbernth | 18:5e9b288793bb | 245 | double speed_RPM = deltaPulses / SAMPLE_TIME_US * 1000000;//get in pulses per second |
| juliusbernth | 18:5e9b288793bb | 246 | speed_RPM *= 60/4/PULSES_PER_REV;//convert to RPM |
| juliusbernth | 18:5e9b288793bb | 247 | |
| juliusbernth | 18:5e9b288793bb | 248 | currentSpeedRPM = 0.1*speed_RPM + 0.9*lastSpeedRPM;//filter speed |
| juliusbernth | 12:bc34f264e2f2 | 249 | |
| juliusbernth | 18:5e9b288793bb | 250 | double error = demandSpeed_RPM - currentSpeedRPM;//calculate error |
| juliusbernth | 18:5e9b288793bb | 251 | deltaError = error - lastError; |
| juliusbernth | 18:5e9b288793bb | 252 | double errorDot = deltaError/SAMPLE_TIME_US*1000000.0; |
| juliusbernth | 18:5e9b288793bb | 253 | errorDot = 0.1*errorDot + 0.9*lastErrorDot; |
| juliusbernth | 18:5e9b288793bb | 254 | integralTerm += error*Ki*SAMPLE_TIME_US/1000000.0; |
| juliusbernth | 18:5e9b288793bb | 255 | integralTerm = LimitDouble(integralTerm,-0.8, 0.8); |
| juliusbernth | 5:e9bb800a3742 | 256 | |
| juliusbernth | 18:5e9b288793bb | 257 | output = Kp * error; //calculate output |
| juliusbernth | 18:5e9b288793bb | 258 | output += integralTerm; |
| juliusbernth | 18:5e9b288793bb | 259 | output += Kd*errorDot; |
| juliusbernth | 18:5e9b288793bb | 260 | output += Ko*demandSpeed_RPM; |
| juliusbernth | 18:5e9b288793bb | 261 | output = LimitDouble(output,-1.0,1.0); |
| juliusbernth | 18:5e9b288793bb | 262 | //printf("wd:%f\t w:%f\t e:%f\t o:%f \r\n",demandSpeed_RPM, currentSpeedRPM, error, output); |
| juliusbernth | 18:5e9b288793bb | 263 | |
| juliusbernth | 18:5e9b288793bb | 264 | if(output >=0){//Set direction |
| juliusbernth | 18:5e9b288793bb | 265 | PHA.write(0); |
| juliusbernth | 18:5e9b288793bb | 266 | }else{ |
| juliusbernth | 18:5e9b288793bb | 267 | PHA.write(1); |
| juliusbernth | 18:5e9b288793bb | 268 | output = -1*output; |
| juliusbernth | 18:5e9b288793bb | 269 | } |
| juliusbernth | 18:5e9b288793bb | 270 | |
| juliusbernth | 18:5e9b288793bb | 271 | // if(demandSpeed_RPM = 0.0){ |
| juliusbernth | 18:5e9b288793bb | 272 | // output = 0.0; |
| juliusbernth | 18:5e9b288793bb | 273 | // } |
| juliusbernth | 18:5e9b288793bb | 274 | |
| juliusbernth | 12:bc34f264e2f2 | 275 | // if(state == STATE_ERROR){ |
| juliusbernth | 12:bc34f264e2f2 | 276 | // output = 0; |
| juliusbernth | 12:bc34f264e2f2 | 277 | // } |
| juliusbernth | 18:5e9b288793bb | 278 | |
| juliusbernth | 18:5e9b288793bb | 279 | PWMA.write(output);//write to motor |
| juliusbernth | 18:5e9b288793bb | 280 | lastPulses = currentPulses;//update |
| juliusbernth | 18:5e9b288793bb | 281 | lastError = error; |
| juliusbernth | 18:5e9b288793bb | 282 | lastSpeedRPM = currentSpeedRPM; |
| juliusbernth | 18:5e9b288793bb | 283 | lastErrorDot = errorDot; |
| juliusbernth | 18:5e9b288793bb | 284 | |
| juliusbernth | 18:5e9b288793bb | 285 | //exit when test has completed |
| juliusbernth | 18:5e9b288793bb | 286 | if (Tnow >= SPIN_T[5]){ |
| juliusbernth | 18:5e9b288793bb | 287 | //printf("Terminating Test %f \t %f\r\n", Tnow, SPIN_T[5]); |
| juliusbernth | 18:5e9b288793bb | 288 | state = STATE_READY;//change state |
| juliusbernth | 18:5e9b288793bb | 289 | testTimer.stop(); //stop and reset timers |
| juliusbernth | 18:5e9b288793bb | 290 | testTimer.reset(); |
| juliusbernth | 18:5e9b288793bb | 291 | Tnow = testTimer.read(); |
| juliusbernth | 18:5e9b288793bb | 292 | encoderTimer.stop(); |
| juliusbernth | 18:5e9b288793bb | 293 | encoderTimer.reset(); |
| juliusbernth | 18:5e9b288793bb | 294 | PWMA.write(0.0); |
| juliusbernth | 5:e9bb800a3742 | 295 | |
| juliusbernth | 18:5e9b288793bb | 296 | //add check if motor has stopped |
| juliusbernth | 18:5e9b288793bb | 297 | //printf("Wating for motor to stop.\r\n"); |
| juliusbernth | 18:5e9b288793bb | 298 | while(isSpinning()){ |
| juliusbernth | 18:5e9b288793bb | 299 | ThisThread::sleep_for(200); |
| juliusbernth | 7:e36f61608c10 | 300 | } |
| juliusbernth | 18:5e9b288793bb | 301 | //EN_FAULTA.write(0);//disable motor |
| juliusbernth | 18:5e9b288793bb | 302 | //EN_FAULTB.write(0); |
| juliusbernth | 18:5e9b288793bb | 303 | |
| juliusbernth | 18:5e9b288793bb | 304 | tickerMotorControl.detach(); //detach the semaphore release for motor control |
| juliusbernth | 18:5e9b288793bb | 305 | tickerPrint.detach(); |
| juliusbernth | 18:5e9b288793bb | 306 | |
| juliusbernth | 18:5e9b288793bb | 307 | //printf("state = %d\r\n",state); |
| juliusbernth | 18:5e9b288793bb | 308 | pinLedRed = 0; |
| juliusbernth | 18:5e9b288793bb | 309 | TestCompleteNotification();//send notification |
| juliusbernth | 18:5e9b288793bb | 310 | lastErrorDot = 0.0; |
| juliusbernth | 18:5e9b288793bb | 311 | lastError = 0.0; |
| juliusbernth | 18:5e9b288793bb | 312 | integralTerm = 0.0; |
| juliusbernth | 18:5e9b288793bb | 313 | //deactivate motor |
| juliusbernth | 18:5e9b288793bb | 314 | //PrintThread.terminate();//terminate print thread |
| juliusbernth | 18:5e9b288793bb | 315 | //CentrifugeTestThread.terminate();//terminate threads |
| juliusbernth | 18:5e9b288793bb | 316 | } |
| juliusbernth | 9:b1f53e4eb453 | 317 | |
| juliusbernth | 12:bc34f264e2f2 | 318 | //} //end running conditional |
| juliusbernth | 12:bc34f264e2f2 | 319 | |
| juliusbernth | 12:bc34f264e2f2 | 320 | }//end while |
| juliusbernth | 0:64442857169c | 321 | } |
| juliusbernth | 0:64442857169c | 322 | |
| juliusbernth | 5:e9bb800a3742 | 323 | void CentrifugeTest(){ |
| juliusbernth | 5:e9bb800a3742 | 324 | while(1){ |
| juliusbernth | 5:e9bb800a3742 | 325 | semStartTest.wait(); |
| juliusbernth | 17:eb74805e8f9b | 326 | //printf("\r\n Test starting \r\n"); |
| juliusbernth | 5:e9bb800a3742 | 327 | state = STATE_RUNNING; |
| juliusbernth | 0:64442857169c | 328 | //set up test |
| juliusbernth | 0:64442857169c | 329 | testTimer.reset(); |
| juliusbernth | 0:64442857169c | 330 | testTimer.start();//start timer |
| juliusbernth | 0:64442857169c | 331 | |
| juliusbernth | 5:e9bb800a3742 | 332 | char spinState; |
| juliusbernth | 8:41cd1fc8cbbe | 333 | pinLedRed = 1; |
| juliusbernth | 0:64442857169c | 334 | //set up ticker to allow motor control thread to run periodically |
| juliusbernth | 0:64442857169c | 335 | encoder.reset();//reset encoder |
| juliusbernth | 0:64442857169c | 336 | lastPulses = 0;//reset previous encoder reading |
| juliusbernth | 5:e9bb800a3742 | 337 | encoderTimer.start(); |
| juliusbernth | 5:e9bb800a3742 | 338 | PrintThread.start(PrintTimeRemaining); |
| juliusbernth | 17:eb74805e8f9b | 339 | //printf("\r\n Test setup complete, State:%d \r\n", state); |
| juliusbernth | 18:5e9b288793bb | 340 | EN_FAULTA.write(1);//enable motor |
| juliusbernth | 13:32e1ae4221f7 | 341 | |
| juliusbernth | 9:b1f53e4eb453 | 342 | //EN_FAULTB.write(1); |
| juliusbernth | 7:e36f61608c10 | 343 | if(state == STATE_RUNNING){ |
| juliusbernth | 9:b1f53e4eb453 | 344 | //printf("\r\n running %d\r\n",state); |
| juliusbernth | 7:e36f61608c10 | 345 | } |
| juliusbernth | 0:64442857169c | 346 | |
| juliusbernth | 5:e9bb800a3742 | 347 | |
| juliusbernth | 5:e9bb800a3742 | 348 | } |
| juliusbernth | 0:64442857169c | 349 | } |
| juliusbernth | 0:64442857169c | 350 | |
| juliusbernth | 0:64442857169c | 351 | void ReleaseReadButton(){ |
| juliusbernth | 0:64442857169c | 352 | semButton.release(); |
| juliusbernth | 0:64442857169c | 353 | } |
| juliusbernth | 0:64442857169c | 354 | |
| juliusbernth | 16:cefb22c49c2f | 355 | |
| juliusbernth | 13:32e1ae4221f7 | 356 | |
| juliusbernth | 0:64442857169c | 357 | void ReadButton() |
| juliusbernth | 0:64442857169c | 358 | { |
| juliusbernth | 0:64442857169c | 359 | int countThreashold = int(BUTTON_HOLD_TIME_S/BUTTON_READ_SAMPLETIME_S); |
| juliusbernth | 12:bc34f264e2f2 | 360 | int errorBlinkCount; |
| juliusbernth | 0:64442857169c | 361 | while (1) |
| juliusbernth | 0:64442857169c | 362 | { |
| juliusbernth | 0:64442857169c | 363 | semButton.wait(); |
| juliusbernth | 12:bc34f264e2f2 | 364 | |
| juliusbernth | 12:bc34f264e2f2 | 365 | if(state == STATE_ERROR){ |
| juliusbernth | 12:bc34f264e2f2 | 366 | |
| juliusbernth | 12:bc34f264e2f2 | 367 | } |
| juliusbernth | 12:bc34f264e2f2 | 368 | |
| juliusbernth | 0:64442857169c | 369 | int count = 0; |
| juliusbernth | 9:b1f53e4eb453 | 370 | int blinkCount = 0; |
| juliusbernth | 14:81d390496d4e | 371 | if(state == STATE_READY){ |
| juliusbernth | 17:eb74805e8f9b | 372 | //pinLedRed = 0; |
| juliusbernth | 14:81d390496d4e | 373 | } |
| juliusbernth | 8:41cd1fc8cbbe | 374 | while(pinButton.read()) |
| juliusbernth | 0:64442857169c | 375 | { |
| juliusbernth | 0:64442857169c | 376 | count++; |
| juliusbernth | 9:b1f53e4eb453 | 377 | if(count < countThreashold){ |
| juliusbernth | 9:b1f53e4eb453 | 378 | blinkCount++; |
| juliusbernth | 9:b1f53e4eb453 | 379 | if(blinkCount>=10){ |
| juliusbernth | 9:b1f53e4eb453 | 380 | blinkCount = 0; |
| juliusbernth | 9:b1f53e4eb453 | 381 | pinLedRed = !pinLedRed; |
| juliusbernth | 9:b1f53e4eb453 | 382 | if(count == 1){ |
| juliusbernth | 17:eb74805e8f9b | 383 | pinLedRed = 1; |
| juliusbernth | 9:b1f53e4eb453 | 384 | } |
| juliusbernth | 9:b1f53e4eb453 | 385 | } |
| juliusbernth | 9:b1f53e4eb453 | 386 | } |
| juliusbernth | 9:b1f53e4eb453 | 387 | else{ |
| juliusbernth | 9:b1f53e4eb453 | 388 | pinLedRed = 1; |
| juliusbernth | 9:b1f53e4eb453 | 389 | } |
| juliusbernth | 0:64442857169c | 390 | if (count ==1){ |
| juliusbernth | 11:62d2a592b1ae | 391 | if(state == STATE_READY){ |
| juliusbernth | 17:eb74805e8f9b | 392 | //printf("Button pressed. Hold for %f s\r\n",BUTTON_HOLD_TIME_S); |
| juliusbernth | 11:62d2a592b1ae | 393 | } |
| juliusbernth | 11:62d2a592b1ae | 394 | else if(state == STATE_RUNNING){ |
| juliusbernth | 17:eb74805e8f9b | 395 | //printf("Test terminated by user. Slowing to stop. Please restart system when safe.\r\n"); |
| juliusbernth | 11:62d2a592b1ae | 396 | state = STATE_ERROR; |
| juliusbernth | 11:62d2a592b1ae | 397 | } |
| juliusbernth | 0:64442857169c | 398 | } |
| juliusbernth | 0:64442857169c | 399 | ThisThread::sleep_for(10); |
| juliusbernth | 0:64442857169c | 400 | } |
| juliusbernth | 7:e36f61608c10 | 401 | switch (state) |
| juliusbernth | 0:64442857169c | 402 | { |
| juliusbernth | 0:64442857169c | 403 | case STATE_READY: |
| juliusbernth | 0:64442857169c | 404 | if(count > countThreashold) |
| juliusbernth | 0:64442857169c | 405 | { |
| juliusbernth | 9:b1f53e4eb453 | 406 | pinLedRed = 1; |
| juliusbernth | 9:b1f53e4eb453 | 407 | //printf("button released count = %d\r\n",count); |
| juliusbernth | 0:64442857169c | 408 | count = 0; |
| juliusbernth | 0:64442857169c | 409 | //CentrifugeTestThread.start(CentrifugeTest); |
| juliusbernth | 13:32e1ae4221f7 | 410 | if(!isSpinning()){ |
| juliusbernth | 13:32e1ae4221f7 | 411 | semStartTest.release(); |
| juliusbernth | 13:32e1ae4221f7 | 412 | tickerMotorControl.attach_us(&MotorControlRelease, SAMPLE_TIME_US);//set up signal for control frequency |
| juliusbernth | 13:32e1ae4221f7 | 413 | tickerPrint.attach(&PrintRelease,PRINT_TIME_S); |
| juliusbernth | 13:32e1ae4221f7 | 414 | }else{ |
| juliusbernth | 17:eb74805e8f9b | 415 | //printf("Holder still spinning. Wait until it has stopped.\r\n"); |
| juliusbernth | 13:32e1ae4221f7 | 416 | } |
| juliusbernth | 9:b1f53e4eb453 | 417 | }else{ |
| juliusbernth | 9:b1f53e4eb453 | 418 | pinLedRed = 0; |
| juliusbernth | 0:64442857169c | 419 | } |
| juliusbernth | 0:64442857169c | 420 | break; |
| juliusbernth | 0:64442857169c | 421 | case STATE_RUNNING: |
| juliusbernth | 0:64442857169c | 422 | if(count >1){ |
| juliusbernth | 11:62d2a592b1ae | 423 | //EN_FAULTA.write(0); |
| juliusbernth | 11:62d2a592b1ae | 424 | // EN_FAULTB.write(0); |
| juliusbernth | 9:b1f53e4eb453 | 425 | state = STATE_ERROR; |
| juliusbernth | 9:b1f53e4eb453 | 426 | //EN_FAULTB.write(0); |
| juliusbernth | 0:64442857169c | 427 | } |
| juliusbernth | 0:64442857169c | 428 | break; |
| juliusbernth | 0:64442857169c | 429 | |
| juliusbernth | 0:64442857169c | 430 | case STATE_ERROR: |
| juliusbernth | 9:b1f53e4eb453 | 431 | //printf("Please restart the system. \r\n"); |
| juliusbernth | 17:eb74805e8f9b | 432 | pinLedRed = !pinLedRed; |
| juliusbernth | 17:eb74805e8f9b | 433 | ThisThread::sleep_for(400); |
| juliusbernth | 17:eb74805e8f9b | 434 | // errorBlinkCount++; |
| juliusbernth | 17:eb74805e8f9b | 435 | // if(errorBlinkCount >=50){ |
| juliusbernth | 17:eb74805e8f9b | 436 | // errorBlinkCount = 0; |
| juliusbernth | 17:eb74805e8f9b | 437 | // |
| juliusbernth | 17:eb74805e8f9b | 438 | // } |
| juliusbernth | 0:64442857169c | 439 | break; |
| juliusbernth | 0:64442857169c | 440 | default: |
| juliusbernth | 0:64442857169c | 441 | break; |
| juliusbernth | 0:64442857169c | 442 | } |
| juliusbernth | 0:64442857169c | 443 | count = 0; |
| juliusbernth | 5:e9bb800a3742 | 444 | ThisThread::sleep_for(100); |
| juliusbernth | 0:64442857169c | 445 | } |
| juliusbernth | 0:64442857169c | 446 | } |
| juliusbernth | 0:64442857169c | 447 | |
| juliusbernth | 0:64442857169c | 448 | int main(){ |
| juliusbernth | 18:5e9b288793bb | 449 | printf("\r\nSystem Online\r\n"); |
| juliusbernth | 0:64442857169c | 450 | //set up system |
| juliusbernth | 0:64442857169c | 451 | |
| juliusbernth | 0:64442857169c | 452 | //set up motor |
| juliusbernth | 0:64442857169c | 453 | //EN_FAULTA.mode(PullDown); |
| juliusbernth | 0:64442857169c | 454 | //STBY.mode(PullDown); |
| juliusbernth | 0:64442857169c | 455 | |
| juliusbernth | 0:64442857169c | 456 | REF_PWM_A.period_us(100); |
| juliusbernth | 9:b1f53e4eb453 | 457 | PWMA.period_us(33); |
| juliusbernth | 17:eb74805e8f9b | 458 | REF_PWM_A.write(0.05);//set current reference |
| juliusbernth | 9:b1f53e4eb453 | 459 | |
| juliusbernth | 9:b1f53e4eb453 | 460 | REF_PWMB.period_us(100); |
| juliusbernth | 9:b1f53e4eb453 | 461 | PWMB.period_us(33); |
| juliusbernth | 9:b1f53e4eb453 | 462 | REF_PWMB.write(0.0); |
| juliusbernth | 9:b1f53e4eb453 | 463 | PHB.write(0); |
| juliusbernth | 9:b1f53e4eb453 | 464 | EN_FAULTB.write(0); |
| juliusbernth | 0:64442857169c | 465 | |
| juliusbernth | 16:cefb22c49c2f | 466 | EN_FAULTA.write(1);//remove incase of restart? |
| juliusbernth | 0:64442857169c | 467 | STBY.write(1); |
| juliusbernth | 0:64442857169c | 468 | //calculate filter constant |
| juliusbernth | 0:64442857169c | 469 | speedFilterConstant = 2*3.141593*SAMPLE_TIME_US/1000000*FILTER_CUTOFF_FREQ; |
| juliusbernth | 0:64442857169c | 470 | speedFilterConstant /= (1 + speedFilterConstant); |
| juliusbernth | 0:64442857169c | 471 | |
| juliusbernth | 0:64442857169c | 472 | SPIN_T[0] = 0.0; |
| juliusbernth | 0:64442857169c | 473 | SPIN_T[1] = T_WARMUP; |
| juliusbernth | 0:64442857169c | 474 | SPIN_T[2] = T_WARMUP + T_RAMP; |
| juliusbernth | 0:64442857169c | 475 | SPIN_T[3] = T_WARMUP + T_RAMP + T_TEST; |
| juliusbernth | 0:64442857169c | 476 | SPIN_T[4] = T_WARMUP + T_RAMP + T_TEST + T_RAMP; |
| juliusbernth | 0:64442857169c | 477 | SPIN_T[5] = T_WARMUP + T_RAMP + T_TEST + T_RAMP + T_WARMUP; |
| juliusbernth | 0:64442857169c | 478 | |
| juliusbernth | 0:64442857169c | 479 | /* //set up button interrupts |
| juliusbernth | 0:64442857169c | 480 | pinButton.rise(ISR_Button_Rise); |
| juliusbernth | 0:64442857169c | 481 | pinButton.fall(ISR_Button_Fall); |
| juliusbernth | 0:64442857169c | 482 | */ |
| juliusbernth | 8:41cd1fc8cbbe | 483 | pinButton.mode(PullDown); |
| juliusbernth | 8:41cd1fc8cbbe | 484 | pinLedWhite = 1; |
| juliusbernth | 8:41cd1fc8cbbe | 485 | |
| juliusbernth | 8:41cd1fc8cbbe | 486 | |
| juliusbernth | 5:e9bb800a3742 | 487 | |
| juliusbernth | 5:e9bb800a3742 | 488 | state = STATE_READY;//set state to READY |
| juliusbernth | 0:64442857169c | 489 | //start all threads |
| juliusbernth | 0:64442857169c | 490 | MotorControlThread.start(MotorControl); |
| juliusbernth | 0:64442857169c | 491 | |
| juliusbernth | 5:e9bb800a3742 | 492 | CentrifugeTestThread.start(CentrifugeTest); |
| juliusbernth | 0:64442857169c | 493 | //start print thread. |
| juliusbernth | 0:64442857169c | 494 | |
| juliusbernth | 0:64442857169c | 495 | ReadButtonThread.start(ReadButton); |
| juliusbernth | 0:64442857169c | 496 | tickerReleaseButton.attach(ReleaseReadButton,BUTTON_READ_SAMPLETIME_S); |
| juliusbernth | 0:64442857169c | 497 | |
| juliusbernth | 0:64442857169c | 498 | |
| juliusbernth | 17:eb74805e8f9b | 499 | //printf("\r\nSetup Complete\r\n"); |
| juliusbernth | 0:64442857169c | 500 | while (true) { |
| juliusbernth | 0:64442857169c | 501 | //check state of button |
| juliusbernth | 0:64442857169c | 502 | ThisThread::sleep_for(osWaitForever); |
| juliusbernth | 0:64442857169c | 503 | } |
| juliusbernth | 0:64442857169c | 504 | } |