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@16:cefb22c49c2f, 2021-08-12 (annotated)

Committer:
juliusbernth
Date:
Thu Aug 12 17:39:38 2021 +0000
Revision:
16:cefb22c49c2f
Parent:
15:f86310960aab
Child:
17:eb74805e8f9b
Working but fried MCU during last test.

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