syouichi imamori / Mbed OS MulticopterQuadX

Quad X Type Multicopter

Dependencies:   IAP

config.cpp@4:4060309b9cc0, 2014-10-14 (annotated)

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