config.cpp

00001 #include "mbed.h"
00002 #include "I2cPeripherals.h"
00003 #include "InterruptIn.h"
00004 #include "config.h"
00005 #include "PulseWidthCounter.h"
00006 #include "SerialLcd.h"
00007 #include "Limiter.h"
00008 //#include "PID.h"
00009 
00010 //Serial pc(USBTX, USBRX); 
00011 
00012 enum DispNum {  CALIBURATE=1,
00013                 GYROGAIN,
00014                 GYRODIR,
00015                 SERVODIR,
00016 //                ACCELGAIN,
00017                 ACCELCORRECT,
00018 //                PIDSET,
00019 //                PIDHEIGHT,
00020 //                GIMBAL,
00021                 STICKMIX,
00022                 DISPPULSE,
00023                 DISPSENSOR,
00024                 DISPPWM,
00025                 PARMSET,
00026                 CONFSTORE,
00027                 CONFRESET,
00028                 FINAL   };
00029 
00030 void FlashLED(int,float tm=0.1);
00031 char Check_Stick_Dir(char);
00032 void Param_Set_Prompt1(char *,int *,int,int,int,int,char);
00033 void Param_Set_Prompt1(char *,float *,int,float,float,float,char);
00034 void Set_Arrow(int dir);
00035 void Get_Stick_Pos();
00036 void CalibrateGyros(void);
00037 void CalibrateAccel(void);
00038 void Get_Gyro();
00039 void Get_Accel();
00040 void Get_Angle(float);
00041 void PWM_Out(bool);
00042 void WriteConfig();
00043 void ESC_SetUp(void);
00044 void Get_Pressure();
00045 void LCD_printf(char *);
00046 void LCD_cls();
00047 void LCD_locate(int,int);
00048 void wait(float);
00049 
00050 Timer elaps;
00051 
00052 extern volatile int CH[9];
00053 extern volatile int M[6];
00054 extern volatile float Gyro[3];
00055 extern volatile float Accel[3];
00056 extern volatile float Accel_Save[3];
00057 extern volatile float Accel_Angle[3];
00058 extern volatile float Angle[3];
00059 extern volatile float Gyro_Ref[3];
00060 extern volatile int Stick[6];
00061 extern volatile float Press;
00062 extern volatile float interval;
00063 extern Limiter throLimit;
00064 //extern bool tick_flag;
00065 //extern PID pid[4];
00066 //extern PID height;
00067 //extern int pid_reg[4];
00068 const char steering[3][6]= {"Roll ","Pitch","Yaw  "};
00069 const char ModelName[6][9] = { "Quad-X  ","Quad-VP ","Quad-3D ","Delta   ","Delta-TW","AirPlane" };    
00070 int mode;//
00071 char sw,ret_mode;
00072 int vnum,hnum,vmax,hmax;//
00073 int idx,i;//
00074 char str[33];
00075 config init;
00076 
00077 void SetUpPrompt(config& conf,I2cPeripherals& i2c)
00078 {
00079     float x,y,z;
00080     LCD_cls();
00081     mode = 0;
00082     vnum = 0;
00083     hnum = 0;
00084     vmax = FINAL - 1;
00085 
00086     while( 1 ) {
00087 //        FlashLED(1);
00088         ret_mode = 'W';
00089         mode = vnum * 10 + hnum;
00090 
00091         switch ( mode ) {
00092             
00093             //初期画面
00094             case 0:
00095                 LCD_locate(0,0);
00096                 LCD_printf( (char*)ModelName[conf.Model_Type] );
00097                 LCD_locate(8,0);
00098                 sprintf(str,"Ver %4.2f",conf.Revision);
00099                 LCD_printf(str);
00100                 LCD_locate(4,1);
00101                 LCD_printf("By AZUKITEN");
00102                 hmax = 1;
00103                 break;
00104             case 1:         //モデルタイプの設定
00105                 LCD_locate(0,0);
00106                 LCD_printf("Model Type");
00107                 LCD_locate(0,1);
00108                 switch ( sw ) {
00109                     case 'D':
00110                         if ( conf.Model_Type > 0 ) conf.Model_Type -= 1;
00111                         else conf.Model_Type = 4;
00112                         break;
00113                     case 'U':
00114                         if ( conf.Model_Type < 5 ) conf.Model_Type += 1;
00115                         else conf.Model_Type = 0;
00116                 }
00117                 LCD_printf( (char*)ModelName[conf.Model_Type] );
00118                 Set_Arrow(2);
00119                 break;
00120 
00121             //送信機信号のキャリブレーション
00122             case CALIBURATE*10:        //Calibrate Transmitter
00123                 LCD_printf("Calibrate");
00124                 Set_Arrow(1);
00125                 hmax = 1;
00126                 break;
00127             case CALIBURATE*10+1:        //Calibrate Transmitter
00128                 LCD_printf("Start Calibrate");
00129                 wait(1);
00130                 for(i=0; i<4; i++)  {
00131                     conf.Stick_Ref[i] = 0;
00132                 }
00133                 for(i=0; i<16; i++) {
00134                     wait(0.03);
00135                     Get_Stick_Pos();                   
00136                     conf.Stick_Ref[ROL] += AIL;
00137                     conf.Stick_Ref[PIT] += ELE;
00138                     conf.Stick_Ref[YAW] += RUD;
00139                     conf.Stick_Ref[COL] += THR;
00140 //                    conf.Stick_Ref[GAIN] += AUX;
00141                 }
00142                 for(i=0; i<4; i++) {
00143                     conf.Stick_Ref[i] = conf.Stick_Ref[i]/16;
00144                 }
00145 
00146                 CalibrateGyros();
00147                 CalibrateAccel();
00148                 LCD_cls();                 //Clear LCD
00149                 LCD_printf("Calibrate Completed");
00150                 Set_Arrow(3);
00151                 FlashLED(5);
00152                 hnum = 0;
00153                 break;
00154 
00155             //ジャイロ感度の設定
00156             case GYROGAIN*10:        //Set Gyro Gain
00157                 LCD_printf("Set Gyro Gain");
00158                 Set_Arrow(1);
00159                 hmax = 5;
00160                 break;
00161             case GYROGAIN*10+1:                                //Set Gyro Gain Roll
00162                 if ( conf.Gyro_Gain_Setting == 1 )
00163                     Param_Set_Prompt1("GyroGain>Roll",&conf.Gyro_Gain[0],2,0.00f,1.00f,0.01f,sw);
00164                 else
00165                     Param_Set_Prompt1("GyroGain>Roll",&conf.Gyro_Gain[3],2,-1.00f,1.00f,0.01f,sw);
00166                 break;
00167             case GYROGAIN*10+2:
00168                 if ( conf.Gyro_Gain_Setting == 1 )
00169                     Param_Set_Prompt1("GyroGain>Pitch",&conf.Gyro_Gain[1],2,0.00f,1.00f,0.01f,sw);
00170                 else
00171                     Param_Set_Prompt1("GyroGain>Pitch",&conf.Gyro_Gain[4],2,-1.00f,1.00f,0.01f,sw);
00172                 break;
00173             case GYROGAIN*10+3:
00174                 if ( conf.Gyro_Gain_Setting == 1 )
00175                     Param_Set_Prompt1("GyroGain>Yaw",&conf.Gyro_Gain[2],2,0.00f,1.00f,0.01f,sw);
00176                 else
00177                     Param_Set_Prompt1("GyroGain>Yaw",&conf.Gyro_Gain[5],2,-1.00f,1.00f,0.01f,sw);
00178                 break;
00179             case GYROGAIN*10+4:
00180                 Param_Set_Prompt1("Active Gyro Gain",&conf.Active_Gyro_Gain,3,0.0f,1.0f,0.01f,sw);
00181                 break;
00182             case GYROGAIN*10+5:
00183 //                ret_mode = 'R';
00184                 LCD_printf("GyroGain>setting");
00185                 LCD_locate(0,1);
00186                 switch ( sw ) {
00187                     case 'U':
00188                     case 'D':
00189                         conf.Gyro_Gain_Setting *= -1;
00190                 }
00191                 if ( conf.Gyro_Gain_Setting == 1 )
00192                     LCD_printf("Controller");
00193                 else
00194                     LCD_printf("Transmitter");
00195                 Set_Arrow(3);
00196                 break;
00197                 
00198             //ジャイロの効きの逆転                
00199             case GYRODIR*10:        //Set Gyro Direction
00200                 LCD_printf("Gyro Direction");
00201                 Set_Arrow(1);
00202                 hmax = 4;
00203                 break;
00204             case GYRODIR*10+1:                                //Set Gyro Direction Roll
00205             case GYRODIR*10+2:
00206             case GYRODIR*10+3:
00207             case GYRODIR*10+4:          //ｘｙ軸の入れ替え
00208 //              ret_mode = 'R';
00209                 idx = mode - (GYRODIR*10+1);
00210                 if ( mode == (GYRODIR*10+4) )
00211                     LCD_printf("Gyro>Swap X-Y");
00212                 else {
00213                     LCD_printf("Gyro>Dir>");
00214                     LCD_locate(9,0);
00215                     LCD_printf((char*)steering[idx]);
00216                 }
00217                 LCD_locate(0,1);
00218                 switch ( sw ) {
00219                     case 'U':
00220                     case 'D':
00221                         conf.Gyro_Dir[idx] *= -1;
00222                 }
00223                 if ( conf.Gyro_Dir[idx] == 1 )
00224                     LCD_printf("Normal ");
00225                 else
00226                     LCD_printf("Reverse");
00227                 if ( mode == (GYRODIR*10+4) )
00228                     Set_Arrow(3);
00229                 else
00230                     Set_Arrow(2);
00231                 break;
00232 
00233             //サーボの向きの逆転
00234             case SERVODIR*10:        //Set Servo Direction
00235                 LCD_printf("Servo Direction");
00236                 Set_Arrow(1);
00237                 hmax = 6;
00238                 break;
00239             case SERVODIR*10+1:                                //Set Gyro Direction Roll
00240             case SERVODIR*10+2:
00241             case SERVODIR*10+3:
00242             case SERVODIR*10+4:
00243             case SERVODIR*10+5:
00244             case SERVODIR*10+6:
00245 //              ret_mode = 'R';
00246                 idx = mode - (SERVODIR*10+1);
00247                 sprintf(str,"Servo>Dir>M%d",idx+1);
00248                 LCD_printf(str);
00249                 LCD_locate(0,1);
00250                 switch ( sw ) {
00251                     case 'U':
00252                     case 'D':
00253                         conf.Servo_Dir[idx] *= -1;
00254                 }
00255                 if ( conf.Servo_Dir[idx] == 1 )
00256                     LCD_printf("Normal ");
00257                 else
00258                     LCD_printf("Reverse");
00259                 if ( mode == (SERVODIR*10+4) )
00260                     Set_Arrow(3);
00261                 else
00262                     Set_Arrow(2);
00263                 break;
00264 
00265             //加速度計の水平レベルの校正
00266             case ACCELCORRECT*10:
00267                 LCD_printf("Acceleration");
00268                 LCD_locate(2,1);
00269                 LCD_printf("Trim");
00270                 Set_Arrow(1);
00271                 hmax = 3;
00272                 break;
00273             case ACCELCORRECT*10+1:
00274                 Param_Set_Prompt1("Accel>Rol",&conf.Accel_Ref[ROL],2,-10.0,10.0f,0.001f,sw);
00275                 break;
00276             case ACCELCORRECT*10+2:
00277                 Param_Set_Prompt1("Accel>Pitch",&conf.Accel_Ref[PIT],2,-10.0,10.0f,0.001f,sw);
00278                 break;
00279             case ACCELCORRECT*10+3:
00280                 Param_Set_Prompt1("Accel>Yaw",&conf.Accel_Ref[YAW],3,-10.0,10.0f,0.001f,sw);
00281                 break;
00282 
00283             //スティック操作量の設定
00284             case STICKMIX*10:        //Set Stick Mixing
00285                 LCD_printf("Set Stick Mixing");
00286                 Set_Arrow(1);
00287                 hmax = 3;
00288                 break;
00289             case STICKMIX*10+1:                                //Set Stick Mixing
00290                 Param_Set_Prompt1("Mixing>Roll",&conf.Stick_Mix[0],2,0.00f,2.00f,0.01f,sw);
00291                 break;
00292             case STICKMIX*10+2:
00293                 Param_Set_Prompt1("Mixing>Pitch",&conf.Stick_Mix[1],2,0.00f,2.00f,0.01f,sw);
00294                 break;
00295             case STICKMIX*10+3:
00296                 Param_Set_Prompt1("Mixing>Yaw",&conf.Stick_Mix[2],3,0.00f,2.00f,0.01f,sw);
00297                 break;
00298 
00299             //送信機パルス長の表示
00300             case DISPPULSE*10:        //Display Pulse Width
00301                 LCD_printf("Disp Pulse Width");
00302                 Set_Arrow(1);
00303                 hmax = 3;
00304                 break;
00305             case DISPPULSE*10+1:        //Display Pulse Width
00306 //           DisplayPulseWidth(THR,AIL,ELE,RUD,AUX);
00307                 ret_mode = 'R';
00308                 LCD_locate(0,0);
00309                 sprintf(str,"TR=%4d,AL=%4d",THR,AIL);
00310                 LCD_printf(str);
00311                 LCD_locate(0,1);
00312                 sprintf(str,"EL=%4d,RD=%4d",ELE,RUD);
00313                 LCD_printf(str);
00314                 break;
00315             case DISPPULSE*10+2:        //Display AUX,AX2
00316                 ret_mode = 'R';
00317                 Get_Stick_Pos();
00318                 LCD_locate(0,0);
00319                 sprintf(str,"A1=%4d,A2=%4d",AUX,AX2);
00320                 LCD_printf(str);
00321                 LCD_locate(0,1);
00322                 sprintf(str,"A3=%4d,A4=%4d",AX3,AX4);
00323                 LCD_printf(str);
00324                 break;
00325             case DISPPULSE*10+3:        //Display Stick Ref
00326                 ret_mode = 'R';
00327                 Get_Stick_Pos();
00328                 LCD_locate(0,0);
00329                 sprintf(str,"TR=%4d,AL=%4d",Stick[COL],Stick[ROL]);
00330                 LCD_printf(str);
00331                 LCD_locate(0,1);
00332                 sprintf(str,"EL=%4d,RD=%4d",Stick[PIT],Stick[YAW]);
00333                 LCD_printf(str);
00334                 break;
00335 
00336             //センサー値の表示
00337             case DISPSENSOR*10:        //Display Sensor Value
00338                 LCD_printf("Disp Sensor");
00339                 Set_Arrow(1);
00340                 hmax = 6;
00341                 Angle[ROL]=Angle[PIT]=Angle[YAW]=0;
00342                 Accel[ROL]=Accel[PIT]=Accel[YAW]=0;
00343                 break;
00344             case DISPSENSOR*10+1:        //Gyro
00345 //                Get_Gyro();
00346                 if ( conf.Gyro_Dir[3] ==1 ) i2c.angular(&x,&y,&z);
00347                 else i2c.angular(&y,&x,&z);
00348                 x -= Gyro_Ref[0];
00349                 y -= Gyro_Ref[1];
00350                 z -= Gyro_Ref[2];
00351                 LCD_locate(0,0);
00352                 sprintf(str,"[Gyro]X=%5.1f",x);
00353                 LCD_printf(str);
00354                 LCD_locate(0,1);
00355                 sprintf(str,"y=%5.1f,Z=%5.1f",y,z);
00356                 LCD_printf(str);
00357                 ret_mode = 'R';
00358                 break;
00359             case DISPSENSOR*10+2:            //Gravity
00360                 if ( conf.Gyro_Dir[3] ==1 ) i2c.Acceleration(&x,&y,&z);
00361                 else i2c.Acceleration(&y,&x,&z);
00362                 x -= conf.Accel_Ref[0];
00363                 y -= conf.Accel_Ref[1];
00364                 z -= conf.Accel_Ref[2];
00365                 LCD_locate(0,0);
00366                 sprintf(str,"[Gravity]X=%5.2f",x);
00367                 LCD_printf(str);
00368                 LCD_locate(0,1);
00369                 sprintf(str,"Y=%5.2f,Z=%5.2f",y,z);
00370                 LCD_printf(str);
00371 //                Set_Arrow(2);
00372                 ret_mode = 'R';
00373                 break;
00374             case DISPSENSOR*10+3:            //angle
00375                 PWM_Out(false);
00376                 LCD_locate(0,0);
00377                 sprintf(str,"[Angle]X=%6.1f",Angle[ROL]);
00378                 LCD_printf(str);
00379                 LCD_locate(0,1);
00380                 sprintf(str,"Y=%6.1f,Z=%5.1f",Angle[PIT],Angle[YAW]);
00381                 LCD_printf(str);
00382 //                Set_Arrow(2);
00383                 ret_mode = 'R';
00384                 break;
00385             case DISPSENSOR*10+4:                // Pressure
00386                 elaps.reset();
00387                 elaps.start();
00388                 Get_Pressure();
00389                 elaps.stop();
00390                 LCD_locate(0,0);
00391                 sprintf(str,"Press=%4.1fhp",Press);
00392                 LCD_printf(str);
00393                 LCD_locate(0,1);
00394                 sprintf(str,"Height=%7.2fcm",i2c.height_cm());
00395                 LCD_printf(str);
00396 //                Set_Arrow(2);
00397                 ret_mode = 'R';
00398                 wait(0.05);
00399                 break;
00400             case DISPSENSOR*10+5:
00401                 elaps.reset();
00402                 elaps.start();
00403                 PWM_Out(false);
00404                 elaps.stop();
00405                 i = elaps.read_us();
00406                 LCD_locate(0,0);
00407                 sprintf(str,"ElapsTime=%6d",i);
00408                 LCD_printf(str);
00409 //                Set_Arrow(2);
00410                 ret_mode = 'R';
00411                 break;
00412             case DISPSENSOR*10+6:        //Sensor Calibration
00413                 CalibrateGyros();
00414                 FlashLED(3);
00415                 LCD_printf("Calibrate completed");
00416                 Set_Arrow(3);
00417                 break;
00418 
00419 
00420             //ＥＳＣ用PWMパルス長の表示
00421             case DISPPWM*10:                                //Display PWM Condition
00422                 LCD_printf("Display PWM ");
00423                 Set_Arrow(1);
00424                 hmax = 2;
00425                 break;
00426             case DISPPWM*10+1:                                //Display PWM Width
00427                 ret_mode = 'R';
00428                 PWM_Out(false);
00429                 LCD_locate(0,0);
00430                 sprintf(str,"M1=%4d,M2=%4d",M1,M2);
00431                 LCD_printf(str);
00432                 LCD_locate(0,1);
00433                 sprintf(str,"M4=%4d,M3=%4d",M4,M3);
00434                 LCD_printf(str);
00435                 break;
00436             case DISPPWM*10+2:                                //Display PWM Width
00437                 ret_mode = 'R';
00438                 PWM_Out(false);
00439                 LCD_locate(0,0);
00440                 sprintf(str,"M5=%4d,M6=%4d",M5,M6);
00441                 LCD_printf(str);
00442                 break;
00443 
00444             //その他パラメータ値の設定
00445             case PARMSET*10:    //パラメーター設定
00446                 LCD_printf("Parameter Set");
00447                 Set_Arrow(1);
00448                 hmax = 8;
00449                 break;
00450             case PARMSET*10+1:
00451                 Param_Set_Prompt1("LCD>Contrast",&conf.LCD_Contrast,2,0,63,1,sw);
00452                 break;
00453             case PARMSET*10+2:
00454                 LCD_locate(0,0);
00455                 LCD_printf("PWM>Mode");
00456                 LCD_locate(0,1);
00457                 switch ( sw ) {
00458                     case 'U':
00459                     case 'D':
00460                         conf.PWM_Mode *= -1;
00461                 }
00462                 if ( conf.PWM_Mode == 1 )
00463                     LCD_printf("ESC  ");
00464                 else
00465                     LCD_printf("Moter");
00466                 Set_Arrow(2);
00467                 break;
00468             case PARMSET*10+3:
00469                 Param_Set_Prompt1("PWM>Interval",&conf.PWM_Interval,2,Thro_Hi,20000,100,sw);
00470                 break;
00471             case PARMSET*10+4:                                
00472                 Param_Set_Prompt1("Gyro>CutoffFreq",&conf.Cutoff_Freq,2,0.00f,10.0f,0.01f,sw);
00473                 break;
00474             case PARMSET*10+5:
00475                 Param_Set_Prompt1("ESC>Throttl Trim",&conf.Throttl_Trim,2,Pulse_Min,Pulse_Max,1,sw);
00476                 break;
00477             case PARMSET*10+6:
00478                 Param_Set_Prompt1("ESC>ReversePoint",&conf.Reverse_Point,2,1000,2000,1,sw);
00479                 break;
00480             case PARMSET*10+7:
00481                 Param_Set_Prompt1("Flight Timer",&conf.Flight_Time,2,0,600,10,sw);
00482                 break;
00483             case PARMSET*10+8:
00484                 Param_Set_Prompt1("Active Gyro Gain",&conf.Active_Gyro_Gain,3,0.0f,1.0f,0.01f,sw);
00485                 break;
00486 
00487             //設定データの保存
00488             case CONFSTORE*10:       //E2PROM Store
00489                 LCD_printf("Config Save");
00490                 Set_Arrow(1);
00491                 hmax = 1;
00492                 break;
00493             case CONFSTORE*10+1:
00494                 WriteConfig();
00495                 LCD_locate(0,0);
00496                 sprintf(str,"Config %3dbyte",sizeof(config));
00497                 LCD_printf(str);
00498                 LCD_locate(0,1);
00499                 LCD_printf("Save Sucssesuful");
00500                 Set_Arrow(3);
00501                 wait(0.5);
00502                 FlashLED(5);
00503                 hnum = 0;
00504                 break;
00505 
00506             //設定データの初期化
00507             case CONFRESET*10:       //E2PROM reset
00508                 LCD_printf("Config Reset");
00509                 Set_Arrow(1);
00510                 hmax = 3;
00511                 break;
00512             case CONFRESET*10+1:
00513                 LCD_printf("Ailron stick");
00514                 LCD_locate(0,1);
00515                 LCD_printf("Move to right");
00516                 Set_Arrow(2);
00517                 break;
00518             case CONFRESET*10+2:       //E2PROM reset
00519                 conf = init;
00520                 LCD_printf("Rset sucssesuful");
00521                 Set_Arrow(3);
00522                 break;
00523             default:
00524                 if ( hnum == 0 )
00525                     vnum++;
00526         }
00527 
00528 
00529         sw = Check_Stick_Dir(ret_mode);             //Wait Mode
00530 
00531         switch ( sw ) {
00532             case 'L':
00533                 hnum--;
00534                 if ( hnum <= 0 ) hnum = 0;
00535                 LCD_cls();                 //Clear LCD
00536                 break;
00537             case 'R':
00538                 LCD_cls();
00539                 if ( hnum < hmax ) hnum++;
00540                 break;
00541             case 'U':
00542                 if ( hnum == 0 ) {
00543                     if ( vnum < vmax ) vnum++;
00544                     else vnum = 0;
00545                     LCD_cls();                 //Clear LCD
00546                 }
00547                 break;
00548             case 'D':
00549                 if ( hnum == 0 ) {
00550                     if ( vnum > 0 ) vnum--;
00551                     else vnum = vmax;
00552                     LCD_cls();                 //Clear LCD
00553                 }
00554                 break;
00555             case 'E':
00556                 while ( conf.Model_Type == Quad_3D && Stick[GAIN] < 0 ) {
00557                     FlashLED(2);
00558                     wait(0.5);
00559                 }
00560                 LCD_cls();                 //Clear LCD
00561                 LCD_locate(0,0);
00562                 LCD_printf("PWM Started");
00563                 return;
00564         }
00565     }
00566 
00567 }
00568 
00569 char Check_Stick_Dir(char act)
00570 {
00571     int i;
00572     while ( 1 ) {
00573         Get_Stick_Pos();
00574         if ( Stick[YAW] > Stick_Limit ) {
00575             i = 0;
00576             while ( Stick[YAW] > Stick_Limit && Stick[COL] < 30 ) {
00577                 if ( i > 2000 ) return 'E';     //wait 2 sec
00578                 wait(0.001);                            // wait 1 msec
00579                 Get_Stick_Pos();
00580                 i++;
00581             }
00582         }
00583         if ( Stick[ROL] > Stick_Limit ) {
00584             wait(0.03);
00585             Get_Stick_Pos();
00586             if ( !(Stick[ROL] > Stick_Limit) ) continue;
00587             while ( Stick[ROL] > Stick_Limit ) {
00588                 Get_Stick_Pos();
00589             }
00590             return 'R';
00591         }
00592         if ( Stick[ROL] < -Stick_Limit ) {
00593             wait(0.03);
00594             Get_Stick_Pos();
00595             if ( !(Stick[ROL] < -Stick_Limit) ) continue;
00596             while ( Stick[ROL] < -Stick_Limit ) {
00597                 Get_Stick_Pos();
00598             }
00599             return 'L';
00600         }
00601         if ( Stick[PIT] < -Stick_Limit ) {
00602             wait(0.03);
00603             Get_Stick_Pos();
00604             if ( !(Stick[PIT] < -Stick_Limit) ) continue;
00605             if ( act == 'R' ) {
00606                 wait(0.03);
00607                 return 'D';
00608             }
00609             while ( Stick[PIT] < -Stick_Limit ) {
00610                 Get_Stick_Pos();
00611             }
00612             return 'D';
00613         }
00614         if ( Stick[PIT] > Stick_Limit ) {
00615             wait(0.03);
00616             Get_Stick_Pos();
00617             if ( !( Stick[PIT] > Stick_Limit) ) continue;
00618             if ( act == 'R' ) {
00619                 wait(0.03);
00620                 return 'U';
00621             }
00622             while ( Stick[PIT] > Stick_Limit ) {
00623                 Get_Stick_Pos();
00624             }
00625             return 'U';
00626         }
00627         if ( act == 'R' )
00628             return ' ';
00629     }
00630 }
00631 
00632 void Param_Set_Prompt1(char *hd,int *num,int arrow,int min,int max,int increase,char sw)
00633 {
00634     ret_mode = 'R';
00635     LCD_locate(0,0);
00636     LCD_printf(hd);
00637     LCD_locate(0,1);
00638     sprintf(str,"%6d",*num);
00639     LCD_printf(str);
00640     Set_Arrow(arrow);
00641     switch ( sw ) {
00642         case 'U':
00643             *num -= increase;
00644             if ( *num <= min )
00645                 *num = min;
00646             break;
00647         case 'D':
00648             *num += increase;
00649             if ( *num >= max )
00650                 *num = max;
00651     }
00652 }
00653 void Param_Set_Prompt1(char *hd,float *num,int arrow,float min,float max,float increase,char sw)
00654 {
00655     ret_mode = 'R';
00656     LCD_locate(0,0);
00657     LCD_printf(hd);
00658     LCD_locate(0,1);
00659     sprintf(str,"%7.3f",*num);
00660     LCD_printf(str);
00661     Set_Arrow(arrow);
00662     switch ( sw ) {
00663         case 'U':
00664             *num -= increase;
00665             if ( *num <= min )
00666                 *num = min;
00667             break;
00668         case 'D':
00669             *num += increase;
00670             if ( *num >= max )
00671                 *num = max;
00672     }
00673 }
00674 
00675 void Set_Arrow(int dir)
00676 {
00677     LCD_locate(12,1);
00678     switch ( dir ) {
00679         case 1:
00680             LCD_printf("  >>");
00681             break;
00682         case 2:
00683             LCD_printf("<<>>");
00684             break;
00685         case 3:
00686             LCD_printf("  <<");
00687     }
00688 };
00689