syouichi imamori / Mbed OS MulticopterQuadX

Quad X Type Multicopter

Dependencies:   IAP

config.cpp@6:a50e6d3924f1, 2015-02-24 (annotated)

Committer:
komaida424
Date:
Tue Feb 24 09:28:29 2015 +0000
Revision:
6:a50e6d3924f1
Parent:
4:4060309b9cc0
Child:
8:1db19b529b22
Release?revision 3.1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
komaida424 0:cca1c4e84da4 1 #include "mbed.h"
komaida424 0:cca1c4e84da4 2 #include "I2cPeripherals.h"
komaida424 0:cca1c4e84da4 3 #include "InterruptIn.h"
komaida424 0:cca1c4e84da4 4 #include "config.h"
komaida424 0:cca1c4e84da4 5 #include "PulseWidthCounter.h"
komaida424 0:cca1c4e84da4 6 #include "SerialLcd.h"
komaida424 6:a50e6d3924f1 7 #include "Limiter.h"
komaida424 4:4060309b9cc0 8 //#include "PID.h"
komaida424 2:59ac9df97701 9
komaida424 2:59ac9df97701 10 //Serial pc(USBTX, USBRX);
komaida424 2:59ac9df97701 11
komaida424 4:4060309b9cc0 12 enum DispNum { CALIBURATE=1,
komaida424 4:4060309b9cc0 13 GYROGAIN,
komaida424 4:4060309b9cc0 14 GYRODIR,
komaida424 4:4060309b9cc0 15 SERVODIR,
komaida424 4:4060309b9cc0 16 // ACCELGAIN,
komaida424 4:4060309b9cc0 17 ACCELCORRECT,
komaida424 4:4060309b9cc0 18 // PIDSET,
komaida424 4:4060309b9cc0 19 // PIDHEIGHT,
komaida424 4:4060309b9cc0 20 // GIMBAL,
komaida424 4:4060309b9cc0 21 STICKMIX,
komaida424 4:4060309b9cc0 22 DISPPULSE,
komaida424 4:4060309b9cc0 23 DISPSENSOR,
komaida424 4:4060309b9cc0 24 DISPPWM,
komaida424 4:4060309b9cc0 25 PARMSET,
komaida424 4:4060309b9cc0 26 CONFSTORE,
komaida424 4:4060309b9cc0 27 CONFRESET,
komaida424 4:4060309b9cc0 28 FINAL };
komaida424 0:cca1c4e84da4 29
komaida424 0:cca1c4e84da4 30 void FlashLED(int);
komaida424 0:cca1c4e84da4 31 char Check_Stick_Dir(char);
komaida424 2:59ac9df97701 32 void Param_Set_Prompt1(char *,int *,int,int,int,int,char);
komaida424 2:59ac9df97701 33 void Param_Set_Prompt1(char *,float *,int,float,float,float,char);
komaida424 0:cca1c4e84da4 34 void Set_Arrow(int dir);
komaida424 0:cca1c4e84da4 35 void Get_Stick_Pos();
komaida424 0:cca1c4e84da4 36 void CalibrateGyros(void);
komaida424 0:cca1c4e84da4 37 void CalibrateAccel(void);
komaida424 0:cca1c4e84da4 38 void Get_Gyro();
komaida424 0:cca1c4e84da4 39 void Get_Accel();
komaida424 2:59ac9df97701 40 void Get_Angle(float);
komaida424 0:cca1c4e84da4 41 void PWM_Out(bool);
komaida424 0:cca1c4e84da4 42 void WriteConfig();
komaida424 0:cca1c4e84da4 43 void ESC_SetUp(void);
komaida424 0:cca1c4e84da4 44 void Get_Pressure();
komaida424 2:59ac9df97701 45 void LCD_printf(char *);
komaida424 2:59ac9df97701 46 void LCD_cls();
komaida424 2:59ac9df97701 47 void LCD_locate(int,int);
komaida424 0:cca1c4e84da4 48
komaida424 0:cca1c4e84da4 49 Timer elaps;
komaida424 0:cca1c4e84da4 50
komaida424 4:4060309b9cc0 51 extern volatile int CH[9];
komaida424 2:59ac9df97701 52 extern volatile int M[6];
komaida424 2:59ac9df97701 53 extern volatile float Gyro[3];
komaida424 2:59ac9df97701 54 extern volatile float Accel[3];
komaida424 4:4060309b9cc0 55 extern volatile float Accel_Save[3];
komaida424 4:4060309b9cc0 56 extern volatile float Accel_Angle[3];
komaida424 2:59ac9df97701 57 extern volatile float Angle[3];
komaida424 2:59ac9df97701 58 extern volatile float Gyro_Ref[3];
komaida424 4:4060309b9cc0 59 extern volatile int Stick[6];
komaida424 2:59ac9df97701 60 extern volatile float Press;
komaida424 2:59ac9df97701 61 extern volatile float interval;
komaida424 6:a50e6d3924f1 62 extern Limiter throLimit;
komaida424 2:59ac9df97701 63 //extern bool tick_flag;
komaida424 4:4060309b9cc0 64 //extern PID pid[4];
komaida424 4:4060309b9cc0 65 //extern PID height;
komaida424 4:4060309b9cc0 66 //extern int pid_reg[4];
komaida424 0:cca1c4e84da4 67 const char steering[3][6]= {"Roll ","Pitch","Yaw "};
komaida424 6:a50e6d3924f1 68 const char ModelName[6][9] = { "Quad-X ","Quad-VP ","Quad-3D ","Delta ","Delta-TW","AirPlane" };
komaida424 4:4060309b9cc0 69 int mode;//
komaida424 0:cca1c4e84da4 70 char sw,ret_mode;
komaida424 4:4060309b9cc0 71 int vnum,hnum,vmax,hmax;//
komaida424 4:4060309b9cc0 72 int idx,i;//
komaida424 0:cca1c4e84da4 73 char str[33];
komaida424 2:59ac9df97701 74 config init;
komaida424 0:cca1c4e84da4 75
komaida424 2:59ac9df97701 76 void SetUpPrompt(config& conf,I2cPeripherals& i2c)
komaida424 0:cca1c4e84da4 77 {
komaida424 2:59ac9df97701 78 float x,y,z;
komaida424 2:59ac9df97701 79 LCD_cls();
komaida424 0:cca1c4e84da4 80 mode = 0;
komaida424 0:cca1c4e84da4 81 vnum = 0;
komaida424 0:cca1c4e84da4 82 hnum = 0;
komaida424 4:4060309b9cc0 83 vmax = FINAL - 1;
komaida424 2:59ac9df97701 84
komaida424 0:cca1c4e84da4 85 while( 1 ) {
komaida424 2:59ac9df97701 86 // FlashLED(1);
komaida424 0:cca1c4e84da4 87 ret_mode = 'W';
komaida424 0:cca1c4e84da4 88 mode = vnum * 10 + hnum;
komaida424 2:59ac9df97701 89
komaida424 0:cca1c4e84da4 90 switch ( mode ) {
komaida424 4:4060309b9cc0 91
komaida424 4:4060309b9cc0 92 //初期画面
komaida424 0:cca1c4e84da4 93 case 0:
komaida424 2:59ac9df97701 94 LCD_locate(0,0);
komaida424 4:4060309b9cc0 95 LCD_printf( (char*)ModelName[conf.Model_Type] );
komaida424 4:4060309b9cc0 96 LCD_locate(8,0);
komaida424 4:4060309b9cc0 97 sprintf(str,"Ver %4.2f",conf.Revision);
komaida424 2:59ac9df97701 98 LCD_printf(str);
komaida424 2:59ac9df97701 99 LCD_locate(4,1);
komaida424 2:59ac9df97701 100 LCD_printf("By AZUKITEN");
komaida424 6:a50e6d3924f1 101 hmax = 1;
komaida424 6:a50e6d3924f1 102 break;
komaida424 6:a50e6d3924f1 103 case 1: //モデルタイプの設定
komaida424 6:a50e6d3924f1 104 LCD_locate(0,0);
komaida424 6:a50e6d3924f1 105 LCD_printf("Model Type");
komaida424 6:a50e6d3924f1 106 LCD_locate(0,1);
komaida424 6:a50e6d3924f1 107 switch ( sw ) {
komaida424 6:a50e6d3924f1 108 case 'D':
komaida424 6:a50e6d3924f1 109 if ( conf.Model_Type > 0 ) conf.Model_Type -= 1;
komaida424 6:a50e6d3924f1 110 else conf.Model_Type = 4;
komaida424 6:a50e6d3924f1 111 break;
komaida424 6:a50e6d3924f1 112 case 'U':
komaida424 6:a50e6d3924f1 113 if ( conf.Model_Type < 5 ) conf.Model_Type += 1;
komaida424 6:a50e6d3924f1 114 else conf.Model_Type = 0;
komaida424 6:a50e6d3924f1 115 }
komaida424 6:a50e6d3924f1 116 LCD_printf( (char*)ModelName[conf.Model_Type] );
komaida424 6:a50e6d3924f1 117 Set_Arrow(2);
komaida424 0:cca1c4e84da4 118 break;
komaida424 4:4060309b9cc0 119
komaida424 4:4060309b9cc0 120 //送信機信号のキャリブレーション
komaida424 4:4060309b9cc0 121 case CALIBURATE*10: //Calibrate Transmitter
komaida424 2:59ac9df97701 122 LCD_printf("Calibrate");
komaida424 0:cca1c4e84da4 123 Set_Arrow(1);
komaida424 0:cca1c4e84da4 124 hmax = 1;
komaida424 0:cca1c4e84da4 125 break;
komaida424 4:4060309b9cc0 126 case CALIBURATE*10+1: //Calibrate Transmitter
komaida424 2:59ac9df97701 127 LCD_printf("Start Calibrate");
komaida424 3:27407c4984cf 128 wait(1);
komaida424 0:cca1c4e84da4 129 for(i=0; i<4; i++) {
komaida424 0:cca1c4e84da4 130 conf.Stick_Ref[i] = 0;
komaida424 0:cca1c4e84da4 131 }
komaida424 0:cca1c4e84da4 132 for(i=0; i<16; i++) {
komaida424 3:27407c4984cf 133 wait(0.03);
komaida424 3:27407c4984cf 134 Get_Stick_Pos();
komaida424 0:cca1c4e84da4 135 conf.Stick_Ref[ROL] += AIL;
komaida424 0:cca1c4e84da4 136 conf.Stick_Ref[PIT] += ELE;
komaida424 0:cca1c4e84da4 137 conf.Stick_Ref[YAW] += RUD;
komaida424 0:cca1c4e84da4 138 conf.Stick_Ref[COL] += THR;
komaida424 0:cca1c4e84da4 139 // conf.Stick_Ref[GAIN] += AUX;
komaida424 0:cca1c4e84da4 140 }
komaida424 0:cca1c4e84da4 141 for(i=0; i<4; i++) {
komaida424 0:cca1c4e84da4 142 conf.Stick_Ref[i] = conf.Stick_Ref[i]/16;
komaida424 0:cca1c4e84da4 143 }
komaida424 4:4060309b9cc0 144
komaida424 2:59ac9df97701 145 CalibrateGyros();
komaida424 2:59ac9df97701 146 CalibrateAccel();
komaida424 2:59ac9df97701 147 LCD_cls(); //Clear LCD
komaida424 2:59ac9df97701 148 LCD_printf("Calibrate Completed");
komaida424 0:cca1c4e84da4 149 Set_Arrow(3);
komaida424 0:cca1c4e84da4 150 FlashLED(5);
komaida424 2:59ac9df97701 151 hnum = 0;
komaida424 0:cca1c4e84da4 152 break;
komaida424 4:4060309b9cc0 153
komaida424 4:4060309b9cc0 154 //ジャイロ感度の設定
komaida424 4:4060309b9cc0 155 case GYROGAIN*10: //Set Gyro Gain
komaida424 2:59ac9df97701 156 LCD_printf("Set Gyro Gain");
komaida424 0:cca1c4e84da4 157 Set_Arrow(1);
komaida424 6:a50e6d3924f1 158 hmax = 5;
komaida424 0:cca1c4e84da4 159 break;
komaida424 4:4060309b9cc0 160 case GYROGAIN*10+1: //Set Gyro Gain Roll
komaida424 0:cca1c4e84da4 161 if ( conf.Gyro_Gain_Setting == 1 )
komaida424 0:cca1c4e84da4 162 Param_Set_Prompt1("GyroGain>Roll",&conf.Gyro_Gain[0],2,0.00f,1.00f,0.01f,sw);
komaida424 0:cca1c4e84da4 163 else
komaida424 2:59ac9df97701 164 Param_Set_Prompt1("GyroGain>Roll",&conf.Gyro_Gain[3],2,-1.00f,1.00f,0.01f,sw);
komaida424 0:cca1c4e84da4 165 break;
komaida424 4:4060309b9cc0 166 case GYROGAIN*10+2:
komaida424 0:cca1c4e84da4 167 if ( conf.Gyro_Gain_Setting == 1 )
komaida424 0:cca1c4e84da4 168 Param_Set_Prompt1("GyroGain>Pitch",&conf.Gyro_Gain[1],2,0.00f,1.00f,0.01f,sw);
komaida424 0:cca1c4e84da4 169 else
komaida424 0:cca1c4e84da4 170 Param_Set_Prompt1("GyroGain>Pitch",&conf.Gyro_Gain[4],2,-1.00f,1.00f,0.01f,sw);
komaida424 0:cca1c4e84da4 171 break;
komaida424 4:4060309b9cc0 172 case GYROGAIN*10+3:
komaida424 0:cca1c4e84da4 173 if ( conf.Gyro_Gain_Setting == 1 )
komaida424 0:cca1c4e84da4 174 Param_Set_Prompt1("GyroGain>Yaw",&conf.Gyro_Gain[2],2,0.00f,1.00f,0.01f,sw);
komaida424 0:cca1c4e84da4 175 else
komaida424 0:cca1c4e84da4 176 Param_Set_Prompt1("GyroGain>Yaw",&conf.Gyro_Gain[5],2,-1.00f,1.00f,0.01f,sw);
komaida424 0:cca1c4e84da4 177 break;
komaida424 4:4060309b9cc0 178 case GYROGAIN*10+4:
komaida424 6:a50e6d3924f1 179 Param_Set_Prompt1("Active Gyro Gain",&conf.Active_Gyro_Gain,3,0.0f,1.0f,0.01f,sw);
komaida424 6:a50e6d3924f1 180 break;
komaida424 6:a50e6d3924f1 181 case GYROGAIN*10+5:
komaida424 0:cca1c4e84da4 182 // ret_mode = 'R';
komaida424 2:59ac9df97701 183 LCD_printf("GyroGain>setting");
komaida424 2:59ac9df97701 184 LCD_locate(0,1);
komaida424 0:cca1c4e84da4 185 switch ( sw ) {
komaida424 0:cca1c4e84da4 186 case 'U':
komaida424 0:cca1c4e84da4 187 case 'D':
komaida424 0:cca1c4e84da4 188 conf.Gyro_Gain_Setting *= -1;
komaida424 0:cca1c4e84da4 189 }
komaida424 0:cca1c4e84da4 190 if ( conf.Gyro_Gain_Setting == 1 )
komaida424 2:59ac9df97701 191 LCD_printf("Controller");
komaida424 0:cca1c4e84da4 192 else
komaida424 2:59ac9df97701 193 LCD_printf("Transmitter");
komaida424 0:cca1c4e84da4 194 Set_Arrow(3);
komaida424 0:cca1c4e84da4 195 break;
komaida424 4:4060309b9cc0 196
komaida424 4:4060309b9cc0 197 //ジャイロの効きの逆転
komaida424 4:4060309b9cc0 198 case GYRODIR*10: //Set Gyro Direction
komaida424 2:59ac9df97701 199 LCD_printf("Gyro Direction");
komaida424 0:cca1c4e84da4 200 Set_Arrow(1);
komaida424 0:cca1c4e84da4 201 hmax = 4;
komaida424 0:cca1c4e84da4 202 break;
komaida424 4:4060309b9cc0 203 case GYRODIR*10+1: //Set Gyro Direction Roll
komaida424 4:4060309b9cc0 204 case GYRODIR*10+2:
komaida424 4:4060309b9cc0 205 case GYRODIR*10+3:
komaida424 4:4060309b9cc0 206 case GYRODIR*10+4: //ｘｙ軸の入れ替え
komaida424 2:59ac9df97701 207 // ret_mode = 'R';
komaida424 4:4060309b9cc0 208 idx = mode - (GYRODIR*10+1);
komaida424 4:4060309b9cc0 209 if ( mode == (GYRODIR*10+4) )
komaida424 2:59ac9df97701 210 LCD_printf("Gyro>Swap X-Y");
komaida424 0:cca1c4e84da4 211 else {
komaida424 2:59ac9df97701 212 LCD_printf("Gyro>Dir>");
komaida424 2:59ac9df97701 213 LCD_locate(9,0);
komaida424 2:59ac9df97701 214 LCD_printf((char*)steering[idx]);
komaida424 0:cca1c4e84da4 215 }
komaida424 2:59ac9df97701 216 LCD_locate(0,1);
komaida424 0:cca1c4e84da4 217 switch ( sw ) {
komaida424 0:cca1c4e84da4 218 case 'U':
komaida424 0:cca1c4e84da4 219 case 'D':
komaida424 0:cca1c4e84da4 220 conf.Gyro_Dir[idx] *= -1;
komaida424 0:cca1c4e84da4 221 }
komaida424 0:cca1c4e84da4 222 if ( conf.Gyro_Dir[idx] == 1 )
komaida424 2:59ac9df97701 223 LCD_printf("Normal ");
komaida424 0:cca1c4e84da4 224 else
komaida424 2:59ac9df97701 225 LCD_printf("Reverse");
komaida424 4:4060309b9cc0 226 if ( mode == (GYRODIR*10+4) )
komaida424 4:4060309b9cc0 227 Set_Arrow(3);
komaida424 4:4060309b9cc0 228 else
komaida424 4:4060309b9cc0 229 Set_Arrow(2);
komaida424 4:4060309b9cc0 230 break;
komaida424 4:4060309b9cc0 231
komaida424 4:4060309b9cc0 232 //サーボの向きの逆転
komaida424 4:4060309b9cc0 233 case SERVODIR*10: //Set Servo Direction
komaida424 4:4060309b9cc0 234 LCD_printf("Servo Direction");
komaida424 4:4060309b9cc0 235 Set_Arrow(1);
komaida424 4:4060309b9cc0 236 hmax = 6;
komaida424 4:4060309b9cc0 237 break;
komaida424 4:4060309b9cc0 238 case SERVODIR*10+1: //Set Gyro Direction Roll
komaida424 4:4060309b9cc0 239 case SERVODIR*10+2:
komaida424 4:4060309b9cc0 240 case SERVODIR*10+3:
komaida424 4:4060309b9cc0 241 case SERVODIR*10+4:
komaida424 4:4060309b9cc0 242 case SERVODIR*10+5:
komaida424 4:4060309b9cc0 243 case SERVODIR*10+6:
komaida424 4:4060309b9cc0 244 // ret_mode = 'R';
komaida424 4:4060309b9cc0 245 idx = mode - (SERVODIR*10+1);
komaida424 4:4060309b9cc0 246 sprintf(str,"Servo>Dir>M%d",idx+1);
komaida424 4:4060309b9cc0 247 LCD_printf(str);
komaida424 4:4060309b9cc0 248 LCD_locate(0,1);
komaida424 4:4060309b9cc0 249 switch ( sw ) {
komaida424 4:4060309b9cc0 250 case 'U':
komaida424 4:4060309b9cc0 251 case 'D':
komaida424 4:4060309b9cc0 252 conf.Servo_Dir[idx] *= -1;
komaida424 4:4060309b9cc0 253 }
komaida424 4:4060309b9cc0 254 if ( conf.Servo_Dir[idx] == 1 )
komaida424 4:4060309b9cc0 255 LCD_printf("Normal ");
komaida424 4:4060309b9cc0 256 else
komaida424 4:4060309b9cc0 257 LCD_printf("Reverse");
komaida424 4:4060309b9cc0 258 if ( mode == (SERVODIR*10+4) )
komaida424 0:cca1c4e84da4 259 Set_Arrow(3);
komaida424 0:cca1c4e84da4 260 else
komaida424 0:cca1c4e84da4 261 Set_Arrow(2);
komaida424 0:cca1c4e84da4 262 break;
komaida424 4:4060309b9cc0 263
komaida424 4:4060309b9cc0 264 //加速度計の水平レベルの校正
komaida424 4:4060309b9cc0 265 case ACCELCORRECT*10:
komaida424 2:59ac9df97701 266 LCD_printf("Acceleration");
komaida424 2:59ac9df97701 267 LCD_locate(2,1);
komaida424 4:4060309b9cc0 268 LCD_printf("Trim");
komaida424 0:cca1c4e84da4 269 Set_Arrow(1);
komaida424 0:cca1c4e84da4 270 hmax = 3;
komaida424 0:cca1c4e84da4 271 break;
komaida424 4:4060309b9cc0 272 case ACCELCORRECT*10+1:
komaida424 3:27407c4984cf 273 Param_Set_Prompt1("Accel>Rol",&conf.Accel_Ref[ROL],2,-10.0,10.0f,0.001f,sw);
komaida424 2:59ac9df97701 274 break;
komaida424 4:4060309b9cc0 275 case ACCELCORRECT*10+2:
komaida424 3:27407c4984cf 276 Param_Set_Prompt1("Accel>Pitch",&conf.Accel_Ref[PIT],2,-10.0,10.0f,0.001f,sw);
komaida424 2:59ac9df97701 277 break;
komaida424 4:4060309b9cc0 278 case ACCELCORRECT*10+3:
komaida424 3:27407c4984cf 279 Param_Set_Prompt1("Accel>Yaw",&conf.Accel_Ref[YAW],3,-10.0,10.0f,0.001f,sw);
komaida424 0:cca1c4e84da4 280 break;
komaida424 4:4060309b9cc0 281
komaida424 4:4060309b9cc0 282 //スティック操作量の設定
komaida424 4:4060309b9cc0 283 case STICKMIX*10: //Set Stick Mixing
komaida424 2:59ac9df97701 284 LCD_printf("Set Stick Mixing");
komaida424 0:cca1c4e84da4 285 Set_Arrow(1);
komaida424 0:cca1c4e84da4 286 hmax = 3;
komaida424 0:cca1c4e84da4 287 break;
komaida424 4:4060309b9cc0 288 case STICKMIX*10+1: //Set Stick Mixing
komaida424 0:cca1c4e84da4 289 Param_Set_Prompt1("Mixing>Roll",&conf.Stick_Mix[0],2,0.00f,2.00f,0.01f,sw);
komaida424 0:cca1c4e84da4 290 break;
komaida424 4:4060309b9cc0 291 case STICKMIX*10+2:
komaida424 0:cca1c4e84da4 292 Param_Set_Prompt1("Mixing>Pitch",&conf.Stick_Mix[1],2,0.00f,2.00f,0.01f,sw);
komaida424 0:cca1c4e84da4 293 break;
komaida424 4:4060309b9cc0 294 case STICKMIX*10+3:
komaida424 0:cca1c4e84da4 295 Param_Set_Prompt1("Mixing>Yaw",&conf.Stick_Mix[2],3,0.00f,2.00f,0.01f,sw);
komaida424 0:cca1c4e84da4 296 break;
komaida424 4:4060309b9cc0 297
komaida424 4:4060309b9cc0 298 //送信機パルス長の表示
komaida424 4:4060309b9cc0 299 case DISPPULSE*10: //Display Pulse Width
komaida424 2:59ac9df97701 300 LCD_printf("Disp Pulse Width");
komaida424 0:cca1c4e84da4 301 Set_Arrow(1);
komaida424 4:4060309b9cc0 302 hmax = 3;
komaida424 0:cca1c4e84da4 303 break;
komaida424 4:4060309b9cc0 304 case DISPPULSE*10+1: //Display Pulse Width
komaida424 0:cca1c4e84da4 305 // DisplayPulseWidth(THR,AIL,ELE,RUD,AUX);
komaida424 2:59ac9df97701 306 ret_mode = 'R';
komaida424 2:59ac9df97701 307 LCD_locate(0,0);
komaida424 0:cca1c4e84da4 308 sprintf(str,"TR=%4d,AL=%4d",THR,AIL);
komaida424 2:59ac9df97701 309 LCD_printf(str);
komaida424 2:59ac9df97701 310 LCD_locate(0,1);
komaida424 0:cca1c4e84da4 311 sprintf(str,"EL=%4d,RD=%4d",ELE,RUD);
komaida424 2:59ac9df97701 312 LCD_printf(str);
komaida424 0:cca1c4e84da4 313 break;
komaida424 4:4060309b9cc0 314 case DISPPULSE*10+2: //Display AUX,AX2
komaida424 4:4060309b9cc0 315 ret_mode = 'R';
komaida424 4:4060309b9cc0 316 Get_Stick_Pos();
komaida424 4:4060309b9cc0 317 LCD_locate(0,0);
komaida424 4:4060309b9cc0 318 sprintf(str,"A1=%4d,A2=%4d",AUX,AX2);
komaida424 4:4060309b9cc0 319 LCD_printf(str);
komaida424 4:4060309b9cc0 320 LCD_locate(0,1);
komaida424 4:4060309b9cc0 321 sprintf(str,"A3=%4d,A4=%4d",AX3,AX4);
komaida424 4:4060309b9cc0 322 LCD_printf(str);
komaida424 4:4060309b9cc0 323 break;
komaida424 4:4060309b9cc0 324 case DISPPULSE*10+3: //Display Stick Ref
komaida424 2:59ac9df97701 325 ret_mode = 'R';
komaida424 0:cca1c4e84da4 326 Get_Stick_Pos();
komaida424 2:59ac9df97701 327 LCD_locate(0,0);
komaida424 0:cca1c4e84da4 328 sprintf(str,"TR=%4d,AL=%4d",Stick[COL],Stick[ROL]);
komaida424 2:59ac9df97701 329 LCD_printf(str);
komaida424 2:59ac9df97701 330 LCD_locate(0,1);
komaida424 0:cca1c4e84da4 331 sprintf(str,"EL=%4d,RD=%4d",Stick[PIT],Stick[YAW]);
komaida424 2:59ac9df97701 332 LCD_printf(str);
komaida424 0:cca1c4e84da4 333 break;
komaida424 4:4060309b9cc0 334
komaida424 4:4060309b9cc0 335 //センサー値の表示
komaida424 4:4060309b9cc0 336 case DISPSENSOR*10: //Display Sensor Value
komaida424 4:4060309b9cc0 337 LCD_printf("Disp Sensor");
komaida424 2:59ac9df97701 338 Set_Arrow(1);
komaida424 4:4060309b9cc0 339 hmax = 6;
komaida424 4:4060309b9cc0 340 Angle[ROL]=Angle[PIT]=Angle[YAW]=0;
komaida424 4:4060309b9cc0 341 Accel[ROL]=Accel[PIT]=Accel[YAW]=0;
komaida424 2:59ac9df97701 342 break;
komaida424 4:4060309b9cc0 343 case DISPSENSOR*10+1: //Gyro
komaida424 3:27407c4984cf 344 // Get_Gyro();
komaida424 3:27407c4984cf 345 if ( conf.Gyro_Dir[3] ==1 ) i2c.angular(&x,&y,&z);
komaida424 3:27407c4984cf 346 else i2c.angular(&y,&x,&z);
komaida424 3:27407c4984cf 347 x -= Gyro_Ref[0];
komaida424 3:27407c4984cf 348 y -= Gyro_Ref[1];
komaida424 3:27407c4984cf 349 z -= Gyro_Ref[2];
komaida424 2:59ac9df97701 350 LCD_locate(0,0);
komaida424 3:27407c4984cf 351 sprintf(str,"[Gyro]X=%5.1f",x);
komaida424 2:59ac9df97701 352 LCD_printf(str);
komaida424 2:59ac9df97701 353 LCD_locate(0,1);
komaida424 3:27407c4984cf 354 sprintf(str,"y=%5.1f,Z=%5.1f",y,z);
komaida424 2:59ac9df97701 355 LCD_printf(str);
komaida424 0:cca1c4e84da4 356 ret_mode = 'R';
komaida424 0:cca1c4e84da4 357 break;
komaida424 4:4060309b9cc0 358 case DISPSENSOR*10+2: //Gravity
komaida424 2:59ac9df97701 359 if ( conf.Gyro_Dir[3] ==1 ) i2c.Acceleration(&x,&y,&z);
komaida424 2:59ac9df97701 360 else i2c.Acceleration(&y,&x,&z);
komaida424 2:59ac9df97701 361 x -= conf.Accel_Ref[0];
komaida424 2:59ac9df97701 362 y -= conf.Accel_Ref[1];
komaida424 2:59ac9df97701 363 z -= conf.Accel_Ref[2];
komaida424 2:59ac9df97701 364 LCD_locate(0,0);
komaida424 4:4060309b9cc0 365 sprintf(str,"[Gravity]X=%5.2f",x);
komaida424 2:59ac9df97701 366 LCD_printf(str);
komaida424 2:59ac9df97701 367 LCD_locate(0,1);
komaida424 3:27407c4984cf 368 sprintf(str,"Y=%5.2f,Z=%5.2f",y,z);
komaida424 2:59ac9df97701 369 LCD_printf(str);
komaida424 0:cca1c4e84da4 370 // Set_Arrow(2);
komaida424 0:cca1c4e84da4 371 ret_mode = 'R';
komaida424 0:cca1c4e84da4 372 break;
komaida424 4:4060309b9cc0 373 case DISPSENSOR*10+3: //angle
komaida424 2:59ac9df97701 374 PWM_Out(false);
komaida424 2:59ac9df97701 375 LCD_locate(0,0);
komaida424 2:59ac9df97701 376 sprintf(str,"[Angle]X=%6.1f",Angle[ROL]);
komaida424 2:59ac9df97701 377 LCD_printf(str);
komaida424 2:59ac9df97701 378 LCD_locate(0,1);
komaida424 2:59ac9df97701 379 sprintf(str,"Y=%6.1f,Z=%5.1f",Angle[PIT],Angle[YAW]);
komaida424 2:59ac9df97701 380 LCD_printf(str);
komaida424 2:59ac9df97701 381 // Set_Arrow(2);
komaida424 2:59ac9df97701 382 ret_mode = 'R';
komaida424 2:59ac9df97701 383 break;
komaida424 4:4060309b9cc0 384 case DISPSENSOR*10+4: // Pressure
komaida424 0:cca1c4e84da4 385 elaps.reset();
komaida424 0:cca1c4e84da4 386 elaps.start();
komaida424 0:cca1c4e84da4 387 Get_Pressure();
komaida424 0:cca1c4e84da4 388 elaps.stop();
komaida424 2:59ac9df97701 389 LCD_locate(0,0);
komaida424 4:4060309b9cc0 390 sprintf(str,"Press=%4.1fhp",Press);
komaida424 2:59ac9df97701 391 LCD_printf(str);
komaida424 2:59ac9df97701 392 LCD_locate(0,1);
komaida424 4:4060309b9cc0 393 sprintf(str,"Height=%7.2fcm",i2c.height_cm());
komaida424 2:59ac9df97701 394 LCD_printf(str);
komaida424 0:cca1c4e84da4 395 // Set_Arrow(2);
komaida424 0:cca1c4e84da4 396 ret_mode = 'R';
komaida424 2:59ac9df97701 397 wait(0.05);
komaida424 0:cca1c4e84da4 398 break;
komaida424 4:4060309b9cc0 399 case DISPSENSOR*10+5:
komaida424 0:cca1c4e84da4 400 elaps.reset();
komaida424 0:cca1c4e84da4 401 elaps.start();
komaida424 0:cca1c4e84da4 402 PWM_Out(false);
komaida424 0:cca1c4e84da4 403 elaps.stop();
komaida424 2:59ac9df97701 404 i = elaps.read_us();
komaida424 2:59ac9df97701 405 LCD_locate(0,0);
komaida424 2:59ac9df97701 406 sprintf(str,"ElapsTime=%6d",i);
komaida424 2:59ac9df97701 407 LCD_printf(str);
komaida424 4:4060309b9cc0 408 // Set_Arrow(2);
komaida424 0:cca1c4e84da4 409 ret_mode = 'R';
komaida424 0:cca1c4e84da4 410 break;
komaida424 4:4060309b9cc0 411 case DISPSENSOR*10+6: //Sensor Calibration
komaida424 0:cca1c4e84da4 412 CalibrateGyros();
komaida424 2:59ac9df97701 413 FlashLED(3);
komaida424 2:59ac9df97701 414 LCD_printf("Calibrate completed");
komaida424 0:cca1c4e84da4 415 Set_Arrow(3);
komaida424 0:cca1c4e84da4 416 break;
komaida424 4:4060309b9cc0 417
komaida424 4:4060309b9cc0 418
komaida424 4:4060309b9cc0 419 //ＥＳＣ用PWMパルス長の表示
komaida424 4:4060309b9cc0 420 case DISPPWM*10: //Display PWM Condition
komaida424 2:59ac9df97701 421 LCD_printf("Display PWM ");
komaida424 0:cca1c4e84da4 422 Set_Arrow(1);
komaida424 4:4060309b9cc0 423 hmax = 2;
komaida424 0:cca1c4e84da4 424 break;
komaida424 4:4060309b9cc0 425 case DISPPWM*10+1: //Display PWM Width
komaida424 2:59ac9df97701 426 ret_mode = 'R';
komaida424 0:cca1c4e84da4 427 PWM_Out(false);
komaida424 2:59ac9df97701 428 LCD_locate(0,0);
komaida424 6:a50e6d3924f1 429 sprintf(str,"M1=%4d,M2=%4d",M1,M2);
komaida424 2:59ac9df97701 430 LCD_printf(str);
komaida424 2:59ac9df97701 431 LCD_locate(0,1);
komaida424 6:a50e6d3924f1 432 sprintf(str,"M4=%4d,M3=%4d",M4,M3);
komaida424 2:59ac9df97701 433 LCD_printf(str);
komaida424 0:cca1c4e84da4 434 break;
komaida424 4:4060309b9cc0 435 case DISPPWM*10+2: //Display PWM Width
komaida424 4:4060309b9cc0 436 ret_mode = 'R';
komaida424 4:4060309b9cc0 437 PWM_Out(false);
komaida424 4:4060309b9cc0 438 LCD_locate(0,0);
komaida424 6:a50e6d3924f1 439 sprintf(str,"M5=%4d,M6=%4d",M5,M6);
komaida424 4:4060309b9cc0 440 LCD_printf(str);
komaida424 4:4060309b9cc0 441 break;
komaida424 4:4060309b9cc0 442
komaida424 4:4060309b9cc0 443 //その他パラメータ値の設定
komaida424 4:4060309b9cc0 444 case PARMSET*10: //パラメーター設定
komaida424 2:59ac9df97701 445 LCD_printf("Parameter Set");
komaida424 0:cca1c4e84da4 446 Set_Arrow(1);
komaida424 6:a50e6d3924f1 447 hmax = 9;
komaida424 0:cca1c4e84da4 448 break;
komaida424 4:4060309b9cc0 449 case PARMSET*10+1:
komaida424 0:cca1c4e84da4 450 Param_Set_Prompt1("LCD>Contrast",&conf.LCD_Contrast,2,0,63,1,sw);
komaida424 0:cca1c4e84da4 451 break;
komaida424 4:4060309b9cc0 452 case PARMSET*10+2:
komaida424 2:59ac9df97701 453 LCD_locate(0,0);
komaida424 2:59ac9df97701 454 LCD_printf("PWM>Mode");
komaida424 2:59ac9df97701 455 LCD_locate(0,1);
komaida424 0:cca1c4e84da4 456 switch ( sw ) {
komaida424 0:cca1c4e84da4 457 case 'U':
komaida424 0:cca1c4e84da4 458 case 'D':
komaida424 0:cca1c4e84da4 459 conf.PWM_Mode *= -1;
komaida424 0:cca1c4e84da4 460 }
komaida424 0:cca1c4e84da4 461 if ( conf.PWM_Mode == 1 )
komaida424 2:59ac9df97701 462 LCD_printf("ESC ");
komaida424 0:cca1c4e84da4 463 else
komaida424 2:59ac9df97701 464 LCD_printf("Moter");
komaida424 0:cca1c4e84da4 465 Set_Arrow(2);
komaida424 0:cca1c4e84da4 466 break;
komaida424 4:4060309b9cc0 467 case PARMSET*10+3:
komaida424 4:4060309b9cc0 468 Param_Set_Prompt1("PWM>Interval",&conf.PWM_Interval,2,Thro_Hi,20000,100,sw);
komaida424 2:59ac9df97701 469 break;
komaida424 4:4060309b9cc0 470 case PARMSET*10+4:
komaida424 2:59ac9df97701 471 Param_Set_Prompt1("Gyro>CutoffFreq",&conf.Cutoff_Freq,2,0.00f,10.0f,0.01f,sw);
komaida424 2:59ac9df97701 472 break;
komaida424 4:4060309b9cc0 473 case PARMSET*10+5:
komaida424 6:a50e6d3924f1 474 Param_Set_Prompt1("ESC>Throttl Trim",&conf.Throttl_Trim,2,Pulse_Min,Pulse_Max,1,sw);
komaida424 0:cca1c4e84da4 475 break;
komaida424 4:4060309b9cc0 476 case PARMSET*10+6:
komaida424 6:a50e6d3924f1 477 Param_Set_Prompt1("ESC>ReversePoint",&conf.Reverse_Point,2,1000,2000,1,sw);
komaida424 6:a50e6d3924f1 478 break;
komaida424 6:a50e6d3924f1 479 case PARMSET*10+7:
komaida424 2:59ac9df97701 480 Param_Set_Prompt1("Flight Timer",&conf.Flight_Time,2,0,600,10,sw);
komaida424 0:cca1c4e84da4 481 break;
komaida424 6:a50e6d3924f1 482 case PARMSET*10+8:
komaida424 6:a50e6d3924f1 483 Param_Set_Prompt1("Thro Limit val",&conf.Thro_Limit_Val,2,0,200,1,sw);
komaida424 6:a50e6d3924f1 484 throLimit.differential(conf.Thro_Limit_Val);
komaida424 6:a50e6d3924f1 485 break;
komaida424 6:a50e6d3924f1 486 case PARMSET*10+9:
komaida424 6:a50e6d3924f1 487 Param_Set_Prompt1("Thro Limit Rate",&conf.Thro_Limit_Rate,3,0,1,0.01,sw);
komaida424 6:a50e6d3924f1 488 throLimit.rate(conf.Thro_Limit_Rate);
komaida424 6:a50e6d3924f1 489 break;
komaida424 6:a50e6d3924f1 490 /* case PARMSET*10+7:
komaida424 4:4060309b9cc0 491 LCD_locate(0,0);
komaida424 4:4060309b9cc0 492 LCD_printf("Model Type");
komaida424 4:4060309b9cc0 493 LCD_locate(0,1);
komaida424 4:4060309b9cc0 494 switch ( sw ) {
komaida424 4:4060309b9cc0 495 case 'D':
komaida424 4:4060309b9cc0 496 if ( conf.Model_Type > 0 ) conf.Model_Type -= 1;
komaida424 4:4060309b9cc0 497 else conf.Model_Type = 3;
komaida424 4:4060309b9cc0 498 break;
komaida424 4:4060309b9cc0 499 case 'U':
komaida424 4:4060309b9cc0 500 if ( conf.Model_Type < 4 ) conf.Model_Type += 1;
komaida424 4:4060309b9cc0 501 else conf.Model_Type = 0;
komaida424 4:4060309b9cc0 502 }
komaida424 4:4060309b9cc0 503 LCD_printf( (char*)ModelName[conf.Model_Type] );
komaida424 4:4060309b9cc0 504 Set_Arrow(2);
komaida424 4:4060309b9cc0 505 break;
komaida424 4:4060309b9cc0 506 case PARMSET*10+8:
komaida424 6:a50e6d3924f1 507 Param_Set_Prompt1("Active Gyro Gain",&conf.Active_Gyro_Gain,3,0.0f,1.0f,0.01f,sw);
komaida424 4:4060309b9cc0 508 break;
komaida424 6:a50e6d3924f1 509 */
komaida424 4:4060309b9cc0 510 //設定データの保存
komaida424 4:4060309b9cc0 511 case CONFSTORE*10: //E2PROM Store
komaida424 2:59ac9df97701 512 LCD_printf("Config Save");
komaida424 0:cca1c4e84da4 513 Set_Arrow(1);
komaida424 0:cca1c4e84da4 514 hmax = 1;
komaida424 0:cca1c4e84da4 515 break;
komaida424 4:4060309b9cc0 516 case CONFSTORE*10+1:
komaida424 0:cca1c4e84da4 517 WriteConfig();
komaida424 2:59ac9df97701 518 LCD_locate(0,0);
komaida424 0:cca1c4e84da4 519 sprintf(str,"Config %3dbyte",sizeof(config));
komaida424 2:59ac9df97701 520 LCD_printf(str);
komaida424 2:59ac9df97701 521 LCD_locate(0,1);
komaida424 2:59ac9df97701 522 LCD_printf("Save Sucssesuful");
komaida424 0:cca1c4e84da4 523 Set_Arrow(3);
komaida424 2:59ac9df97701 524 wait(0.5);
komaida424 2:59ac9df97701 525 FlashLED(5);
komaida424 2:59ac9df97701 526 hnum = 0;
komaida424 0:cca1c4e84da4 527 break;
komaida424 4:4060309b9cc0 528
komaida424 4:4060309b9cc0 529 //設定データの初期化
komaida424 4:4060309b9cc0 530 case CONFRESET*10: //E2PROM reset
komaida424 2:59ac9df97701 531 LCD_printf("Config Reset");
komaida424 2:59ac9df97701 532 Set_Arrow(1);
komaida424 2:59ac9df97701 533 hmax = 3;
komaida424 0:cca1c4e84da4 534 break;
komaida424 4:4060309b9cc0 535 case CONFRESET*10+1:
komaida424 3:27407c4984cf 536 LCD_printf("Ailron stick");
komaida424 2:59ac9df97701 537 LCD_locate(0,1);
komaida424 3:27407c4984cf 538 LCD_printf("Move to right");
komaida424 2:59ac9df97701 539 Set_Arrow(2);
komaida424 2:59ac9df97701 540 break;
komaida424 4:4060309b9cc0 541 case CONFRESET*10+2: //E2PROM reset
komaida424 2:59ac9df97701 542 conf = init;
komaida424 2:59ac9df97701 543 LCD_printf("Rset sucssesuful");
komaida424 2:59ac9df97701 544 Set_Arrow(3);
komaida424 2:59ac9df97701 545 break;
komaida424 0:cca1c4e84da4 546 default:
komaida424 0:cca1c4e84da4 547 if ( hnum == 0 )
komaida424 0:cca1c4e84da4 548 vnum++;
komaida424 0:cca1c4e84da4 549 }
komaida424 0:cca1c4e84da4 550
komaida424 0:cca1c4e84da4 551
komaida424 0:cca1c4e84da4 552 sw = Check_Stick_Dir(ret_mode); //Wait Mode
komaida424 0:cca1c4e84da4 553
komaida424 0:cca1c4e84da4 554 switch ( sw ) {
komaida424 0:cca1c4e84da4 555 case 'L':
komaida424 0:cca1c4e84da4 556 hnum--;
komaida424 0:cca1c4e84da4 557 if ( hnum <= 0 ) hnum = 0;
komaida424 2:59ac9df97701 558 LCD_cls(); //Clear LCD
komaida424 0:cca1c4e84da4 559 break;
komaida424 0:cca1c4e84da4 560 case 'R':
komaida424 2:59ac9df97701 561 LCD_cls();
komaida424 0:cca1c4e84da4 562 if ( hnum < hmax ) hnum++;
komaida424 0:cca1c4e84da4 563 break;
komaida424 0:cca1c4e84da4 564 case 'U':
komaida424 0:cca1c4e84da4 565 if ( hnum == 0 ) {
komaida424 0:cca1c4e84da4 566 if ( vnum < vmax ) vnum++;
komaida424 0:cca1c4e84da4 567 else vnum = 0;
komaida424 2:59ac9df97701 568 LCD_cls(); //Clear LCD
komaida424 0:cca1c4e84da4 569 }
komaida424 0:cca1c4e84da4 570 break;
komaida424 0:cca1c4e84da4 571 case 'D':
komaida424 0:cca1c4e84da4 572 if ( hnum == 0 ) {
komaida424 0:cca1c4e84da4 573 if ( vnum > 0 ) vnum--;
komaida424 0:cca1c4e84da4 574 else vnum = vmax;
komaida424 2:59ac9df97701 575 LCD_cls(); //Clear LCD
komaida424 0:cca1c4e84da4 576 }
komaida424 0:cca1c4e84da4 577 break;
komaida424 0:cca1c4e84da4 578 case 'E':
komaida424 6:a50e6d3924f1 579 while ( conf.Model_Type == Quad_3D && Stick[GAIN] < 0 ) {
komaida424 6:a50e6d3924f1 580 FlashLED(2);
komaida424 6:a50e6d3924f1 581 wait(0.5);
komaida424 6:a50e6d3924f1 582 }
komaida424 2:59ac9df97701 583 LCD_cls(); //Clear LCD
komaida424 2:59ac9df97701 584 LCD_locate(0,0);
komaida424 2:59ac9df97701 585 LCD_printf("PWM Started");
komaida424 0:cca1c4e84da4 586 return;
komaida424 0:cca1c4e84da4 587 }
komaida424 0:cca1c4e84da4 588 }
komaida424 0:cca1c4e84da4 589
komaida424 0:cca1c4e84da4 590 }
komaida424 0:cca1c4e84da4 591
komaida424 0:cca1c4e84da4 592 char Check_Stick_Dir(char act)
komaida424 0:cca1c4e84da4 593 {
komaida424 0:cca1c4e84da4 594 int i;
komaida424 0:cca1c4e84da4 595 while ( 1 ) {
komaida424 0:cca1c4e84da4 596 Get_Stick_Pos();
komaida424 0:cca1c4e84da4 597 if ( Stick[YAW] > Stick_Limit ) {
komaida424 0:cca1c4e84da4 598 i = 0;
komaida424 0:cca1c4e84da4 599 while ( Stick[YAW] > Stick_Limit && Stick[COL] < 30 ) {
komaida424 0:cca1c4e84da4 600 if ( i > 2000 ) return 'E'; //wait 2 sec
komaida424 0:cca1c4e84da4 601 wait(0.001); // wait 1 msec
komaida424 0:cca1c4e84da4 602 Get_Stick_Pos();
komaida424 0:cca1c4e84da4 603 i++;
komaida424 0:cca1c4e84da4 604 }
komaida424 0:cca1c4e84da4 605 }
komaida424 0:cca1c4e84da4 606 if ( Stick[ROL] > Stick_Limit ) {
komaida424 0:cca1c4e84da4 607 wait(0.03);
komaida424 0:cca1c4e84da4 608 Get_Stick_Pos();
komaida424 0:cca1c4e84da4 609 if ( !(Stick[ROL] > Stick_Limit) ) continue;
komaida424 0:cca1c4e84da4 610 while ( Stick[ROL] > Stick_Limit ) {
komaida424 0:cca1c4e84da4 611 Get_Stick_Pos();
komaida424 0:cca1c4e84da4 612 }
komaida424 0:cca1c4e84da4 613 return 'R';
komaida424 0:cca1c4e84da4 614 }
komaida424 0:cca1c4e84da4 615 if ( Stick[ROL] < -Stick_Limit ) {
komaida424 0:cca1c4e84da4 616 wait(0.03);
komaida424 0:cca1c4e84da4 617 Get_Stick_Pos();
komaida424 0:cca1c4e84da4 618 if ( !(Stick[ROL] < -Stick_Limit) ) continue;
komaida424 0:cca1c4e84da4 619 while ( Stick[ROL] < -Stick_Limit ) {
komaida424 0:cca1c4e84da4 620 Get_Stick_Pos();
komaida424 0:cca1c4e84da4 621 }
komaida424 0:cca1c4e84da4 622 return 'L';
komaida424 0:cca1c4e84da4 623 }
komaida424 0:cca1c4e84da4 624 if ( Stick[PIT] < -Stick_Limit ) {
komaida424 0:cca1c4e84da4 625 wait(0.03);
komaida424 0:cca1c4e84da4 626 Get_Stick_Pos();
komaida424 0:cca1c4e84da4 627 if ( !(Stick[PIT] < -Stick_Limit) ) continue;
komaida424 0:cca1c4e84da4 628 if ( act == 'R' ) {
komaida424 0:cca1c4e84da4 629 wait(0.03);
komaida424 0:cca1c4e84da4 630 return 'D';
komaida424 0:cca1c4e84da4 631 }
komaida424 0:cca1c4e84da4 632 while ( Stick[PIT] < -Stick_Limit ) {
komaida424 0:cca1c4e84da4 633 Get_Stick_Pos();
komaida424 0:cca1c4e84da4 634 }
komaida424 0:cca1c4e84da4 635 return 'D';
komaida424 0:cca1c4e84da4 636 }
komaida424 0:cca1c4e84da4 637 if ( Stick[PIT] > Stick_Limit ) {
komaida424 0:cca1c4e84da4 638 wait(0.03);
komaida424 0:cca1c4e84da4 639 Get_Stick_Pos();
komaida424 0:cca1c4e84da4 640 if ( !( Stick[PIT] > Stick_Limit) ) continue;
komaida424 0:cca1c4e84da4 641 if ( act == 'R' ) {
komaida424 0:cca1c4e84da4 642 wait(0.03);
komaida424 0:cca1c4e84da4 643 return 'U';
komaida424 0:cca1c4e84da4 644 }
komaida424 0:cca1c4e84da4 645 while ( Stick[PIT] > Stick_Limit ) {
komaida424 0:cca1c4e84da4 646 Get_Stick_Pos();
komaida424 0:cca1c4e84da4 647 }
komaida424 0:cca1c4e84da4 648 return 'U';
komaida424 0:cca1c4e84da4 649 }
komaida424 0:cca1c4e84da4 650 if ( act == 'R' )
komaida424 0:cca1c4e84da4 651 return ' ';
komaida424 0:cca1c4e84da4 652 }
komaida424 0:cca1c4e84da4 653 }
komaida424 0:cca1c4e84da4 654
komaida424 2:59ac9df97701 655 void Param_Set_Prompt1(char *hd,int *num,int arrow,int min,int max,int increase,char sw)
komaida424 0:cca1c4e84da4 656 {
komaida424 0:cca1c4e84da4 657 ret_mode = 'R';
komaida424 2:59ac9df97701 658 LCD_locate(0,0);
komaida424 2:59ac9df97701 659 LCD_printf(hd);
komaida424 2:59ac9df97701 660 LCD_locate(0,1);
komaida424 2:59ac9df97701 661 sprintf(str,"%6d",*num);
komaida424 2:59ac9df97701 662 LCD_printf(str);
komaida424 0:cca1c4e84da4 663 Set_Arrow(arrow);
komaida424 0:cca1c4e84da4 664 switch ( sw ) {
komaida424 0:cca1c4e84da4 665 case 'U':
komaida424 0:cca1c4e84da4 666 *num -= increase;
komaida424 0:cca1c4e84da4 667 if ( *num <= min )
komaida424 0:cca1c4e84da4 668 *num = min;
komaida424 0:cca1c4e84da4 669 break;
komaida424 0:cca1c4e84da4 670 case 'D':
komaida424 0:cca1c4e84da4 671 *num += increase;
komaida424 0:cca1c4e84da4 672 if ( *num >= max )
komaida424 0:cca1c4e84da4 673 *num = max;
komaida424 0:cca1c4e84da4 674 }
komaida424 0:cca1c4e84da4 675 }
komaida424 2:59ac9df97701 676 void Param_Set_Prompt1(char *hd,float *num,int arrow,float min,float max,float increase,char sw)
komaida424 0:cca1c4e84da4 677 {
komaida424 0:cca1c4e84da4 678 ret_mode = 'R';
komaida424 2:59ac9df97701 679 LCD_locate(0,0);
komaida424 2:59ac9df97701 680 LCD_printf(hd);
komaida424 2:59ac9df97701 681 LCD_locate(0,1);
komaida424 3:27407c4984cf 682 sprintf(str,"%7.3f",*num);
komaida424 2:59ac9df97701 683 LCD_printf(str);
komaida424 0:cca1c4e84da4 684 Set_Arrow(arrow);
komaida424 0:cca1c4e84da4 685 switch ( sw ) {
komaida424 0:cca1c4e84da4 686 case 'U':
komaida424 0:cca1c4e84da4 687 *num -= increase;
komaida424 0:cca1c4e84da4 688 if ( *num <= min )
komaida424 0:cca1c4e84da4 689 *num = min;
komaida424 0:cca1c4e84da4 690 break;
komaida424 0:cca1c4e84da4 691 case 'D':
komaida424 0:cca1c4e84da4 692 *num += increase;
komaida424 0:cca1c4e84da4 693 if ( *num >= max )
komaida424 0:cca1c4e84da4 694 *num = max;
komaida424 0:cca1c4e84da4 695 }
komaida424 0:cca1c4e84da4 696 }
komaida424 0:cca1c4e84da4 697
komaida424 0:cca1c4e84da4 698 void Set_Arrow(int dir)
komaida424 0:cca1c4e84da4 699 {
komaida424 2:59ac9df97701 700 LCD_locate(12,1);
komaida424 0:cca1c4e84da4 701 switch ( dir ) {
komaida424 0:cca1c4e84da4 702 case 1:
komaida424 2:59ac9df97701 703 LCD_printf(" >>");
komaida424 0:cca1c4e84da4 704 break;
komaida424 0:cca1c4e84da4 705 case 2:
komaida424 2:59ac9df97701 706 LCD_printf("<<>>");
komaida424 0:cca1c4e84da4 707 break;
komaida424 0:cca1c4e84da4 708 case 3:
komaida424 2:59ac9df97701 709 LCD_printf(" <<");
komaida424 0:cca1c4e84da4 710 }
komaida424 4:4060309b9cc0 711 };
komaida424 6:a50e6d3924f1 712