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@6:004dc33f4081, 2021-08-09 (annotated)

Committer:
juliusbernth
Date:
Mon Aug 09 21:06:17 2021 +0000
Revision:
6:004dc33f4081
Parent:
5:e9bb800a3742
Child:
7:e36f61608c10
Minor Revisions for testing on 10/08

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