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.
main.cpp@8:41cd1fc8cbbe, 2021-08-10 (annotated)
- 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?
User | Revision | Line number | New 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 | } |