Cornelius Bezuidenhout / Mbed 2 deprecated Levitator

3rd year levitator

Dependencies:   MovingAverage NOKIA_5110 PinDetect mbed

main.cpp@0:d7cc9bad311b, 2018-09-09 (annotated)

Committer:
Generic
Date:
Sun Sep 09 18:45:52 2018 +0000
Revision:
0:d7cc9bad311b
Test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Generic 0:d7cc9bad311b 1 #include "mbed.h"
Generic 0:d7cc9bad311b 2 #include "NOKIA_5110.h"
Generic 0:d7cc9bad311b 3 #include "PinDetect.h"
Generic 0:d7cc9bad311b 4 #include "Pattern.h"
Generic 0:d7cc9bad311b 5 #include "MovingAverage.h"
Generic 0:d7cc9bad311b 6
Generic 0:d7cc9bad311b 7 #define PWM_PERIOD_US 100 //10kHz
Generic 0:d7cc9bad311b 8 #define MAX_PWM 1.0f //Maximum duty cycle 100%
Generic 0:d7cc9bad311b 9 #define MIN_PWM 0.0f //Minimum duty cycle 0%
Generic 0:d7cc9bad311b 10 #define OPEN 17080.0f //No object sensor value
Generic 0:d7cc9bad311b 11 #define CLOSED 0.1f //Fully obstructed sensor value
Generic 0:d7cc9bad311b 12 #define REFERENCE 1150.0f //Setpoint (desired) sensor value
Generic 0:d7cc9bad311b 13 #define MAX_POWER 40.8f //Maximum power watts of coil
Generic 0:d7cc9bad311b 14 #define MAX_CURRENT 3.5f //Maximum current amps through coil (Used to calculate instantaneous current)
Generic 0:d7cc9bad311b 15 #define CONST_POWER 4.78f //Const power drawn by fans and other hardware
Generic 0:d7cc9bad311b 16 #define COIL_VOLTAGE 12.0f //Voltage of the coil
Generic 0:d7cc9bad311b 17
Generic 0:d7cc9bad311b 18 //-- State variables
Generic 0:d7cc9bad311b 19
Generic 0:d7cc9bad311b 20 #define REF_PARAM 0
Generic 0:d7cc9bad311b 21 #define KP_PARAM 1
Generic 0:d7cc9bad311b 22 #define KD_PARAM 2
Generic 0:d7cc9bad311b 23
Generic 0:d7cc9bad311b 24 #define TEST 1
Generic 0:d7cc9bad311b 25 #define IDLE 2
Generic 0:d7cc9bad311b 26 #define LEVITATE 3
Generic 0:d7cc9bad311b 27 #define ERROR 4
Generic 0:d7cc9bad311b 28 #define CALIBRATION 5
Generic 0:d7cc9bad311b 29
Generic 0:d7cc9bad311b 30 //-- Function prototypes
Generic 0:d7cc9bad311b 31 void Initialise();
Generic 0:d7cc9bad311b 32 void Idle();
Generic 0:d7cc9bad311b 33 void Calibrate();
Generic 0:d7cc9bad311b 34 void Test();
Generic 0:d7cc9bad311b 35 void Error();
Generic 0:d7cc9bad311b 36 void SetRGB(bool,bool,bool);
Generic 0:d7cc9bad311b 37 void SetState(char);
Generic 0:d7cc9bad311b 38 void SelectButtonPress();
Generic 0:d7cc9bad311b 39 void UpButtonPress();
Generic 0:d7cc9bad311b 40 void DownButtonPress();
Generic 0:d7cc9bad311b 41 void SelectButtonHeld();
Generic 0:d7cc9bad311b 42 void UpButtonHeld();
Generic 0:d7cc9bad311b 43 void DownButtonHeld();
Generic 0:d7cc9bad311b 44 void UpdateScreen();
Generic 0:d7cc9bad311b 45 void PDController();
Generic 0:d7cc9bad311b 46 void Levitate();
Generic 0:d7cc9bad311b 47 void SetErrorLine(int,char*);
Generic 0:d7cc9bad311b 48 float RoundDigits(float,int);
Generic 0:d7cc9bad311b 49 void ResetTime();
Generic 0:d7cc9bad311b 50 void UpdateTime();
Generic 0:d7cc9bad311b 51
Generic 0:d7cc9bad311b 52
Generic 0:d7cc9bad311b 53 //-- Mbed objects
Generic 0:d7cc9bad311b 54 AnalogIn PosSensor(PC_0);
Generic 0:d7cc9bad311b 55 AnalogIn AmbSensor(PC_1);
Generic 0:d7cc9bad311b 56
Generic 0:d7cc9bad311b 57 NokiaLcd *Display;
Generic 0:d7cc9bad311b 58 LcdPins DisplayPins;
Generic 0:d7cc9bad311b 59
Generic 0:d7cc9bad311b 60 PinDetect SelectButton(PB_3);
Generic 0:d7cc9bad311b 61 PinDetect UpButton(PA_10);
Generic 0:d7cc9bad311b 62 PinDetect DownButton(PB_5);
Generic 0:d7cc9bad311b 63
Generic 0:d7cc9bad311b 64 Ticker ScreenUpdateTimer;
Generic 0:d7cc9bad311b 65 Timer CycleTimer;
Generic 0:d7cc9bad311b 66
Generic 0:d7cc9bad311b 67 DigitalOut RedLed(PA_8);
Generic 0:d7cc9bad311b 68 DigitalOut GreenLed(PA_9);
Generic 0:d7cc9bad311b 69 DigitalOut BlueLed(PB_10);
Generic 0:d7cc9bad311b 70
Generic 0:d7cc9bad311b 71 MovingAverage <float>AveragePower(10,0.0f);
Generic 0:d7cc9bad311b 72 MovingAverage <float>AverageCurrent(10,0.0f);
Generic 0:d7cc9bad311b 73
Generic 0:d7cc9bad311b 74 PwmOut CoilPWM(PWM_OUT);
Generic 0:d7cc9bad311b 75
Generic 0:d7cc9bad311b 76 //Variable decleration and initialisation
Generic 0:d7cc9bad311b 77
Generic 0:d7cc9bad311b 78 float ref = 0.0f; //Reference value
Generic 0:d7cc9bad311b 79 float pos = 0.0f; //Position value
Generic 0:d7cc9bad311b 80 float amb = 0.0f; //Ambient sensor value
Generic 0:d7cc9bad311b 81 float err = 0.0f; //Difference between setpoint and measured value
Generic 0:d7cc9bad311b 82 float prev_err = 0.0f; //Previous error
Generic 0:d7cc9bad311b 83 float d_err = 0.0f; //Rate of error change
Generic 0:d7cc9bad311b 84 float duty = 0.0f; //Current duty cycle
Generic 0:d7cc9bad311b 85
Generic 0:d7cc9bad311b 86 float current = 0.0f;
Generic 0:d7cc9bad311b 87 float power_consumed = 0.0f;
Generic 0:d7cc9bad311b 88
Generic 0:d7cc9bad311b 89 char line_buffer[14];
Generic 0:d7cc9bad311b 90
Generic 0:d7cc9bad311b 91 char error_line_1[14];
Generic 0:d7cc9bad311b 92 char error_line_2[14];
Generic 0:d7cc9bad311b 93 char error_line_3[14];
Generic 0:d7cc9bad311b 94 char error_line_4[14];
Generic 0:d7cc9bad311b 95 char error_line_5[14];
Generic 0:d7cc9bad311b 96
Generic 0:d7cc9bad311b 97 int state = 0; //State variable
Generic 0:d7cc9bad311b 98 float T = 0.000016129f; //Cycle time (measured)
Generic 0:d7cc9bad311b 99 float cycle_t = 0.0f;
Generic 0:d7cc9bad311b 100 float open_val = 0.0f;
Generic 0:d7cc9bad311b 101
Generic 0:d7cc9bad311b 102 int seconds = 0;
Generic 0:d7cc9bad311b 103 int minutes = 0;
Generic 0:d7cc9bad311b 104 int hours = 0;
Generic 0:d7cc9bad311b 105 int delay = 260;
Generic 0:d7cc9bad311b 106
Generic 0:d7cc9bad311b 107 int prev_seconds = 0;
Generic 0:d7cc9bad311b 108 int prev_minutes = 0;
Generic 0:d7cc9bad311b 109 int prev_hours = 0;
Generic 0:d7cc9bad311b 110
Generic 0:d7cc9bad311b 111 bool levitating = false;
Generic 0:d7cc9bad311b 112
Generic 0:d7cc9bad311b 113
Generic 0:d7cc9bad311b 114 volatile float Kp = 11000.0; //Proportional value of PD controller
Generic 0:d7cc9bad311b 115 volatile float Kd = 65.0f; //Derivative value of PD controller
Generic 0:d7cc9bad311b 116 volatile float offset = 20.0f; //Control bias
Generic 0:d7cc9bad311b 117 volatile int selected_parameter = 0;
Generic 0:d7cc9bad311b 118
Generic 0:d7cc9bad311b 119 volatile bool select_button_flag = false;
Generic 0:d7cc9bad311b 120 volatile bool down_button_flag = false;
Generic 0:d7cc9bad311b 121 volatile bool up_button_flag = false;
Generic 0:d7cc9bad311b 122 volatile bool select_button_held_flag = false;
Generic 0:d7cc9bad311b 123 volatile bool down_button_held_flag = false;
Generic 0:d7cc9bad311b 124 volatile bool up_button_held_flag = false;
Generic 0:d7cc9bad311b 125
Generic 0:d7cc9bad311b 126
Generic 0:d7cc9bad311b 127 int main()
Generic 0:d7cc9bad311b 128 {
Generic 0:d7cc9bad311b 129 Initialise();
Generic 0:d7cc9bad311b 130
Generic 0:d7cc9bad311b 131 while(1)
Generic 0:d7cc9bad311b 132 {
Generic 0:d7cc9bad311b 133 switch(state)
Generic 0:d7cc9bad311b 134 {
Generic 0:d7cc9bad311b 135 case IDLE:
Generic 0:d7cc9bad311b 136 Idle();
Generic 0:d7cc9bad311b 137 break;
Generic 0:d7cc9bad311b 138 case CALIBRATION:
Generic 0:d7cc9bad311b 139 Calibrate();
Generic 0:d7cc9bad311b 140 break;
Generic 0:d7cc9bad311b 141 case TEST:
Generic 0:d7cc9bad311b 142 Test();
Generic 0:d7cc9bad311b 143 break;
Generic 0:d7cc9bad311b 144 case LEVITATE:
Generic 0:d7cc9bad311b 145 Levitate();
Generic 0:d7cc9bad311b 146 break;
Generic 0:d7cc9bad311b 147 case ERROR:
Generic 0:d7cc9bad311b 148 Error();
Generic 0:d7cc9bad311b 149 break;
Generic 0:d7cc9bad311b 150 }
Generic 0:d7cc9bad311b 151 }
Generic 0:d7cc9bad311b 152 }
Generic 0:d7cc9bad311b 153
Generic 0:d7cc9bad311b 154 void Initialise()
Generic 0:d7cc9bad311b 155 {
Generic 0:d7cc9bad311b 156 DisplayPins.sce = PB_8;
Generic 0:d7cc9bad311b 157 DisplayPins.rst = PB_9;
Generic 0:d7cc9bad311b 158 DisplayPins.dc = PB_6;
Generic 0:d7cc9bad311b 159 DisplayPins.mosi = PA_7;
Generic 0:d7cc9bad311b 160 DisplayPins.miso = NC;
Generic 0:d7cc9bad311b 161 DisplayPins.sclk = PA_5;
Generic 0:d7cc9bad311b 162 Display = new NokiaLcd(DisplayPins);
Generic 0:d7cc9bad311b 163 Display->InitLcd();
Generic 0:d7cc9bad311b 164 Display->ClearLcdMem();
Generic 0:d7cc9bad311b 165 Display->SetXY(0,0);
Generic 0:d7cc9bad311b 166 wait(0.1);
Generic 0:d7cc9bad311b 167 CoilPWM.period_us(PWM_PERIOD_US);
Generic 0:d7cc9bad311b 168 ref = REFERENCE;
Generic 0:d7cc9bad311b 169
Generic 0:d7cc9bad311b 170 CoilPWM.period_us(PWM_PERIOD_US);
Generic 0:d7cc9bad311b 171 CoilPWM = 0.0f;
Generic 0:d7cc9bad311b 172
Generic 0:d7cc9bad311b 173 SelectButton.mode(PullDown);
Generic 0:d7cc9bad311b 174 SelectButton.attach_asserted(&SelectButtonPress);
Generic 0:d7cc9bad311b 175 SelectButton.attach_asserted_held(&SelectButtonHeld);
Generic 0:d7cc9bad311b 176 SelectButton.setSampleFrequency();
Generic 0:d7cc9bad311b 177
Generic 0:d7cc9bad311b 178 UpButton.mode(PullDown);
Generic 0:d7cc9bad311b 179 UpButton.attach_asserted(&UpButtonPress);
Generic 0:d7cc9bad311b 180 UpButton.attach_asserted_held(&UpButtonHeld);
Generic 0:d7cc9bad311b 181 UpButton.setSampleFrequency();
Generic 0:d7cc9bad311b 182
Generic 0:d7cc9bad311b 183 DownButton.mode(PullDown);
Generic 0:d7cc9bad311b 184 DownButton.attach_asserted(&DownButtonPress);
Generic 0:d7cc9bad311b 185 DownButton.attach_asserted_held(&DownButtonHeld);
Generic 0:d7cc9bad311b 186 DownButton.setSampleFrequency();
Generic 0:d7cc9bad311b 187
Generic 0:d7cc9bad311b 188 DisplayPattern(Display,1);
Generic 0:d7cc9bad311b 189
Generic 0:d7cc9bad311b 190
Generic 0:d7cc9bad311b 191 SetRGB(true,false,false);
Generic 0:d7cc9bad311b 192 wait(0.5);
Generic 0:d7cc9bad311b 193 SetRGB(false,true,false);
Generic 0:d7cc9bad311b 194 wait(0.5);
Generic 0:d7cc9bad311b 195 SetRGB(false,false,true);
Generic 0:d7cc9bad311b 196 wait(0.5);
Generic 0:d7cc9bad311b 197 SetRGB(true,true,true);
Generic 0:d7cc9bad311b 198 wait(2);
Generic 0:d7cc9bad311b 199 Display->ClearLcdMem();
Generic 0:d7cc9bad311b 200 SetRGB(false,false,false);
Generic 0:d7cc9bad311b 201
Generic 0:d7cc9bad311b 202 ScreenUpdateTimer.attach(&UpdateScreen,1);
Generic 0:d7cc9bad311b 203
Generic 0:d7cc9bad311b 204 SetState(TEST);
Generic 0:d7cc9bad311b 205
Generic 0:d7cc9bad311b 206 }
Generic 0:d7cc9bad311b 207
Generic 0:d7cc9bad311b 208 void Levitate()
Generic 0:d7cc9bad311b 209 {
Generic 0:d7cc9bad311b 210 PDController();
Generic 0:d7cc9bad311b 211
Generic 0:d7cc9bad311b 212 if( down_button_held_flag && up_button_held_flag && select_button_held_flag )
Generic 0:d7cc9bad311b 213 {
Generic 0:d7cc9bad311b 214 down_button_held_flag = false;
Generic 0:d7cc9bad311b 215 up_button_held_flag = false;
Generic 0:d7cc9bad311b 216 select_button_held_flag = false;
Generic 0:d7cc9bad311b 217
Generic 0:d7cc9bad311b 218 SetState(CALIBRATION);
Generic 0:d7cc9bad311b 219 }
Generic 0:d7cc9bad311b 220
Generic 0:d7cc9bad311b 221
Generic 0:d7cc9bad311b 222 if( select_button_held_flag )
Generic 0:d7cc9bad311b 223 {
Generic 0:d7cc9bad311b 224 select_button_held_flag = false;
Generic 0:d7cc9bad311b 225 select_button_flag = false;
Generic 0:d7cc9bad311b 226 SetState(IDLE);
Generic 0:d7cc9bad311b 227 }
Generic 0:d7cc9bad311b 228 }
Generic 0:d7cc9bad311b 229
Generic 0:d7cc9bad311b 230 void PDController()
Generic 0:d7cc9bad311b 231 {
Generic 0:d7cc9bad311b 232 pos = PosSensor.read();
Generic 0:d7cc9bad311b 233 pos *= 3461.5f;
Generic 0:d7cc9bad311b 234
Generic 0:d7cc9bad311b 235 err = pos - ref;
Generic 0:d7cc9bad311b 236
Generic 0:d7cc9bad311b 237 d_err = (err-prev_err)/T;
Generic 0:d7cc9bad311b 238
Generic 0:d7cc9bad311b 239 duty = 17*(Kp*err+Kd*d_err)+10000.0f;
Generic 0:d7cc9bad311b 240 prev_err = err;
Generic 0:d7cc9bad311b 241
Generic 0:d7cc9bad311b 242
Generic 0:d7cc9bad311b 243 if( pos < CLOSED || pos > OPEN )
Generic 0:d7cc9bad311b 244 {
Generic 0:d7cc9bad311b 245 CoilPWM.pulsewidth(0.0f);
Generic 0:d7cc9bad311b 246 AveragePower.Insert(0.0f);
Generic 0:d7cc9bad311b 247 AverageCurrent.Insert(0.0f);
Generic 0:d7cc9bad311b 248 }
Generic 0:d7cc9bad311b 249 else
Generic 0:d7cc9bad311b 250 {
Generic 0:d7cc9bad311b 251 if( duty > PWM_PERIOD_US )
Generic 0:d7cc9bad311b 252 {
Generic 0:d7cc9bad311b 253 CoilPWM.pulsewidth_us(MAX_PWM*PWM_PERIOD_US);
Generic 0:d7cc9bad311b 254 //AveragePower.Insert()
Generic 0:d7cc9bad311b 255 AverageCurrent.Insert(MAX_PWM*MAX_CURRENT);
Generic 0:d7cc9bad311b 256 }
Generic 0:d7cc9bad311b 257 else if( duty < 0.0f )
Generic 0:d7cc9bad311b 258 {
Generic 0:d7cc9bad311b 259 CoilPWM.pulsewidth_us(0.0f);
Generic 0:d7cc9bad311b 260 AveragePower.Insert(0.0f);
Generic 0:d7cc9bad311b 261 AverageCurrent.Insert(0.0f);
Generic 0:d7cc9bad311b 262 }
Generic 0:d7cc9bad311b 263 }
Generic 0:d7cc9bad311b 264
Generic 0:d7cc9bad311b 265
Generic 0:d7cc9bad311b 266
Generic 0:d7cc9bad311b 267 wait_us(260); //Delay longer than PWM period needed to ensure the set duty cycle completes atleast one cycle
Generic 0:d7cc9bad311b 268 }
Generic 0:d7cc9bad311b 269
Generic 0:d7cc9bad311b 270 void Error()
Generic 0:d7cc9bad311b 271 {
Generic 0:d7cc9bad311b 272 CoilPWM = 0.0f;
Generic 0:d7cc9bad311b 273
Generic 0:d7cc9bad311b 274 if( select_button_held_flag )
Generic 0:d7cc9bad311b 275 {
Generic 0:d7cc9bad311b 276 select_button_held_flag = false;
Generic 0:d7cc9bad311b 277 select_button_flag = false;
Generic 0:d7cc9bad311b 278 SetState(TEST);
Generic 0:d7cc9bad311b 279 }
Generic 0:d7cc9bad311b 280 }
Generic 0:d7cc9bad311b 281
Generic 0:d7cc9bad311b 282 void Test()
Generic 0:d7cc9bad311b 283 {
Generic 0:d7cc9bad311b 284 bool test_passed = false;
Generic 0:d7cc9bad311b 285 open_val = 3461.5f*PosSensor.read();
Generic 0:d7cc9bad311b 286 amb = 3461.5f*AmbSensor.read();
Generic 0:d7cc9bad311b 287
Generic 0:d7cc9bad311b 288 test_passed = ( open_val > amb );
Generic 0:d7cc9bad311b 289
Generic 0:d7cc9bad311b 290 Display->ClearLcdMem();
Generic 0:d7cc9bad311b 291 Display->SetXY(0,0);
Generic 0:d7cc9bad311b 292
Generic 0:d7cc9bad311b 293 if( test_passed )
Generic 0:d7cc9bad311b 294 {
Generic 0:d7cc9bad311b 295 Display->DrawString("Test Passed");
Generic 0:d7cc9bad311b 296 }
Generic 0:d7cc9bad311b 297 else
Generic 0:d7cc9bad311b 298 {
Generic 0:d7cc9bad311b 299 Display->DrawString("Test Failed");
Generic 0:d7cc9bad311b 300 }
Generic 0:d7cc9bad311b 301
Generic 0:d7cc9bad311b 302 wait(2);
Generic 0:d7cc9bad311b 303
Generic 0:d7cc9bad311b 304 if( test_passed )
Generic 0:d7cc9bad311b 305 {
Generic 0:d7cc9bad311b 306 SetState(IDLE);
Generic 0:d7cc9bad311b 307 }
Generic 0:d7cc9bad311b 308 else
Generic 0:d7cc9bad311b 309 {
Generic 0:d7cc9bad311b 310 SetErrorLine(1,"Error");
Generic 0:d7cc9bad311b 311 SetErrorLine(2,"!!!!!!!!!!!!!!");
Generic 0:d7cc9bad311b 312 SetErrorLine(3,"Sensor Blocked");
Generic 0:d7cc9bad311b 313 SetErrorLine(4,"or");
Generic 0:d7cc9bad311b 314 SetErrorLine(5,"Disconnected");
Generic 0:d7cc9bad311b 315 SetState(ERROR);
Generic 0:d7cc9bad311b 316 }
Generic 0:d7cc9bad311b 317 }
Generic 0:d7cc9bad311b 318
Generic 0:d7cc9bad311b 319 void Idle()
Generic 0:d7cc9bad311b 320 {
Generic 0:d7cc9bad311b 321 if( select_button_held_flag && down_button_held_flag && up_button_held_flag )
Generic 0:d7cc9bad311b 322 {
Generic 0:d7cc9bad311b 323 select_button_held_flag = false;
Generic 0:d7cc9bad311b 324 down_button_held_flag = false;
Generic 0:d7cc9bad311b 325 up_button_held_flag = false;
Generic 0:d7cc9bad311b 326
Generic 0:d7cc9bad311b 327 SetState(CALIBRATION);
Generic 0:d7cc9bad311b 328 }
Generic 0:d7cc9bad311b 329
Generic 0:d7cc9bad311b 330 if( select_button_flag )
Generic 0:d7cc9bad311b 331 {
Generic 0:d7cc9bad311b 332 select_button_flag = false;
Generic 0:d7cc9bad311b 333 SetState(LEVITATE);
Generic 0:d7cc9bad311b 334 }
Generic 0:d7cc9bad311b 335
Generic 0:d7cc9bad311b 336 CoilPWM = 0.0f;
Generic 0:d7cc9bad311b 337 }
Generic 0:d7cc9bad311b 338
Generic 0:d7cc9bad311b 339 void Calibrate()
Generic 0:d7cc9bad311b 340 {
Generic 0:d7cc9bad311b 341 CycleTimer.start();
Generic 0:d7cc9bad311b 342
Generic 0:d7cc9bad311b 343 if( select_button_held_flag )
Generic 0:d7cc9bad311b 344 {
Generic 0:d7cc9bad311b 345 select_button_held_flag = false;
Generic 0:d7cc9bad311b 346 select_button_flag = false;
Generic 0:d7cc9bad311b 347 SetState(IDLE);
Generic 0:d7cc9bad311b 348 }
Generic 0:d7cc9bad311b 349
Generic 0:d7cc9bad311b 350 if( up_button_held_flag )
Generic 0:d7cc9bad311b 351 {
Generic 0:d7cc9bad311b 352 up_button_held_flag = false;
Generic 0:d7cc9bad311b 353
Generic 0:d7cc9bad311b 354 if( selected_parameter == KP_PARAM )
Generic 0:d7cc9bad311b 355 Kp += 10.0f;
Generic 0:d7cc9bad311b 356 else if( selected_parameter == KD_PARAM )
Generic 0:d7cc9bad311b 357 Kd += 10.0f;
Generic 0:d7cc9bad311b 358 else if( selected_parameter == REF_PARAM )
Generic 0:d7cc9bad311b 359 ref += 10.0f;
Generic 0:d7cc9bad311b 360 }
Generic 0:d7cc9bad311b 361
Generic 0:d7cc9bad311b 362 if( down_button_held_flag )
Generic 0:d7cc9bad311b 363 {
Generic 0:d7cc9bad311b 364 down_button_held_flag = false;
Generic 0:d7cc9bad311b 365
Generic 0:d7cc9bad311b 366 if( selected_parameter == KP_PARAM )
Generic 0:d7cc9bad311b 367 Kp -= 10.0f;
Generic 0:d7cc9bad311b 368 else if( selected_parameter == KD_PARAM )
Generic 0:d7cc9bad311b 369 Kd -= 10.0f;
Generic 0:d7cc9bad311b 370 else if( selected_parameter == REF_PARAM )
Generic 0:d7cc9bad311b 371 ref -= 10.0f;
Generic 0:d7cc9bad311b 372 }
Generic 0:d7cc9bad311b 373
Generic 0:d7cc9bad311b 374 if( select_button_flag )
Generic 0:d7cc9bad311b 375 {
Generic 0:d7cc9bad311b 376 select_button_flag = false;
Generic 0:d7cc9bad311b 377
Generic 0:d7cc9bad311b 378 if( selected_parameter < 2 )
Generic 0:d7cc9bad311b 379 selected_parameter++;
Generic 0:d7cc9bad311b 380 else
Generic 0:d7cc9bad311b 381 selected_parameter = 0;
Generic 0:d7cc9bad311b 382
Generic 0:d7cc9bad311b 383
Generic 0:d7cc9bad311b 384 }
Generic 0:d7cc9bad311b 385
Generic 0:d7cc9bad311b 386 if( up_button_flag )
Generic 0:d7cc9bad311b 387 {
Generic 0:d7cc9bad311b 388 up_button_flag = false;
Generic 0:d7cc9bad311b 389
Generic 0:d7cc9bad311b 390 if( selected_parameter == KP_PARAM )
Generic 0:d7cc9bad311b 391 Kp += 5.0f;
Generic 0:d7cc9bad311b 392 else if( selected_parameter == KD_PARAM )
Generic 0:d7cc9bad311b 393 Kd += 5.0f;
Generic 0:d7cc9bad311b 394 else if( selected_parameter == REF_PARAM )
Generic 0:d7cc9bad311b 395 ref += 5.0f;
Generic 0:d7cc9bad311b 396 }
Generic 0:d7cc9bad311b 397
Generic 0:d7cc9bad311b 398 if( down_button_flag )
Generic 0:d7cc9bad311b 399 {
Generic 0:d7cc9bad311b 400 down_button_flag = false;
Generic 0:d7cc9bad311b 401
Generic 0:d7cc9bad311b 402 if( selected_parameter == KP_PARAM )
Generic 0:d7cc9bad311b 403 Kp -= 5.0f;
Generic 0:d7cc9bad311b 404 else if( selected_parameter == KD_PARAM )
Generic 0:d7cc9bad311b 405 Kd -= 5.0f;
Generic 0:d7cc9bad311b 406 else if( selected_parameter == REF_PARAM )
Generic 0:d7cc9bad311b 407 ref -= 5.0f;
Generic 0:d7cc9bad311b 408 }
Generic 0:d7cc9bad311b 409
Generic 0:d7cc9bad311b 410 PDController();
Generic 0:d7cc9bad311b 411
Generic 0:d7cc9bad311b 412 CycleTimer.stop();
Generic 0:d7cc9bad311b 413 cycle_t = CycleTimer.read()*1000000.0f;
Generic 0:d7cc9bad311b 414 CycleTimer.reset();
Generic 0:d7cc9bad311b 415 }
Generic 0:d7cc9bad311b 416
Generic 0:d7cc9bad311b 417 void SetRGB(bool r, bool g, bool b)
Generic 0:d7cc9bad311b 418 {
Generic 0:d7cc9bad311b 419 RedLed = (r ? 1 : 0 );
Generic 0:d7cc9bad311b 420 GreenLed = (g ? 1 : 0 );
Generic 0:d7cc9bad311b 421 BlueLed = (b ? 1 : 0 );
Generic 0:d7cc9bad311b 422 }
Generic 0:d7cc9bad311b 423
Generic 0:d7cc9bad311b 424 float RoundDigits(float x, int numdigits)
Generic 0:d7cc9bad311b 425 {
Generic 0:d7cc9bad311b 426 return ceil(x * pow(10.0f,numdigits))/pow(10.0f,numdigits);
Generic 0:d7cc9bad311b 427 }
Generic 0:d7cc9bad311b 428
Generic 0:d7cc9bad311b 429 void SetState(char to_state)
Generic 0:d7cc9bad311b 430 {
Generic 0:d7cc9bad311b 431 Display->ClearLcdMem();
Generic 0:d7cc9bad311b 432 Display->SetXY(0,0);
Generic 0:d7cc9bad311b 433 CoilPWM = 0.0f;
Generic 0:d7cc9bad311b 434
Generic 0:d7cc9bad311b 435 if( to_state == IDLE )
Generic 0:d7cc9bad311b 436 {
Generic 0:d7cc9bad311b 437 state = IDLE;
Generic 0:d7cc9bad311b 438 SetRGB(false,true,false);
Generic 0:d7cc9bad311b 439 DisplayPattern(Display,2);
Generic 0:d7cc9bad311b 440 }
Generic 0:d7cc9bad311b 441 else if( to_state == CALIBRATION)
Generic 0:d7cc9bad311b 442 {
Generic 0:d7cc9bad311b 443 state = CALIBRATION;
Generic 0:d7cc9bad311b 444 SetRGB(true,true,false);
Generic 0:d7cc9bad311b 445 }
Generic 0:d7cc9bad311b 446 else if( to_state == TEST )
Generic 0:d7cc9bad311b 447 {
Generic 0:d7cc9bad311b 448 state = TEST;
Generic 0:d7cc9bad311b 449 SetRGB(false,true,true);
Generic 0:d7cc9bad311b 450 Display->DrawString("Testing");
Generic 0:d7cc9bad311b 451 }
Generic 0:d7cc9bad311b 452 else if( to_state == ERROR )
Generic 0:d7cc9bad311b 453 {
Generic 0:d7cc9bad311b 454 SetRGB(true,false,false);
Generic 0:d7cc9bad311b 455 Display->DrawString(error_line_1);
Generic 0:d7cc9bad311b 456 Display->SetXY(0,1);
Generic 0:d7cc9bad311b 457 Display->DrawString(error_line_2);
Generic 0:d7cc9bad311b 458 Display->SetXY(0,2);
Generic 0:d7cc9bad311b 459 Display->DrawString(error_line_3);
Generic 0:d7cc9bad311b 460 Display->SetXY(0,3);
Generic 0:d7cc9bad311b 461 Display->DrawString(error_line_4);
Generic 0:d7cc9bad311b 462 Display->SetXY(0,4);
Generic 0:d7cc9bad311b 463 Display->DrawString(error_line_5);
Generic 0:d7cc9bad311b 464 state = ERROR;
Generic 0:d7cc9bad311b 465 }
Generic 0:d7cc9bad311b 466 else if( to_state == LEVITATE )
Generic 0:d7cc9bad311b 467 {
Generic 0:d7cc9bad311b 468 Display->DrawString("Levitate");
Generic 0:d7cc9bad311b 469 SetRGB(false,false,true);
Generic 0:d7cc9bad311b 470 state = LEVITATE;
Generic 0:d7cc9bad311b 471 }
Generic 0:d7cc9bad311b 472 }
Generic 0:d7cc9bad311b 473
Generic 0:d7cc9bad311b 474 void SelectButtonPress()
Generic 0:d7cc9bad311b 475 {
Generic 0:d7cc9bad311b 476 select_button_flag = true;
Generic 0:d7cc9bad311b 477 }
Generic 0:d7cc9bad311b 478
Generic 0:d7cc9bad311b 479 void UpButtonPress()
Generic 0:d7cc9bad311b 480 {
Generic 0:d7cc9bad311b 481 up_button_flag = true;
Generic 0:d7cc9bad311b 482 }
Generic 0:d7cc9bad311b 483
Generic 0:d7cc9bad311b 484 void DownButtonPress()
Generic 0:d7cc9bad311b 485 {
Generic 0:d7cc9bad311b 486 down_button_flag = true;
Generic 0:d7cc9bad311b 487 }
Generic 0:d7cc9bad311b 488
Generic 0:d7cc9bad311b 489 void SelectButtonHeld()
Generic 0:d7cc9bad311b 490 {
Generic 0:d7cc9bad311b 491 select_button_held_flag = true;
Generic 0:d7cc9bad311b 492 }
Generic 0:d7cc9bad311b 493
Generic 0:d7cc9bad311b 494 void UpButtonHeld()
Generic 0:d7cc9bad311b 495 {
Generic 0:d7cc9bad311b 496 up_button_held_flag = true;
Generic 0:d7cc9bad311b 497 }
Generic 0:d7cc9bad311b 498
Generic 0:d7cc9bad311b 499 void DownButtonHeld()
Generic 0:d7cc9bad311b 500 {
Generic 0:d7cc9bad311b 501 down_button_held_flag = true;
Generic 0:d7cc9bad311b 502 }
Generic 0:d7cc9bad311b 503
Generic 0:d7cc9bad311b 504 void UpdateScreen()
Generic 0:d7cc9bad311b 505 {
Generic 0:d7cc9bad311b 506 power_consumed += (CONST_POWER + AverageCurrent.GetAverage()*COIL_VOLTAGE)/1000.0f;
Generic 0:d7cc9bad311b 507
Generic 0:d7cc9bad311b 508 if( state == CALIBRATION )
Generic 0:d7cc9bad311b 509 {
Generic 0:d7cc9bad311b 510 Display->ClearLcdMem();
Generic 0:d7cc9bad311b 511 Display->SetXY(0,0);
Generic 0:d7cc9bad311b 512 Display->DrawString("Calibrate");
Generic 0:d7cc9bad311b 513
Generic 0:d7cc9bad311b 514 Display->SetXY(0,1);
Generic 0:d7cc9bad311b 515 sprintf(line_buffer,"Kp=%.2f",Kp);
Generic 0:d7cc9bad311b 516
Generic 0:d7cc9bad311b 517 if( selected_parameter == KP_PARAM )
Generic 0:d7cc9bad311b 518 Display->DrawChar('>');
Generic 0:d7cc9bad311b 519
Generic 0:d7cc9bad311b 520 Display->DrawString(line_buffer);
Generic 0:d7cc9bad311b 521
Generic 0:d7cc9bad311b 522 Display->SetXY(0,2);
Generic 0:d7cc9bad311b 523 sprintf(line_buffer,"Kd=%.2f",Kd);
Generic 0:d7cc9bad311b 524
Generic 0:d7cc9bad311b 525 if( selected_parameter == KD_PARAM )
Generic 0:d7cc9bad311b 526 Display->DrawChar('>');
Generic 0:d7cc9bad311b 527
Generic 0:d7cc9bad311b 528 Display->DrawString(line_buffer);
Generic 0:d7cc9bad311b 529
Generic 0:d7cc9bad311b 530 Display->SetXY(0,3);
Generic 0:d7cc9bad311b 531 sprintf(line_buffer,"T=%.2f",ref);
Generic 0:d7cc9bad311b 532
Generic 0:d7cc9bad311b 533 if( selected_parameter == REF_PARAM )
Generic 0:d7cc9bad311b 534 Display->DrawChar('>');
Generic 0:d7cc9bad311b 535
Generic 0:d7cc9bad311b 536 Display->DrawString(line_buffer);
Generic 0:d7cc9bad311b 537
Generic 0:d7cc9bad311b 538 Display->SetXY(0,4);
Generic 0:d7cc9bad311b 539 sprintf(line_buffer,"T=%.2fus",cycle_t);
Generic 0:d7cc9bad311b 540 Display->DrawString(line_buffer);
Generic 0:d7cc9bad311b 541
Generic 0:d7cc9bad311b 542 Display->SetXY(0,5);
Generic 0:d7cc9bad311b 543 sprintf(line_buffer,"Pos=%.2f",pos);
Generic 0:d7cc9bad311b 544 Display->DrawString(line_buffer);
Generic 0:d7cc9bad311b 545 }
Generic 0:d7cc9bad311b 546
Generic 0:d7cc9bad311b 547 if( state == LEVITATE )
Generic 0:d7cc9bad311b 548 {
Generic 0:d7cc9bad311b 549 if( pos < open_val-100)
Generic 0:d7cc9bad311b 550 {
Generic 0:d7cc9bad311b 551 UpdateTime();
Generic 0:d7cc9bad311b 552 levitating = true;
Generic 0:d7cc9bad311b 553 }
Generic 0:d7cc9bad311b 554 else
Generic 0:d7cc9bad311b 555 {
Generic 0:d7cc9bad311b 556 prev_seconds = seconds;
Generic 0:d7cc9bad311b 557 prev_minutes = minutes;
Generic 0:d7cc9bad311b 558 prev_hours = hours;
Generic 0:d7cc9bad311b 559 levitating = false;
Generic 0:d7cc9bad311b 560 ResetTime();
Generic 0:d7cc9bad311b 561 }
Generic 0:d7cc9bad311b 562
Generic 0:d7cc9bad311b 563
Generic 0:d7cc9bad311b 564 Display->ClearLcdMem();
Generic 0:d7cc9bad311b 565 Display->SetXY(0,0);
Generic 0:d7cc9bad311b 566 Display->DrawString("Current:");
Generic 0:d7cc9bad311b 567 //Display->SetXY(0,1);
Generic 0:d7cc9bad311b 568 current = AverageCurrent.GetAverage();
Generic 0:d7cc9bad311b 569
Generic 0:d7cc9bad311b 570 if( current < 0.0f )
Generic 0:d7cc9bad311b 571 current = 0.0f;
Generic 0:d7cc9bad311b 572
Generic 0:d7cc9bad311b 573 sprintf(line_buffer,"%.2fA",current);
Generic 0:d7cc9bad311b 574 Display->DrawString(line_buffer);
Generic 0:d7cc9bad311b 575
Generic 0:d7cc9bad311b 576 sprintf(line_buffer,"%.2fkWs",power_consumed);
Generic 0:d7cc9bad311b 577
Generic 0:d7cc9bad311b 578 Display->SetXY(0,1);
Generic 0:d7cc9bad311b 579 Display->DrawString("Consumption");
Generic 0:d7cc9bad311b 580 Display->SetXY(0,2);
Generic 0:d7cc9bad311b 581 Display->DrawString(line_buffer);
Generic 0:d7cc9bad311b 582
Generic 0:d7cc9bad311b 583 if( levitating )
Generic 0:d7cc9bad311b 584 {
Generic 0:d7cc9bad311b 585 SetRGB(false,false,true);
Generic 0:d7cc9bad311b 586 sprintf(line_buffer,"%dh%dm%ds",hours,minutes,seconds);
Generic 0:d7cc9bad311b 587 }
Generic 0:d7cc9bad311b 588 else
Generic 0:d7cc9bad311b 589 {
Generic 0:d7cc9bad311b 590 SetRGB(true,false,true);
Generic 0:d7cc9bad311b 591 sprintf(line_buffer,"%dh%dm%ds",prev_hours,prev_minutes,prev_seconds);
Generic 0:d7cc9bad311b 592 }
Generic 0:d7cc9bad311b 593
Generic 0:d7cc9bad311b 594 Display->SetXY(0,3);
Generic 0:d7cc9bad311b 595 Display->DrawString("Time:");
Generic 0:d7cc9bad311b 596 //Display->SetXY(0,5);
Generic 0:d7cc9bad311b 597 Display->DrawString(line_buffer);
Generic 0:d7cc9bad311b 598
Generic 0:d7cc9bad311b 599 Display->SetXY(0,4);
Generic 0:d7cc9bad311b 600 Display->DrawString("Airgap:15mm");
Generic 0:d7cc9bad311b 601 }
Generic 0:d7cc9bad311b 602
Generic 0:d7cc9bad311b 603
Generic 0:d7cc9bad311b 604 }
Generic 0:d7cc9bad311b 605
Generic 0:d7cc9bad311b 606 void SetErrorLine(int line,char *message)
Generic 0:d7cc9bad311b 607 {
Generic 0:d7cc9bad311b 608 char *temp;
Generic 0:d7cc9bad311b 609
Generic 0:d7cc9bad311b 610 switch(line)
Generic 0:d7cc9bad311b 611 {
Generic 0:d7cc9bad311b 612 case 1:
Generic 0:d7cc9bad311b 613 temp = error_line_1;
Generic 0:d7cc9bad311b 614 break;
Generic 0:d7cc9bad311b 615 case 2:
Generic 0:d7cc9bad311b 616 temp = error_line_2;
Generic 0:d7cc9bad311b 617 break;
Generic 0:d7cc9bad311b 618 case 3:
Generic 0:d7cc9bad311b 619 temp = error_line_3;
Generic 0:d7cc9bad311b 620 break;
Generic 0:d7cc9bad311b 621 case 4:
Generic 0:d7cc9bad311b 622 temp = error_line_4;
Generic 0:d7cc9bad311b 623 break;
Generic 0:d7cc9bad311b 624 case 5:
Generic 0:d7cc9bad311b 625 temp = error_line_5;
Generic 0:d7cc9bad311b 626 break;
Generic 0:d7cc9bad311b 627 }
Generic 0:d7cc9bad311b 628
Generic 0:d7cc9bad311b 629
Generic 0:d7cc9bad311b 630 for(int i = 0; i < 14 ; i++ )
Generic 0:d7cc9bad311b 631 {
Generic 0:d7cc9bad311b 632 *temp = message[i];
Generic 0:d7cc9bad311b 633 temp++;
Generic 0:d7cc9bad311b 634 }
Generic 0:d7cc9bad311b 635
Generic 0:d7cc9bad311b 636 }
Generic 0:d7cc9bad311b 637
Generic 0:d7cc9bad311b 638 void ResetTime()
Generic 0:d7cc9bad311b 639 {
Generic 0:d7cc9bad311b 640 seconds = 0;
Generic 0:d7cc9bad311b 641 minutes = 0;
Generic 0:d7cc9bad311b 642 hours = 0;
Generic 0:d7cc9bad311b 643 }
Generic 0:d7cc9bad311b 644
Generic 0:d7cc9bad311b 645 void UpdateTime()
Generic 0:d7cc9bad311b 646 {
Generic 0:d7cc9bad311b 647 if( seconds < 60 )
Generic 0:d7cc9bad311b 648 seconds++;
Generic 0:d7cc9bad311b 649 else
Generic 0:d7cc9bad311b 650 {
Generic 0:d7cc9bad311b 651 seconds = 0;
Generic 0:d7cc9bad311b 652
Generic 0:d7cc9bad311b 653 if( minutes < 60 )
Generic 0:d7cc9bad311b 654 minutes++;
Generic 0:d7cc9bad311b 655 else
Generic 0:d7cc9bad311b 656 {
Generic 0:d7cc9bad311b 657 minutes = 0;
Generic 0:d7cc9bad311b 658 hours++;
Generic 0:d7cc9bad311b 659 }
Generic 0:d7cc9bad311b 660 }
Generic 0:d7cc9bad311b 661
Generic 0:d7cc9bad311b 662 }
Generic 0:d7cc9bad311b 663