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@4:d04afc466198, 2021-08-08 (annotated)
- Committer:
- juliusbernth
- Date:
- Sun Aug 08 14:59:54 2021 +0000
- Revision:
- 4:d04afc466198
- Parent:
- 3:4f215646a42b
- Child:
- 5:e9bb800a3742
Updated to first Testable version.
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 | 3:4f215646a42b | 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 | 4:d04afc466198 | 15 | - Test how breaking is performed. |
| juliusbernth | 3:4f215646a42b | 16 | - Test onboard current limitation |
| juliusbernth | 0:64442857169c | 17 | */ |
| juliusbernth | 0:64442857169c | 18 | |
| juliusbernth | 0:64442857169c | 19 | Timer encoderTimer; |
| juliusbernth | 0:64442857169c | 20 | Timer buttonTimer; |
| juliusbernth | 0:64442857169c | 21 | Timer testTimer; |
| juliusbernth | 0:64442857169c | 22 | |
| juliusbernth | 0:64442857169c | 23 | Timeout buttonTimeout; |
| juliusbernth | 0:64442857169c | 24 | |
| juliusbernth | 0:64442857169c | 25 | Thread PrintThread(osPriorityAboveNormal); |
| juliusbernth | 0:64442857169c | 26 | Thread CentrifugeTestThread(osPriorityHigh); |
| juliusbernth | 0:64442857169c | 27 | Thread ReadButtonThread(osPriorityNormal); |
| juliusbernth | 0:64442857169c | 28 | Thread MotorControlThread(osPriorityRealtime); |
| juliusbernth | 0:64442857169c | 29 | |
| juliusbernth | 0:64442857169c | 30 | double SPIN_T[6]; //times for spin cycle |
| juliusbernth | 0:64442857169c | 31 | |
| juliusbernth | 0:64442857169c | 32 | QEI encoder(PA_14, PA_13, PA_15, 256, QEI::X4_ENCODING); |
| juliusbernth | 0:64442857169c | 33 | |
| juliusbernth | 0:64442857169c | 34 | //LIS3DH accel = LIS3DH(SPI_MOSI, SPI_MISO, SPI_SCLK, SPI_CS,LIS3DH_DR_NR_LP_400HZ, LIS3DH_FS_2G); |
| juliusbernth | 0:64442857169c | 35 | |
| juliusbernth | 0:64442857169c | 36 | DigitalOut EN_FAULTA (D2); |
| juliusbernth | 0:64442857169c | 37 | DigitalOut STBY (D8); |
| juliusbernth | 0:64442857169c | 38 | DigitalOut PHA (D3); |
| juliusbernth | 0:64442857169c | 39 | DigitalOut PHB (D7); |
| juliusbernth | 0:64442857169c | 40 | PwmOut PWMA (D5); |
| juliusbernth | 0:64442857169c | 41 | PwmOut PWMB (D4); |
| juliusbernth | 0:64442857169c | 42 | PwmOut REF_PWM_A (D15); |
| juliusbernth | 0:64442857169c | 43 | PwmOut REF_PWM_B (D14); |
| juliusbernth | 0:64442857169c | 44 | |
| juliusbernth | 0:64442857169c | 45 | Serial pc(USBTX,USBRX); |
| juliusbernth | 0:64442857169c | 46 | |
| juliusbernth | 0:64442857169c | 47 | DigitalOut pinStatusLED (D13); |
| juliusbernth | 0:64442857169c | 48 | DigitalIn pinButton (PC_13); |
| juliusbernth | 0:64442857169c | 49 | |
| juliusbernth | 0:64442857169c | 50 | /* Still to allocate |
| juliusbernth | 0:64442857169c | 51 | |
| juliusbernth | 0:64442857169c | 52 | DigitalOut pinBuzzer |
| juliusbernth | 0:64442857169c | 53 | DigitalOut pinPowerLED |
| juliusbernth | 0:64442857169c | 54 | |
| juliusbernth | 0:64442857169c | 55 | */ |
| juliusbernth | 0:64442857169c | 56 | |
| juliusbernth | 0:64442857169c | 57 | //global variables |
| juliusbernth | 0:64442857169c | 58 | double speedFilterConstant; |
| juliusbernth | 3:4f215646a42b | 59 | char _state; |
| juliusbernth | 3:4f215646a42b | 60 | char _errorId = 0x00; |
| juliusbernth | 3:4f215646a42b | 61 | bool _isErrorMsg = 0; |
| juliusbernth | 3:4f215646a42b | 62 | |
| juliusbernth | 3:4f215646a42b | 63 | |
| juliusbernth | 0:64442857169c | 64 | double currentPulses; |
| juliusbernth | 0:64442857169c | 65 | double lastPulses; |
| juliusbernth | 0:64442857169c | 66 | double lastSpeedRPM; |
| juliusbernth | 0:64442857169c | 67 | double Tnow; |
| juliusbernth | 3:4f215646a42b | 68 | double _demandSpeed_RPM = 0.0; |
| juliusbernth | 3:4f215646a42b | 69 | double _currentSpeed_RPM; |
| juliusbernth | 0:64442857169c | 70 | double deltaError; |
| juliusbernth | 0:64442857169c | 71 | double lastError; |
| juliusbernth | 0:64442857169c | 72 | double integralTerm; |
| juliusbernth | 0:64442857169c | 73 | double output; |
| juliusbernth | 0:64442857169c | 74 | |
| juliusbernth | 0:64442857169c | 75 | Ticker tickerMotorControl; |
| juliusbernth | 0:64442857169c | 76 | Ticker tickerPrint; |
| juliusbernth | 0:64442857169c | 77 | Ticker tickerReleaseButton; |
| juliusbernth | 0:64442857169c | 78 | |
| juliusbernth | 0:64442857169c | 79 | Semaphore semMotorControl(0); |
| juliusbernth | 0:64442857169c | 80 | Semaphore semPrint(0); |
| juliusbernth | 0:64442857169c | 81 | Semaphore semStartTest(0); |
| juliusbernth | 0:64442857169c | 82 | Semaphore semButton(0); |
| juliusbernth | 0:64442857169c | 83 | |
| juliusbernth | 3:4f215646a42b | 84 | Mutex mut1; |
| juliusbernth | 0:64442857169c | 85 | |
| juliusbernth | 0:64442857169c | 86 | double LimitDouble(double input, double min, double max){ |
| juliusbernth | 0:64442857169c | 87 | double output; |
| juliusbernth | 0:64442857169c | 88 | output = input; |
| juliusbernth | 0:64442857169c | 89 | if (input > max){ |
| juliusbernth | 0:64442857169c | 90 | output = max; |
| juliusbernth | 0:64442857169c | 91 | } |
| juliusbernth | 0:64442857169c | 92 | if (input < min){ |
| juliusbernth | 0:64442857169c | 93 | output = min; |
| juliusbernth | 0:64442857169c | 94 | } |
| juliusbernth | 0:64442857169c | 95 | return output; |
| juliusbernth | 0:64442857169c | 96 | } |
| juliusbernth | 0:64442857169c | 97 | /* |
| juliusbernth | 0:64442857169c | 98 | double GetSpeed(int pulses, int lastPulses, double currentSpeed){ |
| juliusbernth | 0:64442857169c | 99 | double filteredSpeedRPM; |
| juliusbernth | 0:64442857169c | 100 | |
| juliusbernth | 0:64442857169c | 101 | //find change in pulses. |
| juliusbernth | 0:64442857169c | 102 | double speedRPM = (double) (pulses - lastPulses); //get change in angle |
| juliusbernth | 0:64442857169c | 103 | speedRPM /= encoderTimer.read_us(); //calculate speed in pulses/us |
| juliusbernth | 0:64442857169c | 104 | encoderTimer.reset(); |
| juliusbernth | 0:64442857169c | 105 | speedRPM *= 60000000.0/PULSES_PER_REV/4.0;//convert to RPM; |
| juliusbernth | 0:64442857169c | 106 | |
| juliusbernth | 0:64442857169c | 107 | //filter speed |
| juliusbernth | 0:64442857169c | 108 | //filteredSpeedRPM = speedFilterConstant * speedRPM + currentSpeed - speedFilterConstant * currentSpeed; |
| juliusbernth | 0:64442857169c | 109 | filteredSpeedRPM = speedRPM; |
| juliusbernth | 0:64442857169c | 110 | return filteredSpeedRPM; |
| juliusbernth | 0:64442857169c | 111 | }*/ |
| juliusbernth | 0:64442857169c | 112 | |
| juliusbernth | 0:64442857169c | 113 | |
| juliusbernth | 0:64442857169c | 114 | void PIDControl(){ |
| juliusbernth | 0:64442857169c | 115 | |
| juliusbernth | 0:64442857169c | 116 | } |
| juliusbernth | 0:64442857169c | 117 | |
| juliusbernth | 0:64442857169c | 118 | void TestCompleteNotification(){ |
| juliusbernth | 0:64442857169c | 119 | //send message |
| juliusbernth | 0:64442857169c | 120 | printf("\r\nTest complete\r\n"); |
| juliusbernth | 0:64442857169c | 121 | //sound buzzer and blink 3 times |
| juliusbernth | 0:64442857169c | 122 | } |
| juliusbernth | 0:64442857169c | 123 | |
| juliusbernth | 0:64442857169c | 124 | void MotorControlRelease(){ |
| juliusbernth | 0:64442857169c | 125 | semMotorControl.release(); |
| juliusbernth | 0:64442857169c | 126 | } |
| juliusbernth | 0:64442857169c | 127 | void PrintRelease(){ |
| juliusbernth | 0:64442857169c | 128 | semPrint.release(); |
| juliusbernth | 0:64442857169c | 129 | } |
| juliusbernth | 0:64442857169c | 130 | |
| juliusbernth | 3:4f215646a42b | 131 | void PrintMessages(){ |
| juliusbernth | 0:64442857169c | 132 | while(1){ |
| juliusbernth | 0:64442857169c | 133 | semPrint.wait(); |
| juliusbernth | 3:4f215646a42b | 134 | |
| juliusbernth | 3:4f215646a42b | 135 | mut1.lock();//lock mutex to prevent race condition. |
| juliusbernth | 3:4f215646a42b | 136 | double localDemandSpeed = _demandSpeed_RPM; |
| juliusbernth | 3:4f215646a42b | 137 | double localCurrentSpeed = _currentSpeed_RPM; |
| juliusbernth | 3:4f215646a42b | 138 | char localErrorId = _errorId; |
| juliusbernth | 3:4f215646a42b | 139 | bool localIsErrorMsg = _isErrorMsg; |
| juliusbernth | 3:4f215646a42b | 140 | char localState = _state; |
| juliusbernth | 3:4f215646a42b | 141 | mut1.unlock(); |
| juliusbernth | 3:4f215646a42b | 142 | |
| juliusbernth | 3:4f215646a42b | 143 | if(localState == STATE_RUNNING){ |
| juliusbernth | 0:64442857169c | 144 | double timeElapsed = testTimer.read(); |
| juliusbernth | 0:64442857169c | 145 | double timeRemaining = SPIN_T[5] - timeElapsed; |
| juliusbernth | 0:64442857169c | 146 | int displayTime = int(timeRemaining)+1; |
| juliusbernth | 0:64442857169c | 147 | //printf("Time remaining %d s\r\n", displayTime); |
| juliusbernth | 3:4f215646a42b | 148 | |
| juliusbernth | 3:4f215646a42b | 149 | printf("%f\t%f\r\n",localDemandSpeed, localCurrentSpeed); |
| juliusbernth | 0:64442857169c | 150 | } |
| juliusbernth | 3:4f215646a42b | 151 | if(localIsErrorMsg){ |
| juliusbernth | 3:4f215646a42b | 152 | localIsErrorMsg = 0;//clear local error message flag. |
| juliusbernth | 3:4f215646a42b | 153 | mut1.lock(); |
| juliusbernth | 3:4f215646a42b | 154 | _isErrorMsg = 0;//clear global error message flag. |
| juliusbernth | 3:4f215646a42b | 155 | mut1.unlock(); |
| juliusbernth | 3:4f215646a42b | 156 | |
| juliusbernth | 3:4f215646a42b | 157 | switch (localErrorId){ |
| juliusbernth | 3:4f215646a42b | 158 | case ERROR_ACCEL: |
| juliusbernth | 3:4f215646a42b | 159 | printf("Excess vibration detected\r\n"); |
| juliusbernth | 3:4f215646a42b | 160 | break; |
| juliusbernth | 3:4f215646a42b | 161 | case ERROR_MAN_STOP: |
| juliusbernth | 3:4f215646a42b | 162 | printf("Manual stop detected\r\n"); |
| juliusbernth | 3:4f215646a42b | 163 | break; |
| juliusbernth | 3:4f215646a42b | 164 | default: |
| juliusbernth | 3:4f215646a42b | 165 | break; |
| juliusbernth | 3:4f215646a42b | 166 | } |
| juliusbernth | 3:4f215646a42b | 167 | } |
| juliusbernth | 3:4f215646a42b | 168 | } |
| juliusbernth | 3:4f215646a42b | 169 | } |
| juliusbernth | 3:4f215646a42b | 170 | |
| juliusbernth | 3:4f215646a42b | 171 | int CheckAccelerometer(){ |
| juliusbernth | 3:4f215646a42b | 172 | //read accelerometer |
| juliusbernth | 3:4f215646a42b | 173 | double xAccel; |
| juliusbernth | 3:4f215646a42b | 174 | double yAccel; |
| juliusbernth | 3:4f215646a42b | 175 | |
| juliusbernth | 3:4f215646a42b | 176 | double magnitude = xAccel*xAccel + yAccel*yAccel; |
| juliusbernth | 3:4f215646a42b | 177 | magnitude = sqrt(magnitude); |
| juliusbernth | 3:4f215646a42b | 178 | |
| juliusbernth | 3:4f215646a42b | 179 | if (magnitude > VIBRATION_THRESHOLD){ |
| juliusbernth | 3:4f215646a42b | 180 | return 1; |
| juliusbernth | 3:4f215646a42b | 181 | } |
| juliusbernth | 3:4f215646a42b | 182 | else{ |
| juliusbernth | 3:4f215646a42b | 183 | return 0; |
| juliusbernth | 0:64442857169c | 184 | } |
| juliusbernth | 0:64442857169c | 185 | } |
| juliusbernth | 0:64442857169c | 186 | |
| juliusbernth | 0:64442857169c | 187 | void MotorControl(){ |
| juliusbernth | 0:64442857169c | 188 | while(1){ |
| juliusbernth | 0:64442857169c | 189 | semMotorControl.wait();//wait for a signal |
| juliusbernth | 3:4f215646a42b | 190 | //grab global variables |
| juliusbernth | 3:4f215646a42b | 191 | mut1.lock();//lock mutex to prevent race condition. |
| juliusbernth | 3:4f215646a42b | 192 | char localErrorId = _errorId; |
| juliusbernth | 3:4f215646a42b | 193 | bool localIsErrorMsg = _isErrorMsg; |
| juliusbernth | 3:4f215646a42b | 194 | char localState = _state; |
| juliusbernth | 3:4f215646a42b | 195 | mut1.unlock(); |
| juliusbernth | 3:4f215646a42b | 196 | |
| juliusbernth | 3:4f215646a42b | 197 | Tnow = testTimer.read(); |
| juliusbernth | 3:4f215646a42b | 198 | //check accel. If problem, change state to ERROR |
| juliusbernth | 3:4f215646a42b | 199 | if(CheckAccelerometer()){ |
| juliusbernth | 3:4f215646a42b | 200 | mut1.lock(); |
| juliusbernth | 3:4f215646a42b | 201 | _state= STATE_HALT; |
| juliusbernth | 3:4f215646a42b | 202 | _errorId = ERROR_ACCEL; |
| juliusbernth | 3:4f215646a42b | 203 | _isErrorMsg = 1;//set flag for sending an error message |
| juliusbernth | 3:4f215646a42b | 204 | mut1.unlock(); |
| juliusbernth | 3:4f215646a42b | 205 | } |
| juliusbernth | 3:4f215646a42b | 206 | //int deltaT = encoderTimer.read();//read current time in seconds //don't need this???? |
| juliusbernth | 3:4f215646a42b | 207 | static double demandSpeed_RPM; |
| juliusbernth | 3:4f215646a42b | 208 | |
| juliusbernth | 3:4f215646a42b | 209 | if(localState == STATE_RUNNING){//need to check if this is the best condition to look for. |
| juliusbernth | 0:64442857169c | 210 | //calculate current demand |
| juliusbernth | 3:4f215646a42b | 211 | /*if ( (SPIN_T[0] <= Tnow) && (Tnow < SPIN_T[1]) ) {//test is in warmup |
| juliusbernth | 0:64442857169c | 212 | demandSpeed_RPM = 0.0; |
| juliusbernth | 3:4f215646a42b | 213 | }*/ |
| juliusbernth | 0:64442857169c | 214 | if ((SPIN_T[1] <= Tnow)&&(Tnow < SPIN_T[2])){//test is in ramp up |
| juliusbernth | 3:4f215646a42b | 215 | //double a = Tnow - SPIN_T[1]; |
| juliusbernth | 3:4f215646a42b | 216 | //demandSpeed_RPM = a * targetSpeed_RPM / T_RAMP; |
| juliusbernth | 3:4f215646a42b | 217 | demandSpeed_RPM += targetSpeed_RPM/T_RAMP*SAMPLE_TIME_US/1000000; |
| juliusbernth | 0:64442857169c | 218 | } |
| juliusbernth | 3:4f215646a42b | 219 | else if ((SPIN_T[2] <= Tnow)&&(Tnow < SPIN_T[3])){//test is in coast |
| juliusbernth | 0:64442857169c | 220 | demandSpeed_RPM = targetSpeed_RPM; |
| juliusbernth | 3:4f215646a42b | 221 | } |
| juliusbernth | 3:4f215646a42b | 222 | else if ((SPIN_T[3] <= Tnow)&&(Tnow < SPIN_T[4])){//test is in ramp down |
| juliusbernth | 3:4f215646a42b | 223 | //double a = Tnow - SPIN_T[3]; |
| juliusbernth | 3:4f215646a42b | 224 | //demandSpeed_RPM = targetSpeed_RPM - (a/T_RAMP*targetSpeed_RPM); |
| juliusbernth | 3:4f215646a42b | 225 | demandSpeed_RPM -= targetSpeed_RPM/T_RAMP*SAMPLE_TIME_US/1000000; |
| juliusbernth | 0:64442857169c | 226 | } |
| juliusbernth | 3:4f215646a42b | 227 | else {//((SPIN_T[4] <= Tnow)&&(Tnow < SPIN_T[5])){//test is in cooldown |
| juliusbernth | 3:4f215646a42b | 228 | demandSpeed_RPM = 0.0; |
| juliusbernth | 0:64442857169c | 229 | } |
| juliusbernth | 3:4f215646a42b | 230 | } |
| juliusbernth | 3:4f215646a42b | 231 | if(localState == STATE_HALT){//if halt condition is set |
| juliusbernth | 3:4f215646a42b | 232 | if(demandSpeed_RPM > 0.0){ |
| juliusbernth | 3:4f215646a42b | 233 | demandSpeed_RPM -= targetSpeed_RPM/T_RAMP*SAMPLE_TIME_US/1000000; |
| juliusbernth | 3:4f215646a42b | 234 | } |
| juliusbernth | 3:4f215646a42b | 235 | else{ |
| juliusbernth | 3:4f215646a42b | 236 | mut1.lock(); |
| juliusbernth | 3:4f215646a42b | 237 | _state = STATE_ERROR;//once demand speed is 0, go to error state. |
| juliusbernth | 3:4f215646a42b | 238 | mut1.unlock(); |
| juliusbernth | 0:64442857169c | 239 | } |
| juliusbernth | 3:4f215646a42b | 240 | } |
| juliusbernth | 3:4f215646a42b | 241 | demandSpeed_RPM = LimitDouble(demandSpeed_RPM,0.0,MAX_SPEED_RPM);//limit demand |
| juliusbernth | 3:4f215646a42b | 242 | currentPulses = encoder.getPulses();//calculate current speed |
| juliusbernth | 3:4f215646a42b | 243 | double deltaPulses = currentPulses - lastPulses; |
| juliusbernth | 3:4f215646a42b | 244 | double speed_RPM = deltaPulses / SAMPLE_TIME_US * 1000000;//get in pulses per second |
| juliusbernth | 3:4f215646a42b | 245 | speed_RPM *= 60/4/PULSES_PER_REV;//convert to RPM |
| juliusbernth | 3:4f215646a42b | 246 | double currentSpeed_RPM = 0.3*speed_RPM + 0.7*lastSpeedRPM;//filter speed |
| juliusbernth | 3:4f215646a42b | 247 | |
| juliusbernth | 3:4f215646a42b | 248 | mut1.lock();//update global variables |
| juliusbernth | 4:d04afc466198 | 249 | _demandSpeed_RPM = demandSpeed_RPM; |
| juliusbernth | 3:4f215646a42b | 250 | _currentSpeed_RPM = currentSpeed_RPM; |
| juliusbernth | 3:4f215646a42b | 251 | mut1.unlock();//end of global variable write |
| juliusbernth | 3:4f215646a42b | 252 | |
| juliusbernth | 3:4f215646a42b | 253 | double error = demandSpeed_RPM - currentSpeed_RPM;//calculate error |
| juliusbernth | 3:4f215646a42b | 254 | deltaError = error - lastError; |
| juliusbernth | 3:4f215646a42b | 255 | double errorDot = deltaError/SAMPLE_TIME_US*1000000.0; |
| juliusbernth | 3:4f215646a42b | 256 | integralTerm += error*Ki*SAMPLE_TIME_US/1000000.0; |
| juliusbernth | 3:4f215646a42b | 257 | integralTerm = LimitDouble(integralTerm,-0.8, 0.8); |
| juliusbernth | 3:4f215646a42b | 258 | |
| juliusbernth | 3:4f215646a42b | 259 | output = Kp * error; //calculate output |
| juliusbernth | 3:4f215646a42b | 260 | output += integralTerm; |
| juliusbernth | 3:4f215646a42b | 261 | output += Kd*errorDot; |
| juliusbernth | 3:4f215646a42b | 262 | output = LimitDouble(output,-1.0,1.0); |
| juliusbernth | 3:4f215646a42b | 263 | //printf("wd:%f\t w:%f\t e:%f\t o:%f \r\n",demandSpeed_RPM, currentSpeed_RPM, error, output); |
| juliusbernth | 3:4f215646a42b | 264 | |
| juliusbernth | 3:4f215646a42b | 265 | if(output >=0){//Set direction |
| juliusbernth | 3:4f215646a42b | 266 | PHA.write(0); |
| juliusbernth | 3:4f215646a42b | 267 | } |
| juliusbernth | 3:4f215646a42b | 268 | else{ |
| juliusbernth | 3:4f215646a42b | 269 | PHA.write(1); |
| juliusbernth | 3:4f215646a42b | 270 | output = -1*output; |
| juliusbernth | 3:4f215646a42b | 271 | } |
| juliusbernth | 3:4f215646a42b | 272 | PWMA.write(output);//write to motor |
| juliusbernth | 3:4f215646a42b | 273 | |
| juliusbernth | 3:4f215646a42b | 274 | lastPulses = currentPulses;//update variables |
| juliusbernth | 3:4f215646a42b | 275 | lastError = error; |
| juliusbernth | 3:4f215646a42b | 276 | lastSpeedRPM = currentSpeed_RPM; |
| juliusbernth | 3:4f215646a42b | 277 | |
| juliusbernth | 0:64442857169c | 278 | |
| juliusbernth | 3:4f215646a42b | 279 | //exit when test has completed |
| juliusbernth | 3:4f215646a42b | 280 | if ((Tnow >= SPIN_T[5]) || (localState == STATE_ERROR)){ //either if test time has expired, or if system has gone into an error state |
| juliusbernth | 4:d04afc466198 | 281 | Timer timerMotorStopTimeout; |
| juliusbernth | 4:d04afc466198 | 282 | bool isTimeout; |
| juliusbernth | 4:d04afc466198 | 283 | timerMotorStopTimeout.start(); |
| juliusbernth | 4:d04afc466198 | 284 | if(timerMotorStopTimeout.read() > MOTOR_STOP_TIMEOUT){ |
| juliusbernth | 4:d04afc466198 | 285 | isTimeout = 1; |
| juliusbernth | 4:d04afc466198 | 286 | } |
| juliusbernth | 4:d04afc466198 | 287 | else{ |
| juliusbernth | 4:d04afc466198 | 288 | isTimeout = 0; |
| juliusbernth | 4:d04afc466198 | 289 | } |
| juliusbernth | 4:d04afc466198 | 290 | |
| juliusbernth | 4:d04afc466198 | 291 | |
| juliusbernth | 4:d04afc466198 | 292 | if((deltaPulses == 0) || isTimeout){//check if motor has come to a complete stop or if timeout has occured |
| juliusbernth | 4:d04afc466198 | 293 | timerMotorStopTimeout.stop(); |
| juliusbernth | 4:d04afc466198 | 294 | timerMotorStopTimeout.reset(); |
| juliusbernth | 3:4f215646a42b | 295 | tickerPrint.detach();//stop the print thread from firing |
| juliusbernth | 3:4f215646a42b | 296 | PrintThread.terminate();//terminate print thread??? |
| juliusbernth | 3:4f215646a42b | 297 | EN_FAULTA.write(0);//disable motor |
| juliusbernth | 3:4f215646a42b | 298 | tickerMotorControl.detach(); //detach the semaphore release for motor control |
| juliusbernth | 3:4f215646a42b | 299 | |
| juliusbernth | 3:4f215646a42b | 300 | //ensure rotor has come to a complete stop. |
| juliusbernth | 3:4f215646a42b | 301 | int deltaPulses; |
| juliusbernth | 3:4f215646a42b | 302 | deltaPulses = encoder.getPulses();//calculate current change in pulses |
| juliusbernth | 3:4f215646a42b | 303 | while(deltaPulses>0){//loop forever until system has come to complete stop |
| juliusbernth | 3:4f215646a42b | 304 | deltaPulses = encoder.getPulses();//calculate current speed |
| juliusbernth | 3:4f215646a42b | 305 | encoder.reset();//reset encoder count |
| juliusbernth | 3:4f215646a42b | 306 | ThisThread::sleep_for(10); |
| juliusbernth | 3:4f215646a42b | 307 | } |
| juliusbernth | 3:4f215646a42b | 308 | //Inform user why test has ended. |
| juliusbernth | 3:4f215646a42b | 309 | if(localState == !STATE_ERROR){ |
| juliusbernth | 3:4f215646a42b | 310 | printf("Test complete\r\n");//Test completed cleanly |
| juliusbernth | 3:4f215646a42b | 311 | } |
| juliusbernth | 3:4f215646a42b | 312 | else{ |
| juliusbernth | 4:d04afc466198 | 313 | mut1.lock(); |
| juliusbernth | 4:d04afc466198 | 314 | localErrorId = _errorId; |
| juliusbernth | 4:d04afc466198 | 315 | mut1.unlock(); |
| juliusbernth | 4:d04afc466198 | 316 | switch (localErrorId){ |
| juliusbernth | 3:4f215646a42b | 317 | case ERROR_ACCEL://accelerometer detected excess vibration |
| juliusbernth | 3:4f215646a42b | 318 | printf("Test terminated.\r\n Please check holder and restart system.\r\n"); |
| juliusbernth | 3:4f215646a42b | 319 | break; |
| juliusbernth | 3:4f215646a42b | 320 | case ERROR_MAN_STOP://test was terminated manually |
| juliusbernth | 3:4f215646a42b | 321 | printf("Test terminated manually. \r\nPlease restart system to resume testing.\r\n"); |
| juliusbernth | 3:4f215646a42b | 322 | break; |
| juliusbernth | 3:4f215646a42b | 323 | default: |
| juliusbernth | 3:4f215646a42b | 324 | break; |
| juliusbernth | 3:4f215646a42b | 325 | } |
| juliusbernth | 3:4f215646a42b | 326 | } |
| juliusbernth | 4:d04afc466198 | 327 | mut1.lock(); |
| juliusbernth | 3:4f215646a42b | 328 | _state = STATE_READY;//change state |
| juliusbernth | 3:4f215646a42b | 329 | mut1.unlock(); |
| juliusbernth | 3:4f215646a42b | 330 | demandSpeed_RPM = 0.0;//update variables |
| juliusbernth | 3:4f215646a42b | 331 | lastPulses = 0.0; |
| juliusbernth | 3:4f215646a42b | 332 | lastError = 0.0; |
| juliusbernth | 3:4f215646a42b | 333 | lastSpeedRPM = 0.0; |
| juliusbernth | 0:64442857169c | 334 | testTimer.stop(); //stop and reset timers |
| juliusbernth | 0:64442857169c | 335 | testTimer.reset(); |
| juliusbernth | 0:64442857169c | 336 | Tnow = testTimer.read(); |
| juliusbernth | 3:4f215646a42b | 337 | encoder.reset();//reset encoder |
| juliusbernth | 3:4f215646a42b | 338 | //encoderTimer.stop(); |
| juliusbernth | 3:4f215646a42b | 339 | //encoderTimer.reset(); |
| juliusbernth | 0:64442857169c | 340 | |
| juliusbernth | 3:4f215646a42b | 341 | |
| juliusbernth | 4:d04afc466198 | 342 | //printf("state = %d\r\n",state); |
| juliusbernth | 0:64442857169c | 343 | TestCompleteNotification();//send notification |
| juliusbernth | 0:64442857169c | 344 | //deactivate motor |
| juliusbernth | 0:64442857169c | 345 | //CentrifugeTestThread.terminate();//terminate threads |
| juliusbernth | 0:64442857169c | 346 | } |
| juliusbernth | 0:64442857169c | 347 | } |
| juliusbernth | 0:64442857169c | 348 | } |
| juliusbernth | 0:64442857169c | 349 | } |
| juliusbernth | 0:64442857169c | 350 | |
| juliusbernth | 3:4f215646a42b | 351 | void CentrifugeTestInit(){ |
| juliusbernth | 3:4f215646a42b | 352 | // while(1){ |
| juliusbernth | 3:4f215646a42b | 353 | // semStartTest.wait(); |
| juliusbernth | 0:64442857169c | 354 | printf("\r\n Test starting \r\n"); |
| juliusbernth | 3:4f215646a42b | 355 | mut1.lock(); |
| juliusbernth | 3:4f215646a42b | 356 | _state = STATE_RUNNING; |
| juliusbernth | 3:4f215646a42b | 357 | _errorId = 0x00; |
| juliusbernth | 3:4f215646a42b | 358 | mut1.unlock(); |
| juliusbernth | 0:64442857169c | 359 | //set up test |
| juliusbernth | 0:64442857169c | 360 | testTimer.reset(); |
| juliusbernth | 0:64442857169c | 361 | testTimer.start();//start timer |
| juliusbernth | 0:64442857169c | 362 | |
| juliusbernth | 3:4f215646a42b | 363 | //char spinState; |
| juliusbernth | 0:64442857169c | 364 | |
| juliusbernth | 0:64442857169c | 365 | //set up ticker to allow motor control thread to run periodically |
| juliusbernth | 0:64442857169c | 366 | encoder.reset();//reset encoder |
| juliusbernth | 0:64442857169c | 367 | lastPulses = 0;//reset previous encoder reading |
| juliusbernth | 3:4f215646a42b | 368 | PrintThread.start(PrintMessages); |
| juliusbernth | 0:64442857169c | 369 | printf("\r\n Test setup complete \r\n"); |
| juliusbernth | 3:4f215646a42b | 370 | EN_FAULTA.write(1);//enable motor |
| juliusbernth | 3:4f215646a42b | 371 | //encoderTimer.start(); |
| juliusbernth | 3:4f215646a42b | 372 | // if(state == STATE_RUNNING){ |
| juliusbernth | 3:4f215646a42b | 373 | // printf("\r\n running %d\r\n",state); |
| juliusbernth | 3:4f215646a42b | 374 | // } |
| juliusbernth | 0:64442857169c | 375 | |
| juliusbernth | 3:4f215646a42b | 376 | //} |
| juliusbernth | 0:64442857169c | 377 | } |
| juliusbernth | 0:64442857169c | 378 | |
| juliusbernth | 0:64442857169c | 379 | void ReleaseReadButton(){ |
| juliusbernth | 0:64442857169c | 380 | semButton.release(); |
| juliusbernth | 0:64442857169c | 381 | } |
| juliusbernth | 0:64442857169c | 382 | |
| juliusbernth | 0:64442857169c | 383 | void ReadButton() |
| juliusbernth | 0:64442857169c | 384 | { |
| juliusbernth | 0:64442857169c | 385 | int countThreashold = int(BUTTON_HOLD_TIME_S/BUTTON_READ_SAMPLETIME_S); |
| juliusbernth | 0:64442857169c | 386 | while (1) |
| juliusbernth | 0:64442857169c | 387 | { |
| juliusbernth | 0:64442857169c | 388 | semButton.wait(); |
| juliusbernth | 3:4f215646a42b | 389 | mut1.lock();//grab global variables |
| juliusbernth | 3:4f215646a42b | 390 | char localErrorId = _errorId; |
| juliusbernth | 3:4f215646a42b | 391 | bool localIsErrorMsg = _isErrorMsg; |
| juliusbernth | 3:4f215646a42b | 392 | char localState = _state; |
| juliusbernth | 3:4f215646a42b | 393 | mut1.unlock(); |
| juliusbernth | 3:4f215646a42b | 394 | |
| juliusbernth | 0:64442857169c | 395 | int count = 0; |
| juliusbernth | 0:64442857169c | 396 | |
| juliusbernth | 0:64442857169c | 397 | while(!pinButton.read()) |
| juliusbernth | 0:64442857169c | 398 | { |
| juliusbernth | 0:64442857169c | 399 | count++; |
| juliusbernth | 0:64442857169c | 400 | if (count ==1){ |
| juliusbernth | 0:64442857169c | 401 | printf("button pressed\r\n"); |
| juliusbernth | 0:64442857169c | 402 | } |
| juliusbernth | 0:64442857169c | 403 | ThisThread::sleep_for(10); |
| juliusbernth | 0:64442857169c | 404 | } |
| juliusbernth | 3:4f215646a42b | 405 | switch (localState) |
| juliusbernth | 0:64442857169c | 406 | { |
| juliusbernth | 0:64442857169c | 407 | case STATE_READY: |
| juliusbernth | 0:64442857169c | 408 | if(count > countThreashold) |
| juliusbernth | 0:64442857169c | 409 | { |
| juliusbernth | 0:64442857169c | 410 | printf("button released count = %d\r\n",count); |
| juliusbernth | 0:64442857169c | 411 | count = 0; |
| juliusbernth | 0:64442857169c | 412 | //CentrifugeTestThread.start(CentrifugeTest); |
| juliusbernth | 3:4f215646a42b | 413 | CentrifugeTestInit(); |
| juliusbernth | 0:64442857169c | 414 | tickerMotorControl.attach_us(&MotorControlRelease, SAMPLE_TIME_US);//set up signal for control frequency |
| juliusbernth | 0:64442857169c | 415 | tickerPrint.attach(&PrintRelease,PRINT_TIME_S); |
| juliusbernth | 0:64442857169c | 416 | } |
| juliusbernth | 0:64442857169c | 417 | break; |
| juliusbernth | 0:64442857169c | 418 | case STATE_RUNNING: |
| juliusbernth | 0:64442857169c | 419 | if(count >1){ |
| juliusbernth | 3:4f215646a42b | 420 | //EN_FAULTA.write(0); |
| juliusbernth | 3:4f215646a42b | 421 | mut1.lock(); |
| juliusbernth | 3:4f215646a42b | 422 | _state = STATE_HALT; |
| juliusbernth | 3:4f215646a42b | 423 | mut1.unlock(); |
| juliusbernth | 0:64442857169c | 424 | } |
| juliusbernth | 0:64442857169c | 425 | break; |
| juliusbernth | 0:64442857169c | 426 | |
| juliusbernth | 0:64442857169c | 427 | case STATE_ERROR: |
| juliusbernth | 0:64442857169c | 428 | |
| juliusbernth | 0:64442857169c | 429 | break; |
| juliusbernth | 0:64442857169c | 430 | default: |
| juliusbernth | 0:64442857169c | 431 | break; |
| juliusbernth | 0:64442857169c | 432 | } |
| juliusbernth | 0:64442857169c | 433 | count = 0; |
| juliusbernth | 3:4f215646a42b | 434 | //ThisThread::sleep_for(100); |
| juliusbernth | 0:64442857169c | 435 | } |
| juliusbernth | 0:64442857169c | 436 | } |
| juliusbernth | 0:64442857169c | 437 | |
| juliusbernth | 0:64442857169c | 438 | int main(){ |
| juliusbernth | 0:64442857169c | 439 | printf("\r\nSystem Online\r\n"); |
| juliusbernth | 0:64442857169c | 440 | //set up system |
| juliusbernth | 0:64442857169c | 441 | |
| juliusbernth | 0:64442857169c | 442 | //set up motor |
| juliusbernth | 0:64442857169c | 443 | //EN_FAULTA.mode(PullDown); |
| juliusbernth | 0:64442857169c | 444 | //STBY.mode(PullDown); |
| juliusbernth | 0:64442857169c | 445 | |
| juliusbernth | 0:64442857169c | 446 | REF_PWM_A.period_us(100); |
| juliusbernth | 0:64442857169c | 447 | PWMA.period_us(100); |
| juliusbernth | 0:64442857169c | 448 | REF_PWM_A.write(0.5);//set current reference |
| juliusbernth | 0:64442857169c | 449 | |
| juliusbernth | 0:64442857169c | 450 | EN_FAULTA.write(0); |
| juliusbernth | 0:64442857169c | 451 | STBY.write(1); |
| juliusbernth | 0:64442857169c | 452 | //calculate filter constant |
| juliusbernth | 0:64442857169c | 453 | speedFilterConstant = 2*3.141593*SAMPLE_TIME_US/1000000*FILTER_CUTOFF_FREQ; |
| juliusbernth | 0:64442857169c | 454 | speedFilterConstant /= (1 + speedFilterConstant); |
| juliusbernth | 0:64442857169c | 455 | |
| juliusbernth | 0:64442857169c | 456 | SPIN_T[0] = 0.0; |
| juliusbernth | 0:64442857169c | 457 | SPIN_T[1] = T_WARMUP; |
| juliusbernth | 0:64442857169c | 458 | SPIN_T[2] = T_WARMUP + T_RAMP; |
| juliusbernth | 0:64442857169c | 459 | SPIN_T[3] = T_WARMUP + T_RAMP + T_TEST; |
| juliusbernth | 0:64442857169c | 460 | SPIN_T[4] = T_WARMUP + T_RAMP + T_TEST + T_RAMP; |
| juliusbernth | 0:64442857169c | 461 | SPIN_T[5] = T_WARMUP + T_RAMP + T_TEST + T_RAMP + T_WARMUP; |
| juliusbernth | 0:64442857169c | 462 | |
| juliusbernth | 0:64442857169c | 463 | /* //set up button interrupts |
| juliusbernth | 0:64442857169c | 464 | pinButton.rise(ISR_Button_Rise); |
| juliusbernth | 0:64442857169c | 465 | pinButton.fall(ISR_Button_Fall); |
| juliusbernth | 0:64442857169c | 466 | */ |
| juliusbernth | 4:d04afc466198 | 467 | mut1.lock(); |
| juliusbernth | 4:d04afc466198 | 468 | _state = STATE_READY;//set state to READY |
| juliusbernth | 4:d04afc466198 | 469 | mut1.unlock(); |
| juliusbernth | 0:64442857169c | 470 | //start all threads |
| juliusbernth | 0:64442857169c | 471 | MotorControlThread.start(MotorControl); |
| juliusbernth | 0:64442857169c | 472 | |
| juliusbernth | 3:4f215646a42b | 473 | //CentrifugeTestThread.start(CentrifugeTest); |
| juliusbernth | 0:64442857169c | 474 | //start print thread. |
| juliusbernth | 0:64442857169c | 475 | |
| juliusbernth | 0:64442857169c | 476 | ReadButtonThread.start(ReadButton); |
| juliusbernth | 0:64442857169c | 477 | tickerReleaseButton.attach(ReleaseReadButton,BUTTON_READ_SAMPLETIME_S); |
| juliusbernth | 0:64442857169c | 478 | |
| juliusbernth | 0:64442857169c | 479 | |
| juliusbernth | 0:64442857169c | 480 | printf("\r\nSetup Complete\r\n"); |
| juliusbernth | 0:64442857169c | 481 | while (true) { |
| juliusbernth | 0:64442857169c | 482 | //check state of button |
| juliusbernth | 0:64442857169c | 483 | ThisThread::sleep_for(osWaitForever); |
| juliusbernth | 0:64442857169c | 484 | } |
| juliusbernth | 0:64442857169c | 485 | } |