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@19:a71ef54d9e94, 2021-11-04 (annotated)

Committer:
juliusbernth
Date:
Thu Nov 04 11:29:12 2021 +0000
Revision:
19:a71ef54d9e94
Parent:
18:5e9b288793bb
Child:
21:26a459e4201c
Working with mid test abort logic fixed (now actually exits). Blink frequency when off and in error mode is wrong, though.;

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 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 19:a71ef54d9e94 175 bool isTestComplete = 0;
juliusbernth 0:64442857169c 176 while(1){
juliusbernth 0:64442857169c 177 semMotorControl.wait();//wait for a signal
juliusbernth 12:bc34f264e2f2 178
juliusbernth 17:eb74805e8f9b 179
juliusbernth 12:bc34f264e2f2 180 Tnow = testTimer.read();
juliusbernth 12:bc34f264e2f2 181 //check accel. If problem, change state to ERROR
juliusbernth 12:bc34f264e2f2 182 //check accel here
juliusbernth 12:bc34f264e2f2 183
juliusbernth 12:bc34f264e2f2 184 /*
juliusbernth 12:bc34f264e2f2 185 Ax = 0.2*pinAx.read() + 0.8*Ax - 0.5;
juliusbernth 12:bc34f264e2f2 186 Ay = 0.2*pinAy.read() + 0.8*Ay - 0.5;
juliusbernth 12:bc34f264e2f2 187 Az = 0.2*pinAz.read() + 0.8*Az - 0.5;
juliusbernth 12:bc34f264e2f2 188 */
juliusbernth 12:bc34f264e2f2 189
juliusbernth 12:bc34f264e2f2 190 Ax = pinAx.read() - 0.5;
juliusbernth 12:bc34f264e2f2 191 Ay = pinAy.read() - 0.5;
juliusbernth 12:bc34f264e2f2 192 Az = pinAz.read() - 0.5;
juliusbernth 12:bc34f264e2f2 193
juliusbernth 12:bc34f264e2f2 194
juliusbernth 12:bc34f264e2f2 195 //read accel
juliusbernth 12:bc34f264e2f2 196 Mag = Ax*Ax + Ay*Ay;
juliusbernth 12:bc34f264e2f2 197 Mag = sqrt(Mag);
juliusbernth 12:bc34f264e2f2 198 //Mag *=ACCEL_SCALE;
juliusbernth 12:bc34f264e2f2 199 MagFil = 0.1*Mag + 0.9*MagFil;
juliusbernth 12:bc34f264e2f2 200 _MagFil = MagFil;
juliusbernth 12:bc34f264e2f2 201 if (MagFil > VIBRATION_THRESHOLD){
juliusbernth 12:bc34f264e2f2 202 //mut1.lock();
juliusbernth 15:f86310960aab 203 state = STATE_ERROR;
juliusbernth 17:eb74805e8f9b 204 //printf("Excess vibration detected\r\n");
juliusbernth 12:bc34f264e2f2 205 //mut1.unlock();
juliusbernth 12:bc34f264e2f2 206 }
juliusbernth 12:bc34f264e2f2 207 //int deltaT = encoderTimer.read();//read current time in seconds
juliusbernth 7:e36f61608c10 208 if(state == STATE_RUNNING){//need to check if this is the best condition to look for.
juliusbernth 17:eb74805e8f9b 209
juliusbernth 19:a71ef54d9e94 210
juliusbernth 19:a71ef54d9e94 211 }
juliusbernth 19:a71ef54d9e94 212
juliusbernth 19:a71ef54d9e94 213 switch(state){
juliusbernth 19:a71ef54d9e94 214 case STATE_RUNNING:
juliusbernth 19:a71ef54d9e94 215 //calculate current demand
juliusbernth 19:a71ef54d9e94 216 if ( (SPIN_T[0] <= Tnow) && (Tnow < SPIN_T[1]) ) {//test is in warmup
juliusbernth 19:a71ef54d9e94 217 demandSpeed_RPM = 0.0;
juliusbernth 19:a71ef54d9e94 218 isTestComplete = 0;
juliusbernth 19:a71ef54d9e94 219 //printf("warm up %f\r\n", demandSpeed_RPM);
juliusbernth 19:a71ef54d9e94 220 //deactivate motor?
juliusbernth 19:a71ef54d9e94 221 }
juliusbernth 19:a71ef54d9e94 222 if ((SPIN_T[1] <= Tnow)&&(Tnow < SPIN_T[2])){//test is in ramp up
juliusbernth 19:a71ef54d9e94 223
juliusbernth 19:a71ef54d9e94 224 double a = Tnow - SPIN_T[1];
juliusbernth 19:a71ef54d9e94 225 demandSpeed_RPM = a * targetSpeed_RPM / T_RAMP;
juliusbernth 19:a71ef54d9e94 226 isTestComplete = 0;
juliusbernth 19:a71ef54d9e94 227 //printf("ramp up %f\r\n", demandSpeed_RPM);
juliusbernth 19:a71ef54d9e94 228 }
juliusbernth 19:a71ef54d9e94 229 if ((SPIN_T[2] <= Tnow)&&(Tnow < SPIN_T[3])){//test is in coast
juliusbernth 19:a71ef54d9e94 230 demandSpeed_RPM = targetSpeed_RPM;
juliusbernth 19:a71ef54d9e94 231 isTestComplete = 0;
juliusbernth 19:a71ef54d9e94 232 //printf("coast %f\r\n", demandSpeed_RPM);
juliusbernth 19:a71ef54d9e94 233 }
juliusbernth 19:a71ef54d9e94 234 if ((SPIN_T[3] <= Tnow)&&(Tnow < SPIN_T[4])){//test is in ramp down
juliusbernth 19:a71ef54d9e94 235 double a = Tnow - SPIN_T[3];
juliusbernth 19:a71ef54d9e94 236 demandSpeed_RPM = targetSpeed_RPM - (a/T_RAMP*targetSpeed_RPM);
juliusbernth 19:a71ef54d9e94 237 isTestComplete = 0;
juliusbernth 19:a71ef54d9e94 238 //printf("ramp down %f\r\n", demandSpeed_RPM);
juliusbernth 19:a71ef54d9e94 239 }
juliusbernth 19:a71ef54d9e94 240 if ((SPIN_T[4] <= Tnow)&&(Tnow < SPIN_T[5])){//test is in cooldown
juliusbernth 19:a71ef54d9e94 241 demandSpeed_RPM = 0.0;
juliusbernth 19:a71ef54d9e94 242 isTestComplete = 1;
juliusbernth 19:a71ef54d9e94 243 //printf("cool down %f\r\n", demandSpeed_RPM);
juliusbernth 19:a71ef54d9e94 244 //deactivate motor?
juliusbernth 19:a71ef54d9e94 245 }
juliusbernth 19:a71ef54d9e94 246 break;
juliusbernth 19:a71ef54d9e94 247 case STATE_ERROR:
juliusbernth 19:a71ef54d9e94 248 demandSpeed_RPM -=66.7*SAMPLE_TIME_US/1000000;
juliusbernth 19:a71ef54d9e94 249 if(demandSpeed_RPM < 0.0){
juliusbernth 19:a71ef54d9e94 250 isTestComplete = 1;
juliusbernth 19:a71ef54d9e94 251 } else {
juliusbernth 19:a71ef54d9e94 252 isTestComplete = 0;
juliusbernth 19:a71ef54d9e94 253 }
juliusbernth 19:a71ef54d9e94 254 break;
juliusbernth 19:a71ef54d9e94 255 case STATE_READY:
juliusbernth 0:64442857169c 256 demandSpeed_RPM = 0.0;
juliusbernth 19:a71ef54d9e94 257 break;
juliusbernth 19:a71ef54d9e94 258 default:
juliusbernth 5:e9bb800a3742 259 demandSpeed_RPM = 0.0;
juliusbernth 19:a71ef54d9e94 260 break;
juliusbernth 18:5e9b288793bb 261 }
juliusbernth 18:5e9b288793bb 262
juliusbernth 18:5e9b288793bb 263 demandSpeed_RPM = LimitDouble(demandSpeed_RPM,0.0,MAX_SPEED_RPM);//limit demand
juliusbernth 18:5e9b288793bb 264 currentPulses = encoder.getPulses();//calculate current speed
juliusbernth 18:5e9b288793bb 265 double deltaPulses = currentPulses - lastPulses;
juliusbernth 18:5e9b288793bb 266 double speed_RPM = deltaPulses / SAMPLE_TIME_US * 1000000;//get in pulses per second
juliusbernth 18:5e9b288793bb 267 speed_RPM *= 60/4/PULSES_PER_REV;//convert to RPM
juliusbernth 18:5e9b288793bb 268
juliusbernth 18:5e9b288793bb 269 currentSpeedRPM = 0.1*speed_RPM + 0.9*lastSpeedRPM;//filter speed
juliusbernth 12:bc34f264e2f2 270
juliusbernth 18:5e9b288793bb 271 double error = demandSpeed_RPM - currentSpeedRPM;//calculate error
juliusbernth 18:5e9b288793bb 272 deltaError = error - lastError;
juliusbernth 18:5e9b288793bb 273 double errorDot = deltaError/SAMPLE_TIME_US*1000000.0;
juliusbernth 18:5e9b288793bb 274 errorDot = 0.1*errorDot + 0.9*lastErrorDot;
juliusbernth 18:5e9b288793bb 275 integralTerm += error*Ki*SAMPLE_TIME_US/1000000.0;
juliusbernth 18:5e9b288793bb 276 integralTerm = LimitDouble(integralTerm,-0.8, 0.8);
juliusbernth 5:e9bb800a3742 277
juliusbernth 18:5e9b288793bb 278 output = Kp * error; //calculate output
juliusbernth 18:5e9b288793bb 279 output += integralTerm;
juliusbernth 18:5e9b288793bb 280 output += Kd*errorDot;
juliusbernth 18:5e9b288793bb 281 output += Ko*demandSpeed_RPM;
juliusbernth 18:5e9b288793bb 282 output = LimitDouble(output,-1.0,1.0);
juliusbernth 18:5e9b288793bb 283 //printf("wd:%f\t w:%f\t e:%f\t o:%f \r\n",demandSpeed_RPM, currentSpeedRPM, error, output);
juliusbernth 18:5e9b288793bb 284
juliusbernth 18:5e9b288793bb 285 if(output >=0){//Set direction
juliusbernth 18:5e9b288793bb 286 PHA.write(0);
juliusbernth 18:5e9b288793bb 287 }else{
juliusbernth 18:5e9b288793bb 288 PHA.write(1);
juliusbernth 18:5e9b288793bb 289 output = -1*output;
juliusbernth 18:5e9b288793bb 290 }
juliusbernth 19:a71ef54d9e94 291
juliusbernth 18:5e9b288793bb 292 PWMA.write(output);//write to motor
juliusbernth 18:5e9b288793bb 293 lastPulses = currentPulses;//update
juliusbernth 18:5e9b288793bb 294 lastError = error;
juliusbernth 18:5e9b288793bb 295 lastSpeedRPM = currentSpeedRPM;
juliusbernth 18:5e9b288793bb 296 lastErrorDot = errorDot;
juliusbernth 18:5e9b288793bb 297
juliusbernth 18:5e9b288793bb 298 //exit when test has completed
juliusbernth 19:a71ef54d9e94 299 if(isTestComplete){
juliusbernth 18:5e9b288793bb 300 //printf("Terminating Test %f \t %f\r\n", Tnow, SPIN_T[5]);
juliusbernth 19:a71ef54d9e94 301 if(state = STATE_ERROR){
juliusbernth 19:a71ef54d9e94 302 state = STATE_ERROR;//change state
juliusbernth 19:a71ef54d9e94 303 }
juliusbernth 19:a71ef54d9e94 304 else {
juliusbernth 19:a71ef54d9e94 305 state = STATE_READY;
juliusbernth 19:a71ef54d9e94 306 }
juliusbernth 18:5e9b288793bb 307 testTimer.stop(); //stop and reset timers
juliusbernth 18:5e9b288793bb 308 testTimer.reset();
juliusbernth 18:5e9b288793bb 309 Tnow = testTimer.read();
juliusbernth 18:5e9b288793bb 310 encoderTimer.stop();
juliusbernth 18:5e9b288793bb 311 encoderTimer.reset();
juliusbernth 18:5e9b288793bb 312 PWMA.write(0.0);
juliusbernth 5:e9bb800a3742 313
juliusbernth 18:5e9b288793bb 314 //add check if motor has stopped
juliusbernth 18:5e9b288793bb 315 //printf("Wating for motor to stop.\r\n");
juliusbernth 18:5e9b288793bb 316 while(isSpinning()){
juliusbernth 18:5e9b288793bb 317 ThisThread::sleep_for(200);
juliusbernth 7:e36f61608c10 318 }
juliusbernth 18:5e9b288793bb 319 //EN_FAULTA.write(0);//disable motor
juliusbernth 18:5e9b288793bb 320 //EN_FAULTB.write(0);
juliusbernth 18:5e9b288793bb 321
juliusbernth 18:5e9b288793bb 322 tickerMotorControl.detach(); //detach the semaphore release for motor control
juliusbernth 18:5e9b288793bb 323 tickerPrint.detach();
juliusbernth 18:5e9b288793bb 324
juliusbernth 18:5e9b288793bb 325 //printf("state = %d\r\n",state);
juliusbernth 18:5e9b288793bb 326 pinLedRed = 0;
juliusbernth 18:5e9b288793bb 327 TestCompleteNotification();//send notification
juliusbernth 18:5e9b288793bb 328 lastErrorDot = 0.0;
juliusbernth 18:5e9b288793bb 329 lastError = 0.0;
juliusbernth 18:5e9b288793bb 330 integralTerm = 0.0;
juliusbernth 18:5e9b288793bb 331 //deactivate motor
juliusbernth 18:5e9b288793bb 332 //PrintThread.terminate();//terminate print thread
juliusbernth 18:5e9b288793bb 333 //CentrifugeTestThread.terminate();//terminate threads
juliusbernth 18:5e9b288793bb 334 }
juliusbernth 9:b1f53e4eb453 335
juliusbernth 12:bc34f264e2f2 336 //} //end running conditional
juliusbernth 12:bc34f264e2f2 337
juliusbernth 12:bc34f264e2f2 338 }//end while
juliusbernth 0:64442857169c 339 }
juliusbernth 0:64442857169c 340
juliusbernth 5:e9bb800a3742 341 void CentrifugeTest(){
juliusbernth 5:e9bb800a3742 342 while(1){
juliusbernth 5:e9bb800a3742 343 semStartTest.wait();
juliusbernth 17:eb74805e8f9b 344 //printf("\r\n Test starting \r\n");
juliusbernth 5:e9bb800a3742 345 state = STATE_RUNNING;
juliusbernth 0:64442857169c 346 //set up test
juliusbernth 0:64442857169c 347 testTimer.reset();
juliusbernth 0:64442857169c 348 testTimer.start();//start timer
juliusbernth 0:64442857169c 349
juliusbernth 5:e9bb800a3742 350 char spinState;
juliusbernth 8:41cd1fc8cbbe 351 pinLedRed = 1;
juliusbernth 0:64442857169c 352 //set up ticker to allow motor control thread to run periodically
juliusbernth 0:64442857169c 353 encoder.reset();//reset encoder
juliusbernth 0:64442857169c 354 lastPulses = 0;//reset previous encoder reading
juliusbernth 5:e9bb800a3742 355 encoderTimer.start();
juliusbernth 5:e9bb800a3742 356 PrintThread.start(PrintTimeRemaining);
juliusbernth 17:eb74805e8f9b 357 //printf("\r\n Test setup complete, State:%d \r\n", state);
juliusbernth 18:5e9b288793bb 358 EN_FAULTA.write(1);//enable motor
juliusbernth 13:32e1ae4221f7 359
juliusbernth 9:b1f53e4eb453 360 //EN_FAULTB.write(1);
juliusbernth 7:e36f61608c10 361 if(state == STATE_RUNNING){
juliusbernth 9:b1f53e4eb453 362 //printf("\r\n running %d\r\n",state);
juliusbernth 7:e36f61608c10 363 }
juliusbernth 0:64442857169c 364
juliusbernth 5:e9bb800a3742 365
juliusbernth 5:e9bb800a3742 366 }
juliusbernth 0:64442857169c 367 }
juliusbernth 0:64442857169c 368
juliusbernth 0:64442857169c 369 void ReleaseReadButton(){
juliusbernth 0:64442857169c 370 semButton.release();
juliusbernth 0:64442857169c 371 }
juliusbernth 0:64442857169c 372
juliusbernth 16:cefb22c49c2f 373
juliusbernth 13:32e1ae4221f7 374
juliusbernth 0:64442857169c 375 void ReadButton()
juliusbernth 0:64442857169c 376 {
juliusbernth 0:64442857169c 377 int countThreashold = int(BUTTON_HOLD_TIME_S/BUTTON_READ_SAMPLETIME_S);
juliusbernth 12:bc34f264e2f2 378 int errorBlinkCount;
juliusbernth 0:64442857169c 379 while (1)
juliusbernth 0:64442857169c 380 {
juliusbernth 0:64442857169c 381 semButton.wait();
juliusbernth 12:bc34f264e2f2 382
juliusbernth 12:bc34f264e2f2 383 if(state == STATE_ERROR){
juliusbernth 12:bc34f264e2f2 384
juliusbernth 12:bc34f264e2f2 385 }
juliusbernth 12:bc34f264e2f2 386
juliusbernth 0:64442857169c 387 int count = 0;
juliusbernth 9:b1f53e4eb453 388 int blinkCount = 0;
juliusbernth 14:81d390496d4e 389 if(state == STATE_READY){
juliusbernth 17:eb74805e8f9b 390 //pinLedRed = 0;
juliusbernth 14:81d390496d4e 391 }
juliusbernth 8:41cd1fc8cbbe 392 while(pinButton.read())
juliusbernth 0:64442857169c 393 {
juliusbernth 0:64442857169c 394 count++;
juliusbernth 9:b1f53e4eb453 395 if(count < countThreashold){
juliusbernth 9:b1f53e4eb453 396 blinkCount++;
juliusbernth 9:b1f53e4eb453 397 if(blinkCount>=10){
juliusbernth 9:b1f53e4eb453 398 blinkCount = 0;
juliusbernth 9:b1f53e4eb453 399 pinLedRed = !pinLedRed;
juliusbernth 9:b1f53e4eb453 400 if(count == 1){
juliusbernth 17:eb74805e8f9b 401 pinLedRed = 1;
juliusbernth 9:b1f53e4eb453 402 }
juliusbernth 9:b1f53e4eb453 403 }
juliusbernth 9:b1f53e4eb453 404 }
juliusbernth 9:b1f53e4eb453 405 else{
juliusbernth 9:b1f53e4eb453 406 pinLedRed = 1;
juliusbernth 9:b1f53e4eb453 407 }
juliusbernth 0:64442857169c 408 if (count ==1){
juliusbernth 11:62d2a592b1ae 409 if(state == STATE_READY){
juliusbernth 17:eb74805e8f9b 410 //printf("Button pressed. Hold for %f s\r\n",BUTTON_HOLD_TIME_S);
juliusbernth 11:62d2a592b1ae 411 }
juliusbernth 11:62d2a592b1ae 412 else if(state == STATE_RUNNING){
juliusbernth 17:eb74805e8f9b 413 //printf("Test terminated by user. Slowing to stop. Please restart system when safe.\r\n");
juliusbernth 11:62d2a592b1ae 414 state = STATE_ERROR;
juliusbernth 11:62d2a592b1ae 415 }
juliusbernth 0:64442857169c 416 }
juliusbernth 0:64442857169c 417 ThisThread::sleep_for(10);
juliusbernth 0:64442857169c 418 }
juliusbernth 7:e36f61608c10 419 switch (state)
juliusbernth 0:64442857169c 420 {
juliusbernth 0:64442857169c 421 case STATE_READY:
juliusbernth 0:64442857169c 422 if(count > countThreashold)
juliusbernth 0:64442857169c 423 {
juliusbernth 9:b1f53e4eb453 424 pinLedRed = 1;
juliusbernth 9:b1f53e4eb453 425 //printf("button released count = %d\r\n",count);
juliusbernth 0:64442857169c 426 count = 0;
juliusbernth 0:64442857169c 427 //CentrifugeTestThread.start(CentrifugeTest);
juliusbernth 13:32e1ae4221f7 428 if(!isSpinning()){
juliusbernth 13:32e1ae4221f7 429 semStartTest.release();
juliusbernth 13:32e1ae4221f7 430 tickerMotorControl.attach_us(&MotorControlRelease, SAMPLE_TIME_US);//set up signal for control frequency
juliusbernth 13:32e1ae4221f7 431 tickerPrint.attach(&PrintRelease,PRINT_TIME_S);
juliusbernth 13:32e1ae4221f7 432 }else{
juliusbernth 17:eb74805e8f9b 433 //printf("Holder still spinning. Wait until it has stopped.\r\n");
juliusbernth 13:32e1ae4221f7 434 }
juliusbernth 9:b1f53e4eb453 435 }else{
juliusbernth 9:b1f53e4eb453 436 pinLedRed = 0;
juliusbernth 0:64442857169c 437 }
juliusbernth 0:64442857169c 438 break;
juliusbernth 0:64442857169c 439 case STATE_RUNNING:
juliusbernth 0:64442857169c 440 if(count >1){
juliusbernth 11:62d2a592b1ae 441 //EN_FAULTA.write(0);
juliusbernth 11:62d2a592b1ae 442 // EN_FAULTB.write(0);
juliusbernth 9:b1f53e4eb453 443 state = STATE_ERROR;
juliusbernth 9:b1f53e4eb453 444 //EN_FAULTB.write(0);
juliusbernth 0:64442857169c 445 }
juliusbernth 0:64442857169c 446 break;
juliusbernth 0:64442857169c 447
juliusbernth 0:64442857169c 448 case STATE_ERROR:
juliusbernth 9:b1f53e4eb453 449 //printf("Please restart the system. \r\n");
juliusbernth 17:eb74805e8f9b 450 pinLedRed = !pinLedRed;
juliusbernth 17:eb74805e8f9b 451 ThisThread::sleep_for(400);
juliusbernth 17:eb74805e8f9b 452 // errorBlinkCount++;
juliusbernth 17:eb74805e8f9b 453 // if(errorBlinkCount >=50){
juliusbernth 17:eb74805e8f9b 454 // errorBlinkCount = 0;
juliusbernth 17:eb74805e8f9b 455 //
juliusbernth 17:eb74805e8f9b 456 // }
juliusbernth 0:64442857169c 457 break;
juliusbernth 0:64442857169c 458 default:
juliusbernth 0:64442857169c 459 break;
juliusbernth 0:64442857169c 460 }
juliusbernth 0:64442857169c 461 count = 0;
juliusbernth 5:e9bb800a3742 462 ThisThread::sleep_for(100);
juliusbernth 0:64442857169c 463 }
juliusbernth 0:64442857169c 464 }
juliusbernth 0:64442857169c 465
juliusbernth 0:64442857169c 466 int main(){
juliusbernth 18:5e9b288793bb 467 printf("\r\nSystem Online\r\n");
juliusbernth 0:64442857169c 468 //set up system
juliusbernth 0:64442857169c 469
juliusbernth 0:64442857169c 470 //set up motor
juliusbernth 0:64442857169c 471 //EN_FAULTA.mode(PullDown);
juliusbernth 0:64442857169c 472 //STBY.mode(PullDown);
juliusbernth 0:64442857169c 473
juliusbernth 0:64442857169c 474 REF_PWM_A.period_us(100);
juliusbernth 9:b1f53e4eb453 475 PWMA.period_us(33);
juliusbernth 17:eb74805e8f9b 476 REF_PWM_A.write(0.05);//set current reference
juliusbernth 9:b1f53e4eb453 477
juliusbernth 9:b1f53e4eb453 478 REF_PWMB.period_us(100);
juliusbernth 9:b1f53e4eb453 479 PWMB.period_us(33);
juliusbernth 9:b1f53e4eb453 480 REF_PWMB.write(0.0);
juliusbernth 9:b1f53e4eb453 481 PHB.write(0);
juliusbernth 9:b1f53e4eb453 482 EN_FAULTB.write(0);
juliusbernth 0:64442857169c 483
juliusbernth 16:cefb22c49c2f 484 EN_FAULTA.write(1);//remove incase of restart?
juliusbernth 0:64442857169c 485 STBY.write(1);
juliusbernth 0:64442857169c 486 //calculate filter constant
juliusbernth 0:64442857169c 487 speedFilterConstant = 2*3.141593*SAMPLE_TIME_US/1000000*FILTER_CUTOFF_FREQ;
juliusbernth 0:64442857169c 488 speedFilterConstant /= (1 + speedFilterConstant);
juliusbernth 0:64442857169c 489
juliusbernth 0:64442857169c 490 SPIN_T[0] = 0.0;
juliusbernth 0:64442857169c 491 SPIN_T[1] = T_WARMUP;
juliusbernth 0:64442857169c 492 SPIN_T[2] = T_WARMUP + T_RAMP;
juliusbernth 0:64442857169c 493 SPIN_T[3] = T_WARMUP + T_RAMP + T_TEST;
juliusbernth 0:64442857169c 494 SPIN_T[4] = T_WARMUP + T_RAMP + T_TEST + T_RAMP;
juliusbernth 0:64442857169c 495 SPIN_T[5] = T_WARMUP + T_RAMP + T_TEST + T_RAMP + T_WARMUP;
juliusbernth 0:64442857169c 496
juliusbernth 0:64442857169c 497 /* //set up button interrupts
juliusbernth 0:64442857169c 498 pinButton.rise(ISR_Button_Rise);
juliusbernth 0:64442857169c 499 pinButton.fall(ISR_Button_Fall);
juliusbernth 0:64442857169c 500 */
juliusbernth 8:41cd1fc8cbbe 501 pinButton.mode(PullDown);
juliusbernth 8:41cd1fc8cbbe 502 pinLedWhite = 1;
juliusbernth 8:41cd1fc8cbbe 503
juliusbernth 8:41cd1fc8cbbe 504
juliusbernth 5:e9bb800a3742 505
juliusbernth 5:e9bb800a3742 506 state = STATE_READY;//set state to READY
juliusbernth 0:64442857169c 507 //start all threads
juliusbernth 0:64442857169c 508 MotorControlThread.start(MotorControl);
juliusbernth 0:64442857169c 509
juliusbernth 5:e9bb800a3742 510 CentrifugeTestThread.start(CentrifugeTest);
juliusbernth 0:64442857169c 511 //start print thread.
juliusbernth 0:64442857169c 512
juliusbernth 0:64442857169c 513 ReadButtonThread.start(ReadButton);
juliusbernth 0:64442857169c 514 tickerReleaseButton.attach(ReleaseReadButton,BUTTON_READ_SAMPLETIME_S);
juliusbernth 0:64442857169c 515
juliusbernth 0:64442857169c 516
juliusbernth 17:eb74805e8f9b 517 //printf("\r\nSetup Complete\r\n");
juliusbernth 0:64442857169c 518 while (true) {
juliusbernth 0:64442857169c 519 //check state of button
juliusbernth 0:64442857169c 520 ThisThread::sleep_for(osWaitForever);
juliusbernth 0:64442857169c 521 }
juliusbernth 0:64442857169c 522 }