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@7:e36f61608c10, 2021-08-10 (annotated)

Committer:
juliusbernth
Date:
Tue Aug 10 11:00:13 2021 +0000
Revision:
7:e36f61608c10
Parent:
6:004dc33f4081
Child:
8:41cd1fc8cbbe
Working Version with accelerometer

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