Julius Bernth / Mbed OS Cuvette_Centrifuge_Test_v1-2_ParallelMode

Dependencies:   QEI LIS3DH_spi

Committer:
juliusbernth
Date:
Tue Aug 10 15:52:39 2021 +0000
Revision:
8:41cd1fc8cbbe
Parent:
7:e36f61608c10
Child:
9:b1f53e4eb453
First fully integrated working version

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 0:64442857169c 40 DigitalOut PHB (D7);
juliusbernth 0:64442857169c 41 PwmOut PWMA (D5);
juliusbernth 0:64442857169c 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 7:e36f61608c10 49 DigitalOut pinSt(A3);
juliusbernth 0:64442857169c 50
juliusbernth 0:64442857169c 51 Serial pc(USBTX,USBRX);
juliusbernth 0:64442857169c 52
juliusbernth 0:64442857169c 53 DigitalOut pinStatusLED (D13);
juliusbernth 8:41cd1fc8cbbe 54
juliusbernth 8:41cd1fc8cbbe 55 DigitalIn pinButton (PC_8);
juliusbernth 8:41cd1fc8cbbe 56 DigitalOut pinLedWhite (PC_6);
juliusbernth 8:41cd1fc8cbbe 57 DigitalOut pinLedRed (PC_5);
juliusbernth 0:64442857169c 58
juliusbernth 0:64442857169c 59 /* Still to allocate
juliusbernth 0:64442857169c 60
juliusbernth 0:64442857169c 61 DigitalOut pinBuzzer
juliusbernth 0:64442857169c 62 DigitalOut pinPowerLED
juliusbernth 0:64442857169c 63
juliusbernth 0:64442857169c 64 */
juliusbernth 0:64442857169c 65
juliusbernth 0:64442857169c 66 //global variables
juliusbernth 0:64442857169c 67 double speedFilterConstant;
juliusbernth 5:e9bb800a3742 68 char state;
juliusbernth 0:64442857169c 69 double currentPulses;
juliusbernth 0:64442857169c 70 double lastPulses;
juliusbernth 0:64442857169c 71 double lastSpeedRPM;
juliusbernth 0:64442857169c 72 double Tnow;
juliusbernth 5:e9bb800a3742 73 double demandSpeed_RPM = 0.0;
juliusbernth 5:e9bb800a3742 74 double currentSpeedRPM;
juliusbernth 0:64442857169c 75 double deltaError;
juliusbernth 0:64442857169c 76 double lastError;
juliusbernth 0:64442857169c 77 double integralTerm;
juliusbernth 0:64442857169c 78 double output;
juliusbernth 0:64442857169c 79
juliusbernth 0:64442857169c 80 Ticker tickerMotorControl;
juliusbernth 0:64442857169c 81 Ticker tickerPrint;
juliusbernth 0:64442857169c 82 Ticker tickerReleaseButton;
juliusbernth 0:64442857169c 83
juliusbernth 0:64442857169c 84 Semaphore semMotorControl(0);
juliusbernth 0:64442857169c 85 Semaphore semPrint(0);
juliusbernth 0:64442857169c 86 Semaphore semStartTest(0);
juliusbernth 0:64442857169c 87 Semaphore semButton(0);
juliusbernth 0:64442857169c 88
juliusbernth 3:4f215646a42b 89 Mutex mut1;
juliusbernth 0:64442857169c 90
juliusbernth 0:64442857169c 91 double LimitDouble(double input, double min, double max){
juliusbernth 0:64442857169c 92 double output;
juliusbernth 0:64442857169c 93 output = input;
juliusbernth 0:64442857169c 94 if (input > max){
juliusbernth 0:64442857169c 95 output = max;
juliusbernth 0:64442857169c 96 }
juliusbernth 0:64442857169c 97 if (input < min){
juliusbernth 0:64442857169c 98 output = min;
juliusbernth 0:64442857169c 99 }
juliusbernth 0:64442857169c 100 return output;
juliusbernth 0:64442857169c 101 }
juliusbernth 0:64442857169c 102 /*
juliusbernth 0:64442857169c 103 double GetSpeed(int pulses, int lastPulses, double currentSpeed){
juliusbernth 0:64442857169c 104 double filteredSpeedRPM;
juliusbernth 0:64442857169c 105
juliusbernth 0:64442857169c 106 //find change in pulses.
juliusbernth 0:64442857169c 107 double speedRPM = (double) (pulses - lastPulses); //get change in angle
juliusbernth 0:64442857169c 108 speedRPM /= encoderTimer.read_us(); //calculate speed in pulses/us
juliusbernth 0:64442857169c 109 encoderTimer.reset();
juliusbernth 0:64442857169c 110 speedRPM *= 60000000.0/PULSES_PER_REV/4.0;//convert to RPM;
juliusbernth 0:64442857169c 111
juliusbernth 0:64442857169c 112 //filter speed
juliusbernth 0:64442857169c 113 //filteredSpeedRPM = speedFilterConstant * speedRPM + currentSpeed - speedFilterConstant * currentSpeed;
juliusbernth 0:64442857169c 114 filteredSpeedRPM = speedRPM;
juliusbernth 0:64442857169c 115 return filteredSpeedRPM;
juliusbernth 0:64442857169c 116 }*/
juliusbernth 0:64442857169c 117
juliusbernth 0:64442857169c 118
juliusbernth 0:64442857169c 119 void PIDControl(){
juliusbernth 0:64442857169c 120
juliusbernth 0:64442857169c 121 }
juliusbernth 0:64442857169c 122
juliusbernth 0:64442857169c 123 void TestCompleteNotification(){
juliusbernth 0:64442857169c 124 //send message
juliusbernth 0:64442857169c 125 printf("\r\nTest complete\r\n");
juliusbernth 0:64442857169c 126 //sound buzzer and blink 3 times
juliusbernth 0:64442857169c 127 }
juliusbernth 0:64442857169c 128
juliusbernth 0:64442857169c 129 void MotorControlRelease(){
juliusbernth 0:64442857169c 130 semMotorControl.release();
juliusbernth 0:64442857169c 131 }
juliusbernth 0:64442857169c 132 void PrintRelease(){
juliusbernth 0:64442857169c 133 semPrint.release();
juliusbernth 0:64442857169c 134 }
juliusbernth 0:64442857169c 135
juliusbernth 5:e9bb800a3742 136 void PrintTimeRemaining(){
juliusbernth 0:64442857169c 137 while(1){
juliusbernth 0:64442857169c 138 semPrint.wait();
juliusbernth 5:e9bb800a3742 139 if(state == STATE_RUNNING){
juliusbernth 0:64442857169c 140 double timeElapsed = testTimer.read();
juliusbernth 0:64442857169c 141 double timeRemaining = SPIN_T[5] - timeElapsed;
juliusbernth 0:64442857169c 142 int displayTime = int(timeRemaining)+1;
juliusbernth 0:64442857169c 143 //printf("Time remaining %d s\r\n", displayTime);
juliusbernth 7:e36f61608c10 144 //mut1.lock();
juliusbernth 5:e9bb800a3742 145 double printDemandSpeed = demandSpeed_RPM;
juliusbernth 5:e9bb800a3742 146 double printCurrentSpeed = currentSpeedRPM;
juliusbernth 7:e36f61608c10 147 //mut1.unlock();
juliusbernth 7:e36f61608c10 148 //printf("%f\t%f\r\n",demandSpeed_RPM, currentSpeedRPM);
juliusbernth 3:4f215646a42b 149 }
juliusbernth 3:4f215646a42b 150 }
juliusbernth 3:4f215646a42b 151 }
juliusbernth 3:4f215646a42b 152
juliusbernth 0:64442857169c 153 void MotorControl(){
juliusbernth 0:64442857169c 154 while(1){
juliusbernth 0:64442857169c 155 semMotorControl.wait();//wait for a signal
juliusbernth 7:e36f61608c10 156 if(state == STATE_RUNNING){//need to check if this is the best condition to look for.
juliusbernth 7:e36f61608c10 157
juliusbernth 5:e9bb800a3742 158 Tnow = testTimer.read();
juliusbernth 7:e36f61608c10 159 //check accel. If problem, change state to ERROR
juliusbernth 7:e36f61608c10 160 //check accel here
juliusbernth 7:e36f61608c10 161 double Ax, Ay, Az, Mag;
juliusbernth 7:e36f61608c10 162 static double MagFil, lastAx, lastAy, lastAz;
juliusbernth 7:e36f61608c10 163 /*
juliusbernth 7:e36f61608c10 164 Ax = 0.2*pinAx.read() + 0.8*Ax - 0.5;
juliusbernth 7:e36f61608c10 165 Ay = 0.2*pinAy.read() + 0.8*Ay - 0.5;
juliusbernth 7:e36f61608c10 166 Az = 0.2*pinAz.read() + 0.8*Az - 0.5;
juliusbernth 7:e36f61608c10 167 */
juliusbernth 7:e36f61608c10 168
juliusbernth 7:e36f61608c10 169 Ax = pinAx.read() - 0.5;
juliusbernth 7:e36f61608c10 170 Ay = pinAy.read() - 0.5;
juliusbernth 7:e36f61608c10 171 Az = pinAz.read() - 0.5;
juliusbernth 7:e36f61608c10 172
juliusbernth 7:e36f61608c10 173
juliusbernth 7:e36f61608c10 174 //read accel
juliusbernth 7:e36f61608c10 175 Mag = Ax*Ax + Ay*Ay;
juliusbernth 7:e36f61608c10 176 Mag = sqrt(Mag);
juliusbernth 7:e36f61608c10 177 //Mag *=ACCEL_SCALE;
juliusbernth 7:e36f61608c10 178 MagFil = 0.3*Mag + 0.7*MagFil;
juliusbernth 7:e36f61608c10 179 if (MagFil > VIBRATION_THRESHOLD){
juliusbernth 7:e36f61608c10 180 //mut1.lock();
juliusbernth 7:e36f61608c10 181 state = STATE_ERROR;
juliusbernth 7:e36f61608c10 182
juliusbernth 7:e36f61608c10 183 //mut1.unlock();
juliusbernth 7:e36f61608c10 184 }
juliusbernth 7:e36f61608c10 185 //int deltaT = encoderTimer.read();//read current time in seconds
juliusbernth 5:e9bb800a3742 186
juliusbernth 0:64442857169c 187 //calculate current demand
juliusbernth 5:e9bb800a3742 188 if ( (SPIN_T[0] <= Tnow) && (Tnow < SPIN_T[1]) ) {//test is in warmup
juliusbernth 0:64442857169c 189 demandSpeed_RPM = 0.0;
juliusbernth 5:e9bb800a3742 190 //printf("warm up %f\r\n", demandSpeed_RPM);
juliusbernth 5:e9bb800a3742 191 //deactivate motor?
juliusbernth 0:64442857169c 192 }
juliusbernth 5:e9bb800a3742 193 if ((SPIN_T[1] <= Tnow)&&(Tnow < SPIN_T[2])){//test is in ramp up
juliusbernth 5:e9bb800a3742 194
juliusbernth 5:e9bb800a3742 195 double a = Tnow - SPIN_T[1];
juliusbernth 5:e9bb800a3742 196 demandSpeed_RPM = a * targetSpeed_RPM / T_RAMP;
juliusbernth 5:e9bb800a3742 197 //printf("ramp up %f\r\n", demandSpeed_RPM);
juliusbernth 5:e9bb800a3742 198 }
juliusbernth 5:e9bb800a3742 199 if ((SPIN_T[2] <= Tnow)&&(Tnow < SPIN_T[3])){//test is in coast
juliusbernth 5:e9bb800a3742 200 demandSpeed_RPM = targetSpeed_RPM;
juliusbernth 5:e9bb800a3742 201 //printf("coast %f\r\n", demandSpeed_RPM);
juliusbernth 4:d04afc466198 202 }
juliusbernth 5:e9bb800a3742 203 if ((SPIN_T[3] <= Tnow)&&(Tnow < SPIN_T[4])){//test is in ramp down
juliusbernth 5:e9bb800a3742 204 double a = Tnow - SPIN_T[3];
juliusbernth 5:e9bb800a3742 205 demandSpeed_RPM = targetSpeed_RPM - (a/T_RAMP*targetSpeed_RPM);
juliusbernth 5:e9bb800a3742 206 //printf("ramp down %f\r\n", demandSpeed_RPM);
juliusbernth 5:e9bb800a3742 207 }
juliusbernth 5:e9bb800a3742 208 if ((SPIN_T[4] <= Tnow)&&(Tnow < SPIN_T[5])){//test is in cooldown
juliusbernth 5:e9bb800a3742 209 demandSpeed_RPM = 0.0;
juliusbernth 5:e9bb800a3742 210 //printf("cool down %f\r\n", demandSpeed_RPM);
juliusbernth 5:e9bb800a3742 211 //deactivate motor?
juliusbernth 4:d04afc466198 212 }
juliusbernth 7:e36f61608c10 213
juliusbernth 7:e36f61608c10 214 demandSpeed_RPM = LimitDouble(demandSpeed_RPM,0.0,MAX_SPEED_RPM);//limit demand
juliusbernth 7:e36f61608c10 215 currentPulses = encoder.getPulses();//calculate current speed
juliusbernth 7:e36f61608c10 216 double deltaPulses = currentPulses - lastPulses;
juliusbernth 7:e36f61608c10 217 double speed_RPM = deltaPulses / SAMPLE_TIME_US * 1000000;//get in pulses per second
juliusbernth 7:e36f61608c10 218 speed_RPM *= 60/4/PULSES_PER_REV;//convert to RPM
juliusbernth 7:e36f61608c10 219
juliusbernth 7:e36f61608c10 220 currentSpeedRPM = 0.3*speed_RPM + 0.7*lastSpeedRPM;//filter speed
juliusbernth 5:e9bb800a3742 221
juliusbernth 7:e36f61608c10 222 double error = demandSpeed_RPM - currentSpeedRPM;//calculate error
juliusbernth 7:e36f61608c10 223 deltaError = error - lastError;
juliusbernth 7:e36f61608c10 224 double errorDot = deltaError/SAMPLE_TIME_US*1000000.0;
juliusbernth 7:e36f61608c10 225 integralTerm += error*Ki*SAMPLE_TIME_US/1000000.0;
juliusbernth 7:e36f61608c10 226 integralTerm = LimitDouble(integralTerm,-0.8, 0.8);
juliusbernth 7:e36f61608c10 227
juliusbernth 7:e36f61608c10 228 output = Kp * error; //calculate output
juliusbernth 7:e36f61608c10 229 output += integralTerm;
juliusbernth 7:e36f61608c10 230 output += Kd*errorDot;
juliusbernth 7:e36f61608c10 231 output = LimitDouble(output,-1.0,1.0);
juliusbernth 7:e36f61608c10 232 //printf("wd:%f\t w:%f\t e:%f\t o:%f \r\n",demandSpeed_RPM, currentSpeedRPM, error, output);
juliusbernth 5:e9bb800a3742 233
juliusbernth 7:e36f61608c10 234 if(output >=0){//Set direction
juliusbernth 7:e36f61608c10 235 PHA.write(0);
juliusbernth 7:e36f61608c10 236 }else{
juliusbernth 7:e36f61608c10 237 PHA.write(1);
juliusbernth 7:e36f61608c10 238 output = -1*output;
juliusbernth 7:e36f61608c10 239 }
juliusbernth 7:e36f61608c10 240 PWMA.write(output);//write to motor
juliusbernth 7:e36f61608c10 241
juliusbernth 7:e36f61608c10 242 lastPulses = currentPulses;//update
juliusbernth 7:e36f61608c10 243 lastError = error;
juliusbernth 7:e36f61608c10 244 lastSpeedRPM = currentSpeedRPM;
juliusbernth 5:e9bb800a3742 245
juliusbernth 7:e36f61608c10 246 //exit when test has completed
juliusbernth 7:e36f61608c10 247 if (Tnow >= SPIN_T[5]){
juliusbernth 7:e36f61608c10 248 printf("Terminating Test %f \t %f\r\n", Tnow, SPIN_T[5]);
juliusbernth 7:e36f61608c10 249 state = STATE_READY;//change state
juliusbernth 7:e36f61608c10 250 testTimer.stop(); //stop and reset timers
juliusbernth 7:e36f61608c10 251 testTimer.reset();
juliusbernth 7:e36f61608c10 252 Tnow = testTimer.read();
juliusbernth 7:e36f61608c10 253 encoderTimer.stop();
juliusbernth 7:e36f61608c10 254 encoderTimer.reset();
juliusbernth 7:e36f61608c10 255 EN_FAULTA.write(0);//disable motor
juliusbernth 7:e36f61608c10 256
juliusbernth 7:e36f61608c10 257 tickerMotorControl.detach(); //detach the semaphore release for motor control
juliusbernth 7:e36f61608c10 258 tickerPrint.detach();
juliusbernth 7:e36f61608c10 259
juliusbernth 7:e36f61608c10 260 printf("state = %d\r\n",state);
juliusbernth 8:41cd1fc8cbbe 261 pinLedRed = 0;
juliusbernth 7:e36f61608c10 262 TestCompleteNotification();//send notification
juliusbernth 7:e36f61608c10 263 //deactivate motor
juliusbernth 7:e36f61608c10 264 //PrintThread.terminate();//terminate print thread
juliusbernth 7:e36f61608c10 265 //CentrifugeTestThread.terminate();//terminate threads
juliusbernth 7:e36f61608c10 266 }
juliusbernth 0:64442857169c 267 }
juliusbernth 0:64442857169c 268 }
juliusbernth 0:64442857169c 269 }
juliusbernth 0:64442857169c 270
juliusbernth 5:e9bb800a3742 271 void CentrifugeTest(){
juliusbernth 5:e9bb800a3742 272 while(1){
juliusbernth 5:e9bb800a3742 273 semStartTest.wait();
juliusbernth 0:64442857169c 274 printf("\r\n Test starting \r\n");
juliusbernth 5:e9bb800a3742 275 state = STATE_RUNNING;
juliusbernth 0:64442857169c 276 //set up test
juliusbernth 0:64442857169c 277 testTimer.reset();
juliusbernth 0:64442857169c 278 testTimer.start();//start timer
juliusbernth 0:64442857169c 279
juliusbernth 5:e9bb800a3742 280 char spinState;
juliusbernth 8:41cd1fc8cbbe 281 pinLedRed = 1;
juliusbernth 0:64442857169c 282 //set up ticker to allow motor control thread to run periodically
juliusbernth 5:e9bb800a3742 283
juliusbernth 0:64442857169c 284 encoder.reset();//reset encoder
juliusbernth 0:64442857169c 285 lastPulses = 0;//reset previous encoder reading
juliusbernth 5:e9bb800a3742 286 encoderTimer.start();
juliusbernth 5:e9bb800a3742 287 PrintThread.start(PrintTimeRemaining);
juliusbernth 0:64442857169c 288 printf("\r\n Test setup complete \r\n");
juliusbernth 5:e9bb800a3742 289 EN_FAULTA.write(1);
juliusbernth 7:e36f61608c10 290 if(state == STATE_RUNNING){
juliusbernth 7:e36f61608c10 291 printf("\r\n running %d\r\n",state);
juliusbernth 7:e36f61608c10 292 }
juliusbernth 0:64442857169c 293
juliusbernth 5:e9bb800a3742 294
juliusbernth 5:e9bb800a3742 295 }
juliusbernth 0:64442857169c 296 }
juliusbernth 0:64442857169c 297
juliusbernth 0:64442857169c 298 void ReleaseReadButton(){
juliusbernth 0:64442857169c 299 semButton.release();
juliusbernth 0:64442857169c 300 }
juliusbernth 0:64442857169c 301
juliusbernth 0:64442857169c 302 void ReadButton()
juliusbernth 0:64442857169c 303 {
juliusbernth 0:64442857169c 304 int countThreashold = int(BUTTON_HOLD_TIME_S/BUTTON_READ_SAMPLETIME_S);
juliusbernth 0:64442857169c 305 while (1)
juliusbernth 0:64442857169c 306 {
juliusbernth 0:64442857169c 307 semButton.wait();
juliusbernth 0:64442857169c 308 int count = 0;
juliusbernth 0:64442857169c 309
juliusbernth 8:41cd1fc8cbbe 310 while(pinButton.read())
juliusbernth 0:64442857169c 311 {
juliusbernth 0:64442857169c 312 count++;
juliusbernth 0:64442857169c 313 if (count ==1){
juliusbernth 0:64442857169c 314 printf("button pressed\r\n");
juliusbernth 0:64442857169c 315 }
juliusbernth 0:64442857169c 316 ThisThread::sleep_for(10);
juliusbernth 0:64442857169c 317 }
juliusbernth 7:e36f61608c10 318 switch (state)
juliusbernth 0:64442857169c 319 {
juliusbernth 0:64442857169c 320 case STATE_READY:
juliusbernth 0:64442857169c 321 if(count > countThreashold)
juliusbernth 0:64442857169c 322 {
juliusbernth 0:64442857169c 323 printf("button released count = %d\r\n",count);
juliusbernth 0:64442857169c 324 count = 0;
juliusbernth 0:64442857169c 325 //CentrifugeTestThread.start(CentrifugeTest);
juliusbernth 5:e9bb800a3742 326 semStartTest.release();
juliusbernth 0:64442857169c 327 tickerMotorControl.attach_us(&MotorControlRelease, SAMPLE_TIME_US);//set up signal for control frequency
juliusbernth 0:64442857169c 328 tickerPrint.attach(&PrintRelease,PRINT_TIME_S);
juliusbernth 0:64442857169c 329 }
juliusbernth 0:64442857169c 330 break;
juliusbernth 0:64442857169c 331 case STATE_RUNNING:
juliusbernth 0:64442857169c 332 if(count >1){
juliusbernth 5:e9bb800a3742 333 EN_FAULTA.write(0);
juliusbernth 0:64442857169c 334 }
juliusbernth 0:64442857169c 335 break;
juliusbernth 0:64442857169c 336
juliusbernth 0:64442857169c 337 case STATE_ERROR:
juliusbernth 0:64442857169c 338
juliusbernth 0:64442857169c 339 break;
juliusbernth 0:64442857169c 340 default:
juliusbernth 0:64442857169c 341 break;
juliusbernth 0:64442857169c 342 }
juliusbernth 0:64442857169c 343 count = 0;
juliusbernth 5:e9bb800a3742 344 ThisThread::sleep_for(100);
juliusbernth 0:64442857169c 345 }
juliusbernth 0:64442857169c 346 }
juliusbernth 0:64442857169c 347
juliusbernth 0:64442857169c 348 int main(){
juliusbernth 0:64442857169c 349 printf("\r\nSystem Online\r\n");
juliusbernth 0:64442857169c 350 //set up system
juliusbernth 0:64442857169c 351
juliusbernth 0:64442857169c 352 //set up motor
juliusbernth 0:64442857169c 353 //EN_FAULTA.mode(PullDown);
juliusbernth 0:64442857169c 354 //STBY.mode(PullDown);
juliusbernth 0:64442857169c 355
juliusbernth 0:64442857169c 356 REF_PWM_A.period_us(100);
juliusbernth 0:64442857169c 357 PWMA.period_us(100);
juliusbernth 0:64442857169c 358 REF_PWM_A.write(0.5);//set current reference
juliusbernth 0:64442857169c 359
juliusbernth 0:64442857169c 360 EN_FAULTA.write(0);
juliusbernth 0:64442857169c 361 STBY.write(1);
juliusbernth 0:64442857169c 362 //calculate filter constant
juliusbernth 0:64442857169c 363 speedFilterConstant = 2*3.141593*SAMPLE_TIME_US/1000000*FILTER_CUTOFF_FREQ;
juliusbernth 0:64442857169c 364 speedFilterConstant /= (1 + speedFilterConstant);
juliusbernth 0:64442857169c 365
juliusbernth 0:64442857169c 366 SPIN_T[0] = 0.0;
juliusbernth 0:64442857169c 367 SPIN_T[1] = T_WARMUP;
juliusbernth 0:64442857169c 368 SPIN_T[2] = T_WARMUP + T_RAMP;
juliusbernth 0:64442857169c 369 SPIN_T[3] = T_WARMUP + T_RAMP + T_TEST;
juliusbernth 0:64442857169c 370 SPIN_T[4] = T_WARMUP + T_RAMP + T_TEST + T_RAMP;
juliusbernth 0:64442857169c 371 SPIN_T[5] = T_WARMUP + T_RAMP + T_TEST + T_RAMP + T_WARMUP;
juliusbernth 0:64442857169c 372
juliusbernth 0:64442857169c 373 /* //set up button interrupts
juliusbernth 0:64442857169c 374 pinButton.rise(ISR_Button_Rise);
juliusbernth 0:64442857169c 375 pinButton.fall(ISR_Button_Fall);
juliusbernth 0:64442857169c 376 */
juliusbernth 8:41cd1fc8cbbe 377 pinButton.mode(PullDown);
juliusbernth 8:41cd1fc8cbbe 378 pinLedWhite = 1;
juliusbernth 8:41cd1fc8cbbe 379
juliusbernth 8:41cd1fc8cbbe 380
juliusbernth 5:e9bb800a3742 381
juliusbernth 5:e9bb800a3742 382 state = STATE_READY;//set state to READY
juliusbernth 0:64442857169c 383 //start all threads
juliusbernth 0:64442857169c 384 MotorControlThread.start(MotorControl);
juliusbernth 0:64442857169c 385
juliusbernth 5:e9bb800a3742 386 CentrifugeTestThread.start(CentrifugeTest);
juliusbernth 0:64442857169c 387 //start print thread.
juliusbernth 0:64442857169c 388
juliusbernth 0:64442857169c 389 ReadButtonThread.start(ReadButton);
juliusbernth 0:64442857169c 390 tickerReleaseButton.attach(ReleaseReadButton,BUTTON_READ_SAMPLETIME_S);
juliusbernth 0:64442857169c 391
juliusbernth 0:64442857169c 392
juliusbernth 0:64442857169c 393 printf("\r\nSetup Complete\r\n");
juliusbernth 0:64442857169c 394 while (true) {
juliusbernth 0:64442857169c 395 //check state of button
juliusbernth 0:64442857169c 396 ThisThread::sleep_for(osWaitForever);
juliusbernth 0:64442857169c 397 }
juliusbernth 0:64442857169c 398 }