Julius Bernth / Mbed OS Cuvette_Centrifuge_Test_v1-2_ParallelMode

Dependencies:   QEI LIS3DH_spi

Committer:
juliusbernth
Date:
Thu Aug 12 15:15:32 2021 +0000
Revision:
11:62d2a592b1ae
Parent:
10:8a41143a9f52
Child:
12:bc34f264e2f2
Working - issue with breaking

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