syouichi imamori / Mbed OS MulticopterQuadX

Quad X Type Multicopter

Dependencies:   IAP

config.cpp@0:cca1c4e84da4, 2013-07-11 (annotated)

Committer:
komaida424
Date:
Thu Jul 11 19:18:44 2013 +0000
Revision:
0:cca1c4e84da4
Child:
2:59ac9df97701
version1.03

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 "I2cLCD.h"
komaida424 0:cca1c4e84da4 4 #include "InterruptIn.h"
komaida424 0:cca1c4e84da4 5 //#include "ITG3200.h"
komaida424 0:cca1c4e84da4 6 //#include "LPS331AP.h"
komaida424 0:cca1c4e84da4 7 #include "config.h"
komaida424 0:cca1c4e84da4 8 #include "PulseWidthCounter.h"
komaida424 0:cca1c4e84da4 9 #include "SerialLcd.h"
komaida424 0:cca1c4e84da4 10
komaida424 0:cca1c4e84da4 11 void FlashLED(int);
komaida424 0:cca1c4e84da4 12 char Check_Stick_Dir(char);
komaida424 0:cca1c4e84da4 13 void Param_Set_Prompt1(const char *,int *,int,int,int,int,char);
komaida424 0:cca1c4e84da4 14 void Param_Set_Prompt1(const char *,float *,int,float,float,float,char);
komaida424 0:cca1c4e84da4 15 void Set_Arrow(int dir);
komaida424 0:cca1c4e84da4 16 void Get_Stick_Pos();
komaida424 0:cca1c4e84da4 17 void CalibrateGyros(void);
komaida424 0:cca1c4e84da4 18 void CalibrateAccel(void);
komaida424 0:cca1c4e84da4 19 void Get_Gyro();
komaida424 0:cca1c4e84da4 20 void Get_Accel();
komaida424 0:cca1c4e84da4 21 void PWM_Out(bool);
komaida424 0:cca1c4e84da4 22 void WriteConfig();
komaida424 0:cca1c4e84da4 23 void ESC_SetUp(void);
komaida424 0:cca1c4e84da4 24 void Get_Pressure();
komaida424 0:cca1c4e84da4 25
komaida424 0:cca1c4e84da4 26 Timer elaps;
komaida424 0:cca1c4e84da4 27
komaida424 0:cca1c4e84da4 28 extern int CH[5];
komaida424 0:cca1c4e84da4 29 extern int M[6];
komaida424 0:cca1c4e84da4 30 extern int Gyro[3];
komaida424 0:cca1c4e84da4 31 extern int Accel[3];
komaida424 0:cca1c4e84da4 32 extern int Gyro_Ref[3];
komaida424 0:cca1c4e84da4 33 extern int Stick[5];
komaida424 0:cca1c4e84da4 34 extern float Press;
komaida424 0:cca1c4e84da4 35 const char steering[3][6]= {"Roll ","Pitch","Yaw "};
komaida424 0:cca1c4e84da4 36 short mode;
komaida424 0:cca1c4e84da4 37 char sw,ret_mode;
komaida424 0:cca1c4e84da4 38 short vnum,hnum,vmax,hmax;
komaida424 0:cca1c4e84da4 39 short idx,i;
komaida424 0:cca1c4e84da4 40 char str[33];
komaida424 0:cca1c4e84da4 41
komaida424 0:cca1c4e84da4 42 #ifdef SERIAL_LCD
komaida424 0:cca1c4e84da4 43 SerialLcd *lcdptr;
komaida424 0:cca1c4e84da4 44 void SetUpPrompt(config& conf,SerialLcd& lcd)
komaida424 0:cca1c4e84da4 45 #else
komaida424 0:cca1c4e84da4 46 I2cPeripherals *lcdptr;
komaida424 0:cca1c4e84da4 47 void SetUpPrompt(config& conf,I2cPeripherals& lcd)
komaida424 0:cca1c4e84da4 48 #endif
komaida424 0:cca1c4e84da4 49 {
komaida424 0:cca1c4e84da4 50
komaida424 0:cca1c4e84da4 51 lcdptr = &lcd;
komaida424 0:cca1c4e84da4 52 lcd.cls();
komaida424 0:cca1c4e84da4 53 mode = 0;
komaida424 0:cca1c4e84da4 54 vnum = 0;
komaida424 0:cca1c4e84da4 55 hnum = 0;
komaida424 0:cca1c4e84da4 56 vmax = 11;
komaida424 0:cca1c4e84da4 57
komaida424 0:cca1c4e84da4 58 while( 1 ) {
komaida424 0:cca1c4e84da4 59 FlashLED(1);
komaida424 0:cca1c4e84da4 60 ret_mode = 'W';
komaida424 0:cca1c4e84da4 61 mode = vnum * 10 + hnum;
komaida424 0:cca1c4e84da4 62
komaida424 0:cca1c4e84da4 63 switch ( mode ) {
komaida424 0:cca1c4e84da4 64 case 0:
komaida424 0:cca1c4e84da4 65 lcd.locate(0,0);
komaida424 0:cca1c4e84da4 66 sprintf(str,"Quad-X Ver %4.2f",conf.Revision);
komaida424 0:cca1c4e84da4 67 lcd.printf(str);
komaida424 0:cca1c4e84da4 68 lcd.locate(4,1);
komaida424 0:cca1c4e84da4 69 lcd.printf("By AZUKITEN");
komaida424 0:cca1c4e84da4 70 hmax = 0;
komaida424 0:cca1c4e84da4 71 break;
komaida424 0:cca1c4e84da4 72 case 10: //Calibrate Transmitter
komaida424 0:cca1c4e84da4 73 lcd.printf("Calibrate Transmitter");
komaida424 0:cca1c4e84da4 74 Set_Arrow(1);
komaida424 0:cca1c4e84da4 75 hmax = 1;
komaida424 0:cca1c4e84da4 76 break;
komaida424 0:cca1c4e84da4 77 case 11: //Calibrate Transmitter
komaida424 0:cca1c4e84da4 78 lcd.printf("Start Calibrate");
komaida424 0:cca1c4e84da4 79 for(i=0; i<4; i++) {
komaida424 0:cca1c4e84da4 80 conf.Stick_Ref[i] = 0;
komaida424 0:cca1c4e84da4 81 }
komaida424 0:cca1c4e84da4 82 for(i=0; i<16; i++) {
komaida424 0:cca1c4e84da4 83 wait(0.3);
komaida424 0:cca1c4e84da4 84 conf.Stick_Ref[ROL] += AIL;
komaida424 0:cca1c4e84da4 85 conf.Stick_Ref[PIT] += ELE;
komaida424 0:cca1c4e84da4 86 conf.Stick_Ref[YAW] += RUD;
komaida424 0:cca1c4e84da4 87 conf.Stick_Ref[COL] += THR;
komaida424 0:cca1c4e84da4 88 // conf.Stick_Ref[GAIN] += AUX;
komaida424 0:cca1c4e84da4 89 }
komaida424 0:cca1c4e84da4 90 for(i=0; i<4; i++) {
komaida424 0:cca1c4e84da4 91 conf.Stick_Ref[i] = conf.Stick_Ref[i]/16;
komaida424 0:cca1c4e84da4 92 }
komaida424 0:cca1c4e84da4 93 lcd.cls(); //Clear LCD
komaida424 0:cca1c4e84da4 94 lcd.printf("Calibrate Completed");
komaida424 0:cca1c4e84da4 95 Set_Arrow(3);
komaida424 0:cca1c4e84da4 96 FlashLED(5);
komaida424 0:cca1c4e84da4 97 break;
komaida424 0:cca1c4e84da4 98 case 20: //Set Gyro Gain
komaida424 0:cca1c4e84da4 99 lcd.printf("Set Gyro Gain");
komaida424 0:cca1c4e84da4 100 Set_Arrow(1);
komaida424 0:cca1c4e84da4 101 hmax = 4;
komaida424 0:cca1c4e84da4 102 break;
komaida424 0:cca1c4e84da4 103 case 21: //Set Gyro Gain Roll
komaida424 0:cca1c4e84da4 104 if ( conf.Gyro_Gain_Setting == 1 )
komaida424 0:cca1c4e84da4 105 Param_Set_Prompt1("GyroGain>Roll",&conf.Gyro_Gain[0],2,0.00f,1.00f,0.01f,sw);
komaida424 0:cca1c4e84da4 106 else
komaida424 0:cca1c4e84da4 107 Param_Set_Prompt1("GyroGain>Roll",&conf.Gyro_Gain[3],2,-1.00f,1.00f,0.01f,sw);
komaida424 0:cca1c4e84da4 108 break;
komaida424 0:cca1c4e84da4 109 case 22:
komaida424 0:cca1c4e84da4 110 if ( conf.Gyro_Gain_Setting == 1 )
komaida424 0:cca1c4e84da4 111 Param_Set_Prompt1("GyroGain>Pitch",&conf.Gyro_Gain[1],2,0.00f,1.00f,0.01f,sw);
komaida424 0:cca1c4e84da4 112 else
komaida424 0:cca1c4e84da4 113 Param_Set_Prompt1("GyroGain>Pitch",&conf.Gyro_Gain[4],2,-1.00f,1.00f,0.01f,sw);
komaida424 0:cca1c4e84da4 114 break;
komaida424 0:cca1c4e84da4 115 case 23:
komaida424 0:cca1c4e84da4 116 if ( conf.Gyro_Gain_Setting == 1 )
komaida424 0:cca1c4e84da4 117 Param_Set_Prompt1("GyroGain>Yaw",&conf.Gyro_Gain[2],2,0.00f,1.00f,0.01f,sw);
komaida424 0:cca1c4e84da4 118 else
komaida424 0:cca1c4e84da4 119 Param_Set_Prompt1("GyroGain>Yaw",&conf.Gyro_Gain[5],2,-1.00f,1.00f,0.01f,sw);
komaida424 0:cca1c4e84da4 120 break;
komaida424 0:cca1c4e84da4 121 case 24:
komaida424 0:cca1c4e84da4 122 // ret_mode = 'R';
komaida424 0:cca1c4e84da4 123 lcd.printf("GyroGain>setting");
komaida424 0:cca1c4e84da4 124 lcd.locate(0,1);
komaida424 0:cca1c4e84da4 125 switch ( sw ) {
komaida424 0:cca1c4e84da4 126 case 'U':
komaida424 0:cca1c4e84da4 127 case 'D':
komaida424 0:cca1c4e84da4 128 conf.Gyro_Gain_Setting *= -1;
komaida424 0:cca1c4e84da4 129 }
komaida424 0:cca1c4e84da4 130 if ( conf.Gyro_Gain_Setting == 1 )
komaida424 0:cca1c4e84da4 131 lcd.printf("Controller");
komaida424 0:cca1c4e84da4 132 else
komaida424 0:cca1c4e84da4 133 lcd.printf("Transmitter");
komaida424 0:cca1c4e84da4 134 Set_Arrow(3);
komaida424 0:cca1c4e84da4 135 break;
komaida424 0:cca1c4e84da4 136 case 30: //Set Gyro Direction
komaida424 0:cca1c4e84da4 137 lcd.printf("Gyro Direction");
komaida424 0:cca1c4e84da4 138 Set_Arrow(1);
komaida424 0:cca1c4e84da4 139 hmax = 4;
komaida424 0:cca1c4e84da4 140 break;
komaida424 0:cca1c4e84da4 141 case 31: //Set Gyro Direction Roll
komaida424 0:cca1c4e84da4 142 case 32:
komaida424 0:cca1c4e84da4 143 case 33:
komaida424 0:cca1c4e84da4 144 case 34:
komaida424 0:cca1c4e84da4 145 // ret_mode = 'R';
komaida424 0:cca1c4e84da4 146 idx = mode - 31;
komaida424 0:cca1c4e84da4 147 if ( mode == 34 )
komaida424 0:cca1c4e84da4 148 lcd.printf("Gyro>Swap X-Y");
komaida424 0:cca1c4e84da4 149 else {
komaida424 0:cca1c4e84da4 150 lcd.printf("Gyro>Dir>");
komaida424 0:cca1c4e84da4 151 lcd.locate(9,0);
komaida424 0:cca1c4e84da4 152 lcd.printf(steering[idx]);
komaida424 0:cca1c4e84da4 153 }
komaida424 0:cca1c4e84da4 154 lcd.locate(0,1);
komaida424 0:cca1c4e84da4 155 switch ( sw ) {
komaida424 0:cca1c4e84da4 156 case 'U':
komaida424 0:cca1c4e84da4 157 case 'D':
komaida424 0:cca1c4e84da4 158 conf.Gyro_Dir[idx] *= -1;
komaida424 0:cca1c4e84da4 159 }
komaida424 0:cca1c4e84da4 160 if ( conf.Gyro_Dir[idx] == 1 )
komaida424 0:cca1c4e84da4 161 lcd.printf("Normal ");
komaida424 0:cca1c4e84da4 162 else
komaida424 0:cca1c4e84da4 163 lcd.printf("Reverse");
komaida424 0:cca1c4e84da4 164 if ( mode == 34 )
komaida424 0:cca1c4e84da4 165 Set_Arrow(3);
komaida424 0:cca1c4e84da4 166 else
komaida424 0:cca1c4e84da4 167 Set_Arrow(2);
komaida424 0:cca1c4e84da4 168 break;
komaida424 0:cca1c4e84da4 169 case 40: //Set Accelerometer
komaida424 0:cca1c4e84da4 170 lcd.printf("Acceleration");
komaida424 0:cca1c4e84da4 171 lcd.locate(5,1);
komaida424 0:cca1c4e84da4 172 lcd.printf("Gain");
komaida424 0:cca1c4e84da4 173 Set_Arrow(1);
komaida424 0:cca1c4e84da4 174 hmax = 3;
komaida424 0:cca1c4e84da4 175 break;
komaida424 0:cca1c4e84da4 176 case 41:
komaida424 0:cca1c4e84da4 177 Param_Set_Prompt1("Accel>Roll",&conf.Accel_Gain[0],2,0.00f,1.00f,0.01f,sw);
komaida424 0:cca1c4e84da4 178 break;
komaida424 0:cca1c4e84da4 179 case 42: //Set Accelerometer
komaida424 0:cca1c4e84da4 180 Param_Set_Prompt1("Accel>Pith",&conf.Accel_Gain[1],2,0.00f,1.00f,0.01f,sw);
komaida424 0:cca1c4e84da4 181 break;
komaida424 0:cca1c4e84da4 182 case 43: //Set Accelerometer
komaida424 0:cca1c4e84da4 183 ret_mode = 'R';
komaida424 0:cca1c4e84da4 184 Param_Set_Prompt1("Accel>Yaw",&conf.Accel_Gain[2],3,0.00f,1.00f,0.01f,sw);
komaida424 0:cca1c4e84da4 185 break;
komaida424 0:cca1c4e84da4 186
komaida424 0:cca1c4e84da4 187 case 50: //Set Stick Mixing
komaida424 0:cca1c4e84da4 188 lcd.printf("Set Stick Mixing");
komaida424 0:cca1c4e84da4 189 Set_Arrow(1);
komaida424 0:cca1c4e84da4 190 hmax = 3;
komaida424 0:cca1c4e84da4 191 break;
komaida424 0:cca1c4e84da4 192 case 51: //Set Stick Mixing
komaida424 0:cca1c4e84da4 193 Param_Set_Prompt1("Mixing>Roll",&conf.Stick_Mix[0],2,0.00f,2.00f,0.01f,sw);
komaida424 0:cca1c4e84da4 194 break;
komaida424 0:cca1c4e84da4 195 case 52:
komaida424 0:cca1c4e84da4 196 Param_Set_Prompt1("Mixing>Pitch",&conf.Stick_Mix[1],2,0.00f,2.00f,0.01f,sw);
komaida424 0:cca1c4e84da4 197 break;
komaida424 0:cca1c4e84da4 198 case 53:
komaida424 0:cca1c4e84da4 199 Param_Set_Prompt1("Mixing>Yaw",&conf.Stick_Mix[2],3,0.00f,2.00f,0.01f,sw);
komaida424 0:cca1c4e84da4 200 break;
komaida424 0:cca1c4e84da4 201 case 60: //Display Pulse Width
komaida424 0:cca1c4e84da4 202 lcd.printf("Disp Pulse Width");
komaida424 0:cca1c4e84da4 203 Set_Arrow(1);
komaida424 0:cca1c4e84da4 204 hmax = 2;
komaida424 0:cca1c4e84da4 205 break;
komaida424 0:cca1c4e84da4 206 case 61: //Display Pulse Width
komaida424 0:cca1c4e84da4 207 // DisplayPulseWidth(THR,AIL,ELE,RUD,AUX);
komaida424 0:cca1c4e84da4 208 ret_mode = 'R';
komaida424 0:cca1c4e84da4 209 lcd.locate(0,0);
komaida424 0:cca1c4e84da4 210 sprintf(str,"TR=%4d,AL=%4d",THR,AIL);
komaida424 0:cca1c4e84da4 211 lcd.printf(str);
komaida424 0:cca1c4e84da4 212 lcd.locate(0,1);
komaida424 0:cca1c4e84da4 213 sprintf(str,"EL=%4d,RD=%4d",ELE,RUD);
komaida424 0:cca1c4e84da4 214 lcd.printf(str);
komaida424 0:cca1c4e84da4 215 break;
komaida424 0:cca1c4e84da4 216 case 62: //Display Stick Ref
komaida424 0:cca1c4e84da4 217 // DisplayPulseWidth(Stick[COL],Stick[ROL],Stick[PIT],Stick[YAW],Stick[GAIN]);
komaida424 0:cca1c4e84da4 218 ret_mode = 'R';
komaida424 0:cca1c4e84da4 219 Get_Stick_Pos();
komaida424 0:cca1c4e84da4 220 lcd.locate(0,0);
komaida424 0:cca1c4e84da4 221 sprintf(str,"TR=%4d,AL=%4d",Stick[COL],Stick[ROL]);
komaida424 0:cca1c4e84da4 222 lcd.printf(str);
komaida424 0:cca1c4e84da4 223 lcd.locate(0,1);
komaida424 0:cca1c4e84da4 224 sprintf(str,"EL=%4d,RD=%4d",Stick[PIT],Stick[YAW]);
komaida424 0:cca1c4e84da4 225 lcd.printf(str);
komaida424 0:cca1c4e84da4 226 break;
komaida424 0:cca1c4e84da4 227 case 70: //Display Sensor Value
komaida424 0:cca1c4e84da4 228 lcd.printf("Disp Senser");
komaida424 0:cca1c4e84da4 229 Set_Arrow(1);
komaida424 0:cca1c4e84da4 230 hmax = 5;
komaida424 0:cca1c4e84da4 231 break;
komaida424 0:cca1c4e84da4 232 case 71: //Gyro
komaida424 0:cca1c4e84da4 233 Get_Gyro();
komaida424 0:cca1c4e84da4 234 lcd.locate(0,0);
komaida424 0:cca1c4e84da4 235 sprintf(str,"Gyro//X=%5d",Gyro[ROL]);
komaida424 0:cca1c4e84da4 236 lcd.printf(str);
komaida424 0:cca1c4e84da4 237 wait(0.02);
komaida424 0:cca1c4e84da4 238 lcd.locate(0,1);
komaida424 0:cca1c4e84da4 239 sprintf(str,"y=%5d,Z=%5d",Gyro[PIT],Gyro[YAW]);
komaida424 0:cca1c4e84da4 240 lcd.printf(str);
komaida424 0:cca1c4e84da4 241 wait(0.02);
komaida424 0:cca1c4e84da4 242 ret_mode = 'R';
komaida424 0:cca1c4e84da4 243 break;
komaida424 0:cca1c4e84da4 244 case 72: //Accelerometer
komaida424 0:cca1c4e84da4 245 Get_Accel();
komaida424 0:cca1c4e84da4 246 lcd.locate(0,0);
komaida424 0:cca1c4e84da4 247 sprintf(str,"Accel//X=%5d",Accel[ROL]);
komaida424 0:cca1c4e84da4 248 lcd.printf(str);
komaida424 0:cca1c4e84da4 249 lcd.locate(0,1);
komaida424 0:cca1c4e84da4 250 sprintf(str,"Y=%5d,Z=%5d",Accel[PIT],Accel[YAW]);
komaida424 0:cca1c4e84da4 251 lcd.printf(str);
komaida424 0:cca1c4e84da4 252 // Set_Arrow(2);
komaida424 0:cca1c4e84da4 253 ret_mode = 'R';
komaida424 0:cca1c4e84da4 254 break;
komaida424 0:cca1c4e84da4 255 case 73: // Pressure
komaida424 0:cca1c4e84da4 256 elaps.reset();
komaida424 0:cca1c4e84da4 257 elaps.start();
komaida424 0:cca1c4e84da4 258 Get_Pressure();
komaida424 0:cca1c4e84da4 259 elaps.stop();
komaida424 0:cca1c4e84da4 260 lcd.locate(0,0);
komaida424 0:cca1c4e84da4 261 sprintf(str,"Pressure=%9.3f",Press/4096);
komaida424 0:cca1c4e84da4 262 lcd.printf(str);
komaida424 0:cca1c4e84da4 263 lcd.locate(0,1);
komaida424 0:cca1c4e84da4 264 sprintf(str,"Elaps=%6d",elaps.read_us());
komaida424 0:cca1c4e84da4 265 lcd.printf(str);
komaida424 0:cca1c4e84da4 266 // Set_Arrow(2);
komaida424 0:cca1c4e84da4 267 ret_mode = 'R';
komaida424 0:cca1c4e84da4 268 break;
komaida424 0:cca1c4e84da4 269 case 74:
komaida424 0:cca1c4e84da4 270 elaps.reset();
komaida424 0:cca1c4e84da4 271 elaps.start();
komaida424 0:cca1c4e84da4 272 PWM_Out(false);
komaida424 0:cca1c4e84da4 273 elaps.stop();
komaida424 0:cca1c4e84da4 274 lcd.locate(0,0);
komaida424 0:cca1c4e84da4 275 sprintf(str,"ElapsTime=%5d",elaps.read_us());
komaida424 0:cca1c4e84da4 276 lcd.printf(str);
komaida424 0:cca1c4e84da4 277 Set_Arrow(2);
komaida424 0:cca1c4e84da4 278 ret_mode = 'R';
komaida424 0:cca1c4e84da4 279 break;
komaida424 0:cca1c4e84da4 280 case 75: //Sensor Calibration
komaida424 0:cca1c4e84da4 281 // CalibrateAccel();
komaida424 0:cca1c4e84da4 282 CalibrateGyros();
komaida424 0:cca1c4e84da4 283 CalibrateAccel();
komaida424 0:cca1c4e84da4 284 lcd.printf("Calibrate completed");
komaida424 0:cca1c4e84da4 285 Set_Arrow(3);
komaida424 0:cca1c4e84da4 286 break;
komaida424 0:cca1c4e84da4 287 case 80: //Display PMW Condition
komaida424 0:cca1c4e84da4 288 lcd.printf("Display PMW ");
komaida424 0:cca1c4e84da4 289 Set_Arrow(1);
komaida424 0:cca1c4e84da4 290 hmax = 1;
komaida424 0:cca1c4e84da4 291 break;
komaida424 0:cca1c4e84da4 292 case 81: //Display PMW Width
komaida424 0:cca1c4e84da4 293 PWM_Out(false);
komaida424 0:cca1c4e84da4 294 // DisplayInt(Motor_HD[0],M1,M2,M4,M3);
komaida424 0:cca1c4e84da4 295 ret_mode = 'R';
komaida424 0:cca1c4e84da4 296 lcd.locate(0,0);
komaida424 0:cca1c4e84da4 297 sprintf(str,"M1=%4d,M2=%4d",M1,M2);
komaida424 0:cca1c4e84da4 298 lcd.printf(str);
komaida424 0:cca1c4e84da4 299 lcd.locate(0,1);
komaida424 0:cca1c4e84da4 300 sprintf(str,"M4=%4d,M3=%4d",M4,M3);
komaida424 0:cca1c4e84da4 301 lcd.printf(str);
komaida424 0:cca1c4e84da4 302 break;
komaida424 0:cca1c4e84da4 303 case 90: //パラメーター設定
komaida424 0:cca1c4e84da4 304 lcd.printf("Parameter Set");
komaida424 0:cca1c4e84da4 305 Set_Arrow(1);
komaida424 0:cca1c4e84da4 306 hmax = 4;
komaida424 0:cca1c4e84da4 307 break;
komaida424 0:cca1c4e84da4 308 case 91:
komaida424 0:cca1c4e84da4 309 Param_Set_Prompt1("LCD>Contrast",&conf.LCD_Contrast,2,0,63,1,sw);
komaida424 0:cca1c4e84da4 310 break;
komaida424 0:cca1c4e84da4 311 case 92:
komaida424 0:cca1c4e84da4 312 lcd.locate(0,0);
komaida424 0:cca1c4e84da4 313 lcd.printf("PWM>Mode");
komaida424 0:cca1c4e84da4 314 lcd.locate(0,1);
komaida424 0:cca1c4e84da4 315 switch ( sw ) {
komaida424 0:cca1c4e84da4 316 case 'U':
komaida424 0:cca1c4e84da4 317 case 'D':
komaida424 0:cca1c4e84da4 318 conf.PWM_Mode *= -1;
komaida424 0:cca1c4e84da4 319 }
komaida424 0:cca1c4e84da4 320 if ( conf.PWM_Mode == 1 )
komaida424 0:cca1c4e84da4 321 lcd.printf("ESC ");
komaida424 0:cca1c4e84da4 322 else
komaida424 0:cca1c4e84da4 323 lcd.printf("Moter");
komaida424 0:cca1c4e84da4 324 Set_Arrow(2);
komaida424 0:cca1c4e84da4 325 break;
komaida424 0:cca1c4e84da4 326 case 93:
komaida424 0:cca1c4e84da4 327 Param_Set_Prompt1("PWM>Interval",&conf.PWM_Interval,2,Thro_Hi,10000,10,sw);
komaida424 0:cca1c4e84da4 328 // for ( i=0;i<4;i++ ) pwm[i].period_us(conf.PWM_Interval);
komaida424 0:cca1c4e84da4 329 break;
komaida424 0:cca1c4e84da4 330 case 94:
komaida424 0:cca1c4e84da4 331 lcd.locate(0,0);
komaida424 0:cca1c4e84da4 332 lcd.printf("Gyro>Type");
komaida424 0:cca1c4e84da4 333 lcd.locate(0,1);
komaida424 0:cca1c4e84da4 334 switch ( sw ) {
komaida424 0:cca1c4e84da4 335 case 'U':
komaida424 0:cca1c4e84da4 336 case 'D':
komaida424 0:cca1c4e84da4 337 if ( conf.Gyro_Type == _L3GD20 )
komaida424 0:cca1c4e84da4 338 conf.Gyro_Type = _ITG3200;
komaida424 0:cca1c4e84da4 339 else
komaida424 0:cca1c4e84da4 340 conf.Gyro_Type = _L3GD20;
komaida424 0:cca1c4e84da4 341 }
komaida424 0:cca1c4e84da4 342 if ( conf.Gyro_Type == _L3GD20 )
komaida424 0:cca1c4e84da4 343 lcd.printf("L3GD20");
komaida424 0:cca1c4e84da4 344 else
komaida424 0:cca1c4e84da4 345 lcd.printf("ITG3200");
komaida424 0:cca1c4e84da4 346 Set_Arrow(2);
komaida424 0:cca1c4e84da4 347 break;
komaida424 0:cca1c4e84da4 348 case 100: //E2PROM Store
komaida424 0:cca1c4e84da4 349 lcd.printf("Config Save");
komaida424 0:cca1c4e84da4 350 Set_Arrow(1);
komaida424 0:cca1c4e84da4 351 hmax = 1;
komaida424 0:cca1c4e84da4 352 break;
komaida424 0:cca1c4e84da4 353 case 101:
komaida424 0:cca1c4e84da4 354 WriteConfig();
komaida424 0:cca1c4e84da4 355 FlashLED(5);
komaida424 0:cca1c4e84da4 356 lcd.locate(0,0);
komaida424 0:cca1c4e84da4 357 sprintf(str,"Config %3dbyte",sizeof(config));
komaida424 0:cca1c4e84da4 358 lcd.printf(str);
komaida424 0:cca1c4e84da4 359 lcd.locate(0,1);
komaida424 0:cca1c4e84da4 360 lcd.printf("Save Sucssesuful");
komaida424 0:cca1c4e84da4 361 Set_Arrow(3);
komaida424 0:cca1c4e84da4 362 wait(2);
komaida424 0:cca1c4e84da4 363 break;
komaida424 0:cca1c4e84da4 364 case 110: //ESC Set up
komaida424 0:cca1c4e84da4 365 lcd.locate(0,0);
komaida424 0:cca1c4e84da4 366 lcd.printf("ESC Set UP");
komaida424 0:cca1c4e84da4 367 Set_Arrow(1);
komaida424 0:cca1c4e84da4 368 hmax = 1;
komaida424 0:cca1c4e84da4 369 break;
komaida424 0:cca1c4e84da4 370 case 111:
komaida424 0:cca1c4e84da4 371 if ( conf.PWM_Mode == -1 ) {
komaida424 0:cca1c4e84da4 372 hnum = 0;
komaida424 0:cca1c4e84da4 373 break;
komaida424 0:cca1c4e84da4 374 }
komaida424 0:cca1c4e84da4 375 lcd.locate(0,0);
komaida424 0:cca1c4e84da4 376 lcd.printf("Setup Start");
komaida424 0:cca1c4e84da4 377 wait(1.5);
komaida424 0:cca1c4e84da4 378 lcd.cls(); //Clear LCD
komaida424 0:cca1c4e84da4 379 lcd.locate(0,0);
komaida424 0:cca1c4e84da4 380 lcd.printf("Please power-off");
komaida424 0:cca1c4e84da4 381 lcd.locate(1,0);
komaida424 0:cca1c4e84da4 382 lcd.printf(" after Setup");
komaida424 0:cca1c4e84da4 383
komaida424 0:cca1c4e84da4 384 ESC_SetUp();
komaida424 0:cca1c4e84da4 385 break;
komaida424 0:cca1c4e84da4 386
komaida424 0:cca1c4e84da4 387 default:
komaida424 0:cca1c4e84da4 388 if ( hnum == 0 )
komaida424 0:cca1c4e84da4 389 vnum++;
komaida424 0:cca1c4e84da4 390 }
komaida424 0:cca1c4e84da4 391
komaida424 0:cca1c4e84da4 392
komaida424 0:cca1c4e84da4 393 sw = Check_Stick_Dir(ret_mode); //Wait Mode
komaida424 0:cca1c4e84da4 394
komaida424 0:cca1c4e84da4 395 switch ( sw ) {
komaida424 0:cca1c4e84da4 396 case 'L':
komaida424 0:cca1c4e84da4 397 hnum--;
komaida424 0:cca1c4e84da4 398 if ( hnum <= 0 ) hnum = 0;
komaida424 0:cca1c4e84da4 399 lcd.cls(); //Clear LCD
komaida424 0:cca1c4e84da4 400 break;
komaida424 0:cca1c4e84da4 401 case 'R':
komaida424 0:cca1c4e84da4 402 lcd.cls();
komaida424 0:cca1c4e84da4 403 if ( hnum < hmax ) hnum++;
komaida424 0:cca1c4e84da4 404 break;
komaida424 0:cca1c4e84da4 405 case 'U':
komaida424 0:cca1c4e84da4 406 if ( hnum == 0 ) {
komaida424 0:cca1c4e84da4 407 if ( vnum < vmax ) vnum++;
komaida424 0:cca1c4e84da4 408 else vnum = 0;
komaida424 0:cca1c4e84da4 409 lcd.cls(); //Clear LCD
komaida424 0:cca1c4e84da4 410 }
komaida424 0:cca1c4e84da4 411 break;
komaida424 0:cca1c4e84da4 412 case 'D':
komaida424 0:cca1c4e84da4 413 if ( hnum == 0 ) {
komaida424 0:cca1c4e84da4 414 if ( vnum > 0 ) vnum--;
komaida424 0:cca1c4e84da4 415 else vnum = vmax;
komaida424 0:cca1c4e84da4 416 lcd.cls(); //Clear LCD
komaida424 0:cca1c4e84da4 417 }
komaida424 0:cca1c4e84da4 418 break;
komaida424 0:cca1c4e84da4 419 case 'E':
komaida424 0:cca1c4e84da4 420 lcd.cls(); //Clear LCD
komaida424 0:cca1c4e84da4 421 lcd.locate(0,0);
komaida424 0:cca1c4e84da4 422 lcd.printf("PWM Started");
komaida424 0:cca1c4e84da4 423 return;
komaida424 0:cca1c4e84da4 424 }
komaida424 0:cca1c4e84da4 425 }
komaida424 0:cca1c4e84da4 426
komaida424 0:cca1c4e84da4 427 }
komaida424 0:cca1c4e84da4 428
komaida424 0:cca1c4e84da4 429 char Check_Stick_Dir(char act)
komaida424 0:cca1c4e84da4 430 {
komaida424 0:cca1c4e84da4 431 int i;
komaida424 0:cca1c4e84da4 432 while ( 1 ) {
komaida424 0:cca1c4e84da4 433 Get_Stick_Pos();
komaida424 0:cca1c4e84da4 434 // DisplayInt(Stick[0],Stick[1],Stick[2],Stick[3]);
komaida424 0:cca1c4e84da4 435 // wait(0.2);
komaida424 0:cca1c4e84da4 436 if ( Stick[YAW] > Stick_Limit ) {
komaida424 0:cca1c4e84da4 437 i = 0;
komaida424 0:cca1c4e84da4 438 while ( Stick[YAW] > Stick_Limit && Stick[COL] < 30 ) {
komaida424 0:cca1c4e84da4 439 if ( i > 2000 ) return 'E'; //wait 2 sec
komaida424 0:cca1c4e84da4 440 wait(0.001); // wait 1 msec
komaida424 0:cca1c4e84da4 441 Get_Stick_Pos();
komaida424 0:cca1c4e84da4 442 i++;
komaida424 0:cca1c4e84da4 443 }
komaida424 0:cca1c4e84da4 444 }
komaida424 0:cca1c4e84da4 445 if ( Stick[ROL] > Stick_Limit ) {
komaida424 0:cca1c4e84da4 446 wait(0.03);
komaida424 0:cca1c4e84da4 447 Get_Stick_Pos();
komaida424 0:cca1c4e84da4 448 if ( !(Stick[ROL] > Stick_Limit) ) continue;
komaida424 0:cca1c4e84da4 449 while ( Stick[ROL] > Stick_Limit ) {
komaida424 0:cca1c4e84da4 450 Get_Stick_Pos();
komaida424 0:cca1c4e84da4 451 }
komaida424 0:cca1c4e84da4 452 return 'R';
komaida424 0:cca1c4e84da4 453 }
komaida424 0:cca1c4e84da4 454 if ( Stick[ROL] < -Stick_Limit ) {
komaida424 0:cca1c4e84da4 455 wait(0.03);
komaida424 0:cca1c4e84da4 456 Get_Stick_Pos();
komaida424 0:cca1c4e84da4 457 if ( !(Stick[ROL] < -Stick_Limit) ) continue;
komaida424 0:cca1c4e84da4 458 while ( Stick[ROL] < -Stick_Limit ) {
komaida424 0:cca1c4e84da4 459 Get_Stick_Pos();
komaida424 0:cca1c4e84da4 460 }
komaida424 0:cca1c4e84da4 461 return 'L';
komaida424 0:cca1c4e84da4 462 }
komaida424 0:cca1c4e84da4 463 if ( Stick[PIT] < -Stick_Limit ) {
komaida424 0:cca1c4e84da4 464 wait(0.03);
komaida424 0:cca1c4e84da4 465 Get_Stick_Pos();
komaida424 0:cca1c4e84da4 466 if ( !(Stick[PIT] < -Stick_Limit) ) continue;
komaida424 0:cca1c4e84da4 467 if ( act == 'R' ) {
komaida424 0:cca1c4e84da4 468 wait(0.03);
komaida424 0:cca1c4e84da4 469 return 'D';
komaida424 0:cca1c4e84da4 470 }
komaida424 0:cca1c4e84da4 471 while ( Stick[PIT] < -Stick_Limit ) {
komaida424 0:cca1c4e84da4 472 Get_Stick_Pos();
komaida424 0:cca1c4e84da4 473 }
komaida424 0:cca1c4e84da4 474 return 'D';
komaida424 0:cca1c4e84da4 475 }
komaida424 0:cca1c4e84da4 476 if ( Stick[PIT] > Stick_Limit ) {
komaida424 0:cca1c4e84da4 477 wait(0.03);
komaida424 0:cca1c4e84da4 478 Get_Stick_Pos();
komaida424 0:cca1c4e84da4 479 if ( !( Stick[PIT] > Stick_Limit) ) continue;
komaida424 0:cca1c4e84da4 480 if ( act == 'R' ) {
komaida424 0:cca1c4e84da4 481 wait(0.03);
komaida424 0:cca1c4e84da4 482 return 'U';
komaida424 0:cca1c4e84da4 483 }
komaida424 0:cca1c4e84da4 484 while ( Stick[PIT] > Stick_Limit ) {
komaida424 0:cca1c4e84da4 485 Get_Stick_Pos();
komaida424 0:cca1c4e84da4 486 }
komaida424 0:cca1c4e84da4 487 return 'U';
komaida424 0:cca1c4e84da4 488 }
komaida424 0:cca1c4e84da4 489 if ( act == 'R' )
komaida424 0:cca1c4e84da4 490 return ' ';
komaida424 0:cca1c4e84da4 491 }
komaida424 0:cca1c4e84da4 492 }
komaida424 0:cca1c4e84da4 493
komaida424 0:cca1c4e84da4 494 void Param_Set_Prompt1(const char *hd,int *num,int arrow,int min,int max,int increase,char sw)
komaida424 0:cca1c4e84da4 495 {
komaida424 0:cca1c4e84da4 496 ret_mode = 'R';
komaida424 0:cca1c4e84da4 497 lcdptr->locate(0,0);
komaida424 0:cca1c4e84da4 498 lcdptr->printf(hd);
komaida424 0:cca1c4e84da4 499 lcdptr->locate(0,1);
komaida424 0:cca1c4e84da4 500 lcdptr->printf("%d",*num);
komaida424 0:cca1c4e84da4 501 Set_Arrow(arrow);
komaida424 0:cca1c4e84da4 502 switch ( sw ) {
komaida424 0:cca1c4e84da4 503 case 'U':
komaida424 0:cca1c4e84da4 504 *num -= increase;
komaida424 0:cca1c4e84da4 505 if ( *num <= min )
komaida424 0:cca1c4e84da4 506 *num = min;
komaida424 0:cca1c4e84da4 507 break;
komaida424 0:cca1c4e84da4 508 case 'D':
komaida424 0:cca1c4e84da4 509 *num += increase;
komaida424 0:cca1c4e84da4 510 if ( *num >= max )
komaida424 0:cca1c4e84da4 511 *num = max;
komaida424 0:cca1c4e84da4 512 }
komaida424 0:cca1c4e84da4 513 }
komaida424 0:cca1c4e84da4 514 void Param_Set_Prompt1(const char *hd,float *num,int arrow,float min,float max,float increase,char sw)
komaida424 0:cca1c4e84da4 515 {
komaida424 0:cca1c4e84da4 516 ret_mode = 'R';
komaida424 0:cca1c4e84da4 517 lcdptr->locate(0,0);
komaida424 0:cca1c4e84da4 518 lcdptr->printf(hd);
komaida424 0:cca1c4e84da4 519 lcdptr->locate(0,1);
komaida424 0:cca1c4e84da4 520 sprintf(str,"%7.2f",*num);
komaida424 0:cca1c4e84da4 521 lcdptr->printf(str);
komaida424 0:cca1c4e84da4 522 Set_Arrow(arrow);
komaida424 0:cca1c4e84da4 523 switch ( sw ) {
komaida424 0:cca1c4e84da4 524 case 'U':
komaida424 0:cca1c4e84da4 525 *num -= increase;
komaida424 0:cca1c4e84da4 526 if ( *num <= min )
komaida424 0:cca1c4e84da4 527 *num = min;
komaida424 0:cca1c4e84da4 528 break;
komaida424 0:cca1c4e84da4 529 case 'D':
komaida424 0:cca1c4e84da4 530 *num += increase;
komaida424 0:cca1c4e84da4 531 if ( *num >= max )
komaida424 0:cca1c4e84da4 532 *num = max;
komaida424 0:cca1c4e84da4 533 }
komaida424 0:cca1c4e84da4 534 }
komaida424 0:cca1c4e84da4 535
komaida424 0:cca1c4e84da4 536 void Set_Arrow(int dir)
komaida424 0:cca1c4e84da4 537 {
komaida424 0:cca1c4e84da4 538 lcdptr->locate(12,1);
komaida424 0:cca1c4e84da4 539 switch ( dir ) {
komaida424 0:cca1c4e84da4 540 case 1:
komaida424 0:cca1c4e84da4 541 lcdptr->printf(" >>");
komaida424 0:cca1c4e84da4 542 break;
komaida424 0:cca1c4e84da4 543 case 2:
komaida424 0:cca1c4e84da4 544 lcdptr->printf("<<>>");
komaida424 0:cca1c4e84da4 545 break;
komaida424 0:cca1c4e84da4 546 case 3:
komaida424 0:cca1c4e84da4 547 lcdptr->printf(" <<");
komaida424 0:cca1c4e84da4 548 }
komaida424 0:cca1c4e84da4 549 }
komaida424 0:cca1c4e84da4 550
komaida424 0:cca1c4e84da4 551
komaida424 0:cca1c4e84da4 552
komaida424 0:cca1c4e84da4 553
komaida424 0:cca1c4e84da4 554
komaida424 0:cca1c4e84da4 555