BA / Mbed OS BaBoRo1

Erste version der Software für der Prototyp

Controller.cpp@4:75df35ef4fb6, 2018-03-30 (annotated)

Committer:
borlanic
Date:
Fri Mar 30 14:07:05 2018 +0000
Revision:
4:75df35ef4fb6
Parent:
3:27100cbaaa6e 
commentar

Who changed what in which revision?

UserRevisionLine numberNew contents of line
borlanic 0:380207fcb5c1 1 /*
borlanic 0:380207fcb5c1 2 * Controller.cpp
borlanic 0:380207fcb5c1 3 * Copyright (c) 2018, ZHAW
borlanic 0:380207fcb5c1 4 * All rights reserved.
borlanic 0:380207fcb5c1 5 *
borlanic 0:380207fcb5c1 6 * Created on: 27.03.2018
borlanic 0:380207fcb5c1 7 * Author: BaBoRo Development Team
borlanic 0:380207fcb5c1 8 */
borlanic 0:380207fcb5c1 9
borlanic 0:380207fcb5c1 10 #include <cmath>
borlanic 0:380207fcb5c1 11 #include "Controller.h"
borlanic 0:380207fcb5c1 12
borlanic 0:380207fcb5c1 13 using namespace std;
borlanic 0:380207fcb5c1 14
borlanic 0:380207fcb5c1 15 const float Controller::PERIOD = 0.001f; // the period of the timer interrupt, given in [s]
borlanic 3:27100cbaaa6e 16 const float Controller::ALPHA = 0.785398163397448f; // alpha = 45° in [rad]
borlanic 0:380207fcb5c1 17 const float Controller::RB = 0.095f; // Ball Radius in [m]
borlanic 0:380207fcb5c1 18 const float Controller::RW = 0.024f; // Wheel Radius in [m]
borlanic 0:380207fcb5c1 19 const float Controller::PI = 3.141592653589793f; // PI
borlanic 0:380207fcb5c1 20 const float Controller::SQRT_3 = 1.732050807568877f; // Square root of 3
borlanic 0:380207fcb5c1 21 const float Controller::LOWPASS_FILTER_FREQUENCY = 300.0f; // frequency of lowpass filter for actual speed values, given in [rad/s]
borlanic 0:380207fcb5c1 22 const float Controller::COUNTS_PER_TURN = 1024.0f;
borlanic 3:27100cbaaa6e 23 const float Controller::KI = 58.5f; // torque constant [mNm/A]
borlanic 3:27100cbaaa6e 24 const float Controller::MIN_DUTY_CYCLE = 0.1f; // minimum allowed value for duty cycle (10%)
borlanic 3:27100cbaaa6e 25 const float Controller::MAX_DUTY_CYCLE = 0.9f; // maximum allowed value for duty cycle (90%)
borlanic 0:380207fcb5c1 26
borlanic 0:380207fcb5c1 27 /**
borlanic 0:380207fcb5c1 28 * Create and initialize a robot controller object.
borlanic 0:380207fcb5c1 29 * @param pwm0 a pwm output object to set the duty cycle for the first motor.
borlanic 0:380207fcb5c1 30 * @param pwm1 a pwm output object to set the duty cycle for the second motor.
borlanic 0:380207fcb5c1 31 * @param pwm2 a pwm output object to set the duty cycle for the third motor.
borlanic 4:75df35ef4fb6 32 * @param counter1
borlanic 0:380207fcb5c1 33 */
borlanic 1:d5c5bb30ac90 34 Controller::Controller(PwmOut& pwm0, PwmOut& pwm1, PwmOut& pwm2, EncoderCounter& counter1, EncoderCounter& counter2, EncoderCounter& counter3, IMU& imu) : pwm0(pwm0), pwm1(pwm1), pwm2(pwm2), counter1(counter1), counter2(counter2), counter3(counter3), imu(imu), thread(osPriorityHigh, STACK_SIZE)
borlanic 1:d5c5bb30ac90 35 {
borlanic 1:d5c5bb30ac90 36
borlanic 0:380207fcb5c1 37 // initialize local values
borlanic 1:d5c5bb30ac90 38
borlanic 0:380207fcb5c1 39 pwm0.period(0.002f);
borlanic 0:380207fcb5c1 40 pwm0.write(0.5f);
borlanic 1:d5c5bb30ac90 41
borlanic 0:380207fcb5c1 42 pwm1.period(0.002f);
borlanic 0:380207fcb5c1 43 pwm1.write(0.5f);
borlanic 1:d5c5bb30ac90 44
borlanic 0:380207fcb5c1 45 pwm2.period(0.002f);
borlanic 0:380207fcb5c1 46 pwm2.write(0.5f);
borlanic 1:d5c5bb30ac90 47
borlanic 0:380207fcb5c1 48 gammaX = imu.getGammaX();
borlanic 0:380207fcb5c1 49 gammaY = imu.getGammaY();
borlanic 0:380207fcb5c1 50 gammaZ = imu.getGammaZ();
borlanic 1:d5c5bb30ac90 51
borlanic 0:380207fcb5c1 52 phiX = 0.0f;
borlanic 0:380207fcb5c1 53 phiY = 0.0f;
borlanic 1:d5c5bb30ac90 54
borlanic 0:380207fcb5c1 55 x = 0.0f;
borlanic 0:380207fcb5c1 56 y = 0.0f;
borlanic 1:d5c5bb30ac90 57
borlanic 0:380207fcb5c1 58 float previousValueCounter1 = counter1.read();
borlanic 0:380207fcb5c1 59 float previousValueCounter2 = counter2.read();
borlanic 0:380207fcb5c1 60 float previousValueCounter3 = counter3.read();
borlanic 1:d5c5bb30ac90 61
borlanic 0:380207fcb5c1 62 speedFilter1.setPeriod(PERIOD);
borlanic 0:380207fcb5c1 63 speedFilter1.setFrequency(LOWPASS_FILTER_FREQUENCY);
borlanic 1:d5c5bb30ac90 64
borlanic 0:380207fcb5c1 65 speedFilter2.setPeriod(PERIOD);
borlanic 0:380207fcb5c1 66 speedFilter2.setFrequency(LOWPASS_FILTER_FREQUENCY);
borlanic 1:d5c5bb30ac90 67
borlanic 0:380207fcb5c1 68 speedFilter3.setPeriod(PERIOD);
borlanic 0:380207fcb5c1 69 speedFilter3.setFrequency(LOWPASS_FILTER_FREQUENCY);
borlanic 1:d5c5bb30ac90 70
borlanic 0:380207fcb5c1 71 previousValueCounter1 = 0.0f;
borlanic 0:380207fcb5c1 72 previousValueCounter2 = 0.0f;
borlanic 0:380207fcb5c1 73 previousValueCounter3 = 0.0f;
borlanic 1:d5c5bb30ac90 74
borlanic 0:380207fcb5c1 75 // start thread and timer interrupt
borlanic 1:d5c5bb30ac90 76
borlanic 2:3f836b662104 77 thread.start(callback(this, &Controller::run));
borlanic 0:380207fcb5c1 78 //ticker.attach(callback(this, &Controller::sendSignal), PERIOD);
borlanic 0:380207fcb5c1 79 }
borlanic 0:380207fcb5c1 80
borlanic 0:380207fcb5c1 81 /**
borlanic 0:380207fcb5c1 82 * Delete the robot controller object and release all allocated resources.
borlanic 0:380207fcb5c1 83 */
borlanic 1:d5c5bb30ac90 84 Controller::~Controller()
borlanic 1:d5c5bb30ac90 85 {
borlanic 1:d5c5bb30ac90 86
borlanic 0:380207fcb5c1 87 //ticker.detach();
borlanic 0:380207fcb5c1 88 }
borlanic 0:380207fcb5c1 89
borlanic 0:380207fcb5c1 90 // set --------------------------------
borlanic 1:d5c5bb30ac90 91 void Controller::setGammaX(float gammaX)
borlanic 1:d5c5bb30ac90 92 {
borlanic 1:d5c5bb30ac90 93
borlanic 0:380207fcb5c1 94 this->gammaX = gammaX;
borlanic 0:380207fcb5c1 95 }
borlanic 0:380207fcb5c1 96
borlanic 0:380207fcb5c1 97
borlanic 1:d5c5bb30ac90 98 void Controller::setGammaY(float gammaY)
borlanic 1:d5c5bb30ac90 99 {
borlanic 1:d5c5bb30ac90 100
borlanic 0:380207fcb5c1 101 this->gammaY = gammaY;
borlanic 0:380207fcb5c1 102 }
borlanic 0:380207fcb5c1 103
borlanic 0:380207fcb5c1 104
borlanic 1:d5c5bb30ac90 105 void Controller::setGammaZ(float gammaZ)
borlanic 1:d5c5bb30ac90 106 {
borlanic 1:d5c5bb30ac90 107
borlanic 0:380207fcb5c1 108 this->gammaZ = gammaZ;
borlanic 0:380207fcb5c1 109 }
borlanic 0:380207fcb5c1 110
borlanic 0:380207fcb5c1 111
borlanic 1:d5c5bb30ac90 112 void Controller::setPhiX(float phiX)
borlanic 1:d5c5bb30ac90 113 {
borlanic 1:d5c5bb30ac90 114
borlanic 0:380207fcb5c1 115 this->phiX = phiX;
borlanic 0:380207fcb5c1 116 }
borlanic 0:380207fcb5c1 117
borlanic 1:d5c5bb30ac90 118 void Controller::setPhiY(float phiY)
borlanic 1:d5c5bb30ac90 119 {
borlanic 1:d5c5bb30ac90 120
borlanic 0:380207fcb5c1 121 this->phiY = phiY;
borlanic 0:380207fcb5c1 122 }
borlanic 0:380207fcb5c1 123
borlanic 1:d5c5bb30ac90 124 void Controller::setX(float x)
borlanic 1:d5c5bb30ac90 125 {
borlanic 1:d5c5bb30ac90 126
borlanic 0:380207fcb5c1 127 this->x = x;
borlanic 0:380207fcb5c1 128 }
borlanic 0:380207fcb5c1 129
borlanic 0:380207fcb5c1 130 /**
borlanic 0:380207fcb5c1 131 * Sets the actual y coordinate of the robots position.
borlanic 0:380207fcb5c1 132 * @param y the y coordinate of the position, given in [m].
borlanic 0:380207fcb5c1 133 */
borlanic 1:d5c5bb30ac90 134 void Controller::setY(float y)
borlanic 1:d5c5bb30ac90 135 {
borlanic 1:d5c5bb30ac90 136
borlanic 0:380207fcb5c1 137 this->y = y;
borlanic 0:380207fcb5c1 138 }
borlanic 0:380207fcb5c1 139
borlanic 0:380207fcb5c1 140 // get --------------------------------
borlanic 0:380207fcb5c1 141
borlanic 1:d5c5bb30ac90 142 float Controller::getPhiX()
borlanic 1:d5c5bb30ac90 143 {
borlanic 0:380207fcb5c1 144
borlanic 0:380207fcb5c1 145 return phiX;
borlanic 0:380207fcb5c1 146 }
borlanic 0:380207fcb5c1 147
borlanic 1:d5c5bb30ac90 148 float Controller::getPhiY()
borlanic 1:d5c5bb30ac90 149 {
borlanic 1:d5c5bb30ac90 150
borlanic 0:380207fcb5c1 151 return phiY;
borlanic 0:380207fcb5c1 152 }
borlanic 0:380207fcb5c1 153
borlanic 0:380207fcb5c1 154 /**
borlanic 0:380207fcb5c1 155 * Gets the actual x coordinate of the robots position.
borlanic 0:380207fcb5c1 156 * @return the x coordinate of the position, given in [m].
borlanic 0:380207fcb5c1 157 */
borlanic 1:d5c5bb30ac90 158 float Controller::getX()
borlanic 1:d5c5bb30ac90 159 {
borlanic 1:d5c5bb30ac90 160
borlanic 0:380207fcb5c1 161 return x;
borlanic 0:380207fcb5c1 162 }
borlanic 0:380207fcb5c1 163
borlanic 0:380207fcb5c1 164 /**
borlanic 0:380207fcb5c1 165 * Gets the actual y coordinate of the robots position.
borlanic 0:380207fcb5c1 166 * @return the y coordinate of the position, given in [m].
borlanic 0:380207fcb5c1 167 */
borlanic 1:d5c5bb30ac90 168 float Controller::getY()
borlanic 1:d5c5bb30ac90 169 {
borlanic 1:d5c5bb30ac90 170
borlanic 0:380207fcb5c1 171 return y;
borlanic 0:380207fcb5c1 172 }
borlanic 0:380207fcb5c1 173
borlanic 0:380207fcb5c1 174 /**
borlanic 0:380207fcb5c1 175 * This method is called by the ticker timer interrupt service routine.
borlanic 0:380207fcb5c1 176 * It sends a signal to the thread to make it run again.
borlanic 0:380207fcb5c1 177 */
borlanic 0:380207fcb5c1 178 //void Controller::sendSignal() {
borlanic 1:d5c5bb30ac90 179
borlanic 0:380207fcb5c1 180 // thread.signal_set(signal);
borlanic 0:380207fcb5c1 181 //}
borlanic 0:380207fcb5c1 182
borlanic 0:380207fcb5c1 183 /**
borlanic 0:380207fcb5c1 184 * This <code>run()</code> method contains an infinite loop with the run logic.
borlanic 0:380207fcb5c1 185 */
borlanic 1:d5c5bb30ac90 186 void Controller::run()
borlanic 1:d5c5bb30ac90 187 {
borlanic 1:d5c5bb30ac90 188
borlanic 0:380207fcb5c1 189 float integratedGammaX = 0.0f;
borlanic 0:380207fcb5c1 190 float integratedGammaY = 0.0f;
borlanic 0:380207fcb5c1 191 float integratedGammaZ = 0.0f;
borlanic 0:380207fcb5c1 192 float integratedPhiX = 0.0f;
borlanic 0:380207fcb5c1 193 float integratedPhiY = 0.0f;
borlanic 1:d5c5bb30ac90 194
borlanic 0:380207fcb5c1 195 float previousGammaX = 0.0;
borlanic 0:380207fcb5c1 196 float previousGammaY = 0.0;
borlanic 0:380207fcb5c1 197 float previousGammaZ = 0.0;
borlanic 0:380207fcb5c1 198 float previousPhiX = 0.0;
borlanic 0:380207fcb5c1 199 float previousPhiY = 0.0;
borlanic 1:d5c5bb30ac90 200
borlanic 1:d5c5bb30ac90 201 // K matrix
borlanic 1:d5c5bb30ac90 202 float K[3][15] = {
borlanic 1:d5c5bb30ac90 203 {0.1010, 0.0000, 2.4661, 0.0000, 0.0639, 0.0621, 0.0000, 0.3066, 0.0000, 0.0237, 0.0816, 0.0000, 0.0000, 0.0000, 0.0577},
borlanic 1:d5c5bb30ac90 204 {-0.0505, 0.0875, -1.2330, 2.1357, 0.0639, -0.0311, 0.0538, -0.1533, 0.2655, 0.0237, -0.0408, 0.0707, 0.0000, 0.0000, 0.0577},
borlanic 1:d5c5bb30ac90 205 {-0.0505, -0.0875, -1.2330, -2.1357, 0.0639, -0.0311, -0.0538, -0.1533, -0.2655, 0.0237, -0.0408, -0.0707, 0.0000, 0.0000, 0.0577}
borlanic 1:d5c5bb30ac90 206 };
borlanic 1:d5c5bb30ac90 207
borlanic 1:d5c5bb30ac90 208
borlanic 0:380207fcb5c1 209 while (true) {
borlanic 1:d5c5bb30ac90 210
borlanic 0:380207fcb5c1 211 gammaX = imu.getGammaX();
borlanic 0:380207fcb5c1 212 gammaY = imu.getGammaY();
borlanic 0:380207fcb5c1 213 gammaZ = imu.getGammaZ();
borlanic 0:380207fcb5c1 214 d_gammaX = imu.getDGammaX();
borlanic 0:380207fcb5c1 215 d_gammaY = imu.getDGammaY();
borlanic 0:380207fcb5c1 216 d_gammaZ = imu.getDGammaZ();
borlanic 1:d5c5bb30ac90 217
borlanic 0:380207fcb5c1 218 // wait for the periodic signal
borlanic 1:d5c5bb30ac90 219
borlanic 0:380207fcb5c1 220 // thread.signal_wait(signal);
borlanic 1:d5c5bb30ac90 221
borlanic 0:380207fcb5c1 222 // Read encoder data
borlanic 0:380207fcb5c1 223 float w1 = counter1.read()/1024.0f *1/(2*PI); // Angle wheel 1
borlanic 0:380207fcb5c1 224 float w2 = counter2.read()/1024.0f *1/(2*PI); // Angle wheel 2
borlanic 0:380207fcb5c1 225 float w3 = counter3.read()/1024.0f *1/(2*PI); // Angle wheel 3
borlanic 1:d5c5bb30ac90 226
borlanic 0:380207fcb5c1 227 // Calculate Ball angle in [rad] using the kinematic model
borlanic 0:380207fcb5c1 228 this->phiX = ((w1 - w2 - w3) - sin(ALPHA)*RB/RW*gammaZ)/(2*cos(ALPHA))+gammaX;
borlanic 0:380207fcb5c1 229 this->phiY = (w2 - w3)/(SQRT_3*cos(ALPHA))+gammaY;
borlanic 1:d5c5bb30ac90 230
borlanic 0:380207fcb5c1 231 // Integration states
borlanic 0:380207fcb5c1 232 integratedGammaX = integratedGammaX + (gammaX-previousGammaX)*PERIOD;
borlanic 0:380207fcb5c1 233 integratedGammaY = integratedGammaY + (gammaY-previousGammaY)*PERIOD;
borlanic 0:380207fcb5c1 234 integratedGammaZ = integratedGammaZ + (gammaZ-previousGammaZ)*PERIOD;
borlanic 0:380207fcb5c1 235 integratedPhiX = integratedPhiX + (phiX-previousPhiX)*PERIOD;
borlanic 0:380207fcb5c1 236 integratedPhiY = integratedPhiY + (phiY-previousPhiY)*PERIOD;
borlanic 1:d5c5bb30ac90 237
borlanic 0:380207fcb5c1 238 // Calculate Ball Velocities
borlanic 0:380207fcb5c1 239 short valueCounter1 = counter1.read();
borlanic 0:380207fcb5c1 240 short valueCounter2 = counter2.read();
borlanic 0:380207fcb5c1 241 short valueCounter3 = counter3.read();
borlanic 1:d5c5bb30ac90 242
borlanic 0:380207fcb5c1 243 short countsInPastPeriod1 = valueCounter1-previousValueCounter1;
borlanic 0:380207fcb5c1 244 short countsInPastPeriod2 = valueCounter2-previousValueCounter2;
borlanic 0:380207fcb5c1 245 short countsInPastPeriod3 = valueCounter3-previousValueCounter3;
borlanic 1:d5c5bb30ac90 246
borlanic 0:380207fcb5c1 247 previousValueCounter1 = valueCounter1;
borlanic 0:380207fcb5c1 248 previousValueCounter2 = valueCounter2;
borlanic 0:380207fcb5c1 249 previousValueCounter3 = valueCounter3;
borlanic 1:d5c5bb30ac90 250
borlanic 0:380207fcb5c1 251 actualSpeed1 = speedFilter1.filter((float)countsInPastPeriod1/COUNTS_PER_TURN/PERIOD/(2*PI)); // actual speed motor1 in [rad/s]
borlanic 0:380207fcb5c1 252 actualSpeed2 = speedFilter2.filter((float)countsInPastPeriod2/COUNTS_PER_TURN/PERIOD/(2*PI)); // actual speed motor1 in [rad/s]
borlanic 0:380207fcb5c1 253 actualSpeed3 = speedFilter3.filter((float)countsInPastPeriod3/COUNTS_PER_TURN/PERIOD/(2*PI)); // actual speed motor1 in [rad/s]
borlanic 1:d5c5bb30ac90 254
borlanic 0:380207fcb5c1 255 float d_phiX = ((actualSpeed1 - actualSpeed2 - actualSpeed3) - sin(ALPHA)*RB/RW*d_gammaZ)/(2*cos(ALPHA))+d_gammaX;
borlanic 0:380207fcb5c1 256 float d_phiY = (actualSpeed2 - actualSpeed3)/(SQRT_3*cos(ALPHA))+d_gammaY;
borlanic 1:d5c5bb30ac90 257
borlanic 0:380207fcb5c1 258 // Calculate Torque motors
borlanic 1:d5c5bb30ac90 259
borlanic 1:d5c5bb30ac90 260 //[M1,M2,M3]=K*x
borlanic 1:d5c5bb30ac90 261
borlanic 1:d5c5bb30ac90 262 float M1 = 0.0f;
borlanic 1:d5c5bb30ac90 263 float M2 = 0.0f;
borlanic 1:d5c5bb30ac90 264 float M3 = 0.0f;
borlanic 1:d5c5bb30ac90 265
borlanic 1:d5c5bb30ac90 266 float x[15][1] = {
borlanic 1:d5c5bb30ac90 267 {gammaX},{gammaY},{gammaZ},{phiX},{phiY},{d_gammaX},{d_gammaY},{d_gammaZ},{d_phiX},{d_phiY},{integratedGammaX},{integratedGammaY},{integratedGammaZ},{integratedPhiX},{integratedPhiY}
borlanic 1:d5c5bb30ac90 268 };
borlanic 1:d5c5bb30ac90 269
borlanic 1:d5c5bb30ac90 270 for(int i=0; i<14; i++) {
borlanic 1:d5c5bb30ac90 271 M1 = M1 + K[0][i]*x[i][1];
borlanic 1:d5c5bb30ac90 272 M2 = M2 + K[1][i]*x[i][1];
borlanic 1:d5c5bb30ac90 273 M3 = M3 + K[2][i]*x[i][1];
borlanic 1:d5c5bb30ac90 274 };
borlanic 1:d5c5bb30ac90 275
borlanic 3:27100cbaaa6e 276 // Calculate duty cycles from desired Torque, limit and set duty cycles
borlanic 3:27100cbaaa6e 277 float I1 = M1*1000.0f/KI;
borlanic 3:27100cbaaa6e 278 float I2 = M2*1000.0f/KI;
borlanic 3:27100cbaaa6e 279 float I3 = M3*1000.0f/KI;
borlanic 3:27100cbaaa6e 280
borlanic 3:27100cbaaa6e 281 float dutyCycle1 = 0.5f/6.0f*I1 + 0.5f;
borlanic 3:27100cbaaa6e 282 if (dutyCycle1 < MIN_DUTY_CYCLE) dutyCycle1 = MIN_DUTY_CYCLE;
borlanic 3:27100cbaaa6e 283 else if (dutyCycle1 > MAX_DUTY_CYCLE) dutyCycle1 = MAX_DUTY_CYCLE;
borlanic 3:27100cbaaa6e 284 //pwm0.write(dutyCycle1);
borlanic 1:d5c5bb30ac90 285
borlanic 3:27100cbaaa6e 286 float dutyCycle2 = 0.5f/6.0f*I2 + 0.5f;
borlanic 3:27100cbaaa6e 287 if (dutyCycle2 < MIN_DUTY_CYCLE) dutyCycle2 = MIN_DUTY_CYCLE;
borlanic 3:27100cbaaa6e 288 else if (dutyCycle2 > MAX_DUTY_CYCLE) dutyCycle2 = MAX_DUTY_CYCLE;
borlanic 3:27100cbaaa6e 289 //pwm1.write(dutyCycle2);
borlanic 1:d5c5bb30ac90 290
borlanic 3:27100cbaaa6e 291 float dutyCycle3 = 0.5f/6.0f*I3 + 0.5f;
borlanic 3:27100cbaaa6e 292 if (dutyCycle3 < MIN_DUTY_CYCLE) dutyCycle3 = MIN_DUTY_CYCLE;
borlanic 3:27100cbaaa6e 293 else if (dutyCycle3 > MAX_DUTY_CYCLE) dutyCycle3 = MAX_DUTY_CYCLE;
borlanic 3:27100cbaaa6e 294 //pwm2.write(dutyCycle3);
borlanic 3:27100cbaaa6e 295
borlanic 0:380207fcb5c1 296 // actual robot pose
borlanic 1:d5c5bb30ac90 297 this->x = phiY*RB;
borlanic 1:d5c5bb30ac90 298 this->y = phiX*RB;
borlanic 0:380207fcb5c1 299
borlanic 0:380207fcb5c1 300 // set new gamma's, phi's
borlanic 0:380207fcb5c1 301 previousGammaX = gammaX;
borlanic 0:380207fcb5c1 302 previousGammaY = gammaY;
borlanic 0:380207fcb5c1 303 previousGammaZ = gammaZ;
borlanic 0:380207fcb5c1 304 previousPhiX = phiX;
borlanic 0:380207fcb5c1 305 previousPhiY = phiY;
borlanic 0:380207fcb5c1 306 }
borlanic 0:380207fcb5c1 307 };