Julius Bernth / Mbed OS 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.

Dependencies:   QEI LIS3DH_spi

main.cpp@21:26a459e4201c, 2021-11-12 (annotated)

Committer:
juliusbernth
Date:
Fri Nov 12 15:05:16 2021 +0000
Revision:
21:26a459e4201c
Parent:
19:a71ef54d9e94 
Tested for parallel mode and multi-cuvette holder

Who changed what in which revision?

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