syouichi imamori
/
MulticopterQuadX
Quad X Type Multicopter
config.cpp
- Committer:
- komaida424
- Date:
- 2013-11-15
- Revision:
- 2:59ac9df97701
- Parent:
- 0:cca1c4e84da4
- Child:
- 3:27407c4984cf
File content as of revision 2:59ac9df97701:
#include "mbed.h" #include "I2cPeripherals.h" #include "InterruptIn.h" #include "config.h" #include "PulseWidthCounter.h" #include "SerialLcd.h" #include "PID.h" //Serial pc(USBTX, USBRX); #define CALIBURATE 10 #define GYROGAIN 20 #define GYRODIR 30 #define ACCELCORRECT 40 #define PIDSET 50 #define STICKMIX 60 #define DISPPULSE 70 #define DISPSENSOR 80 #define DISPPWM 90 #define PARMSET 100 #define CONFSTORE 110 #define CONFRESET 120 void FlashLED(int); char Check_Stick_Dir(char); void Param_Set_Prompt1(char *,int *,int,int,int,int,char); void Param_Set_Prompt1(char *,float *,int,float,float,float,char); void Set_Arrow(int dir); void Get_Stick_Pos(); void CalibrateGyros(void); void CalibrateAccel(void); void Get_Gyro(); void Get_Accel(); void Get_Angle(float); void PWM_Out(bool); void WriteConfig(); void ESC_SetUp(void); void Get_Pressure(); void LCD_printf(char *); void LCD_cls(); void LCD_locate(int,int); Timer elaps; extern volatile int CH[5]; extern volatile int M[6]; extern volatile float Gyro[3]; extern volatile float Accel[3]; extern volatile float Angle[3]; extern volatile float Gyro_Ref[3]; extern volatile int Stick[5]; extern volatile float Press; extern volatile float interval; //extern bool tick_flag; extern PID pid[3]; extern int pid_reg[3]; const char steering[3][6]= {"Roll ","Pitch","Yaw "}; short mode; char sw,ret_mode; short vnum,hnum,vmax,hmax; short idx,i; char str[33]; config init; void SetUpPrompt(config& conf,I2cPeripherals& i2c) { float x,y,z; LCD_cls(); mode = 0; vnum = 0; hnum = 0; vmax = 12; while( 1 ) { // FlashLED(1); ret_mode = 'W'; mode = vnum * 10 + hnum; switch ( mode ) { case 0: LCD_locate(0,0); sprintf(str,"Quad-X Ver %4.2f",conf.Revision); LCD_printf(str); LCD_locate(4,1); LCD_printf("By AZUKITEN"); hmax = 0; break; case CALIBURATE: //Calibrate Transmitter LCD_printf("Calibrate"); Set_Arrow(1); hmax = 1; break; case CALIBURATE+1: //Calibrate Transmitter LCD_printf("Start Calibrate"); for(i=0; i<4; i++) { conf.Stick_Ref[i] = 0; } for(i=0; i<16; i++) { wait(0.3); conf.Stick_Ref[ROL] += AIL; conf.Stick_Ref[PIT] += ELE; conf.Stick_Ref[YAW] += RUD; conf.Stick_Ref[COL] += THR; // conf.Stick_Ref[GAIN] += AUX; } for(i=0; i<4; i++) { conf.Stick_Ref[i] = conf.Stick_Ref[i]/16; } CalibrateGyros(); CalibrateAccel(); LCD_cls(); //Clear LCD LCD_printf("Calibrate Completed"); Set_Arrow(3); FlashLED(5); hnum = 0; break; case GYROGAIN: //Set Gyro Gain LCD_printf("Set Gyro Gain"); Set_Arrow(1); hmax = 4; break; case GYROGAIN+1: //Set Gyro Gain Roll if ( conf.Gyro_Gain_Setting == 1 ) Param_Set_Prompt1("GyroGain>Roll",&conf.Gyro_Gain[0],2,0.00f,1.00f,0.01f,sw); else Param_Set_Prompt1("GyroGain>Roll",&conf.Gyro_Gain[3],2,-1.00f,1.00f,0.01f,sw); break; case GYROGAIN+2: if ( conf.Gyro_Gain_Setting == 1 ) Param_Set_Prompt1("GyroGain>Pitch",&conf.Gyro_Gain[1],2,0.00f,1.00f,0.01f,sw); else Param_Set_Prompt1("GyroGain>Pitch",&conf.Gyro_Gain[4],2,-1.00f,1.00f,0.01f,sw); break; case GYROGAIN+3: if ( conf.Gyro_Gain_Setting == 1 ) Param_Set_Prompt1("GyroGain>Yaw",&conf.Gyro_Gain[2],2,0.00f,1.00f,0.01f,sw); else Param_Set_Prompt1("GyroGain>Yaw",&conf.Gyro_Gain[5],2,-1.00f,1.00f,0.01f,sw); break; case GYROGAIN+4: // ret_mode = 'R'; LCD_printf("GyroGain>setting"); LCD_locate(0,1); switch ( sw ) { case 'U': case 'D': conf.Gyro_Gain_Setting *= -1; } if ( conf.Gyro_Gain_Setting == 1 ) LCD_printf("Controller"); else LCD_printf("Transmitter"); Set_Arrow(3); break; case GYRODIR: //Set Gyro Direction LCD_printf("Gyro Direction"); Set_Arrow(1); hmax = 4; break; case GYRODIR+1: //Set Gyro Direction Roll case GYRODIR+2: case GYRODIR+3: case GYRODIR+4: // ret_mode = 'R'; idx = mode - (GYRODIR+1); if ( mode == (GYRODIR+4) ) LCD_printf("Gyro>Swap X-Y"); else { LCD_printf("Gyro>Dir>"); LCD_locate(9,0); LCD_printf((char*)steering[idx]); } LCD_locate(0,1); switch ( sw ) { case 'U': case 'D': conf.Gyro_Dir[idx] *= -1; } if ( conf.Gyro_Dir[idx] == 1 ) LCD_printf("Normal "); else LCD_printf("Reverse"); if ( mode == (GYRODIR+4) ) Set_Arrow(3); else Set_Arrow(2); break; case ACCELCORRECT: LCD_printf("Acceleration"); LCD_locate(2,1); LCD_printf("correction"); Set_Arrow(1); hmax = 3; break; case ACCELCORRECT+1: Param_Set_Prompt1("Accel>ROL",&conf.Accel_Ref[ROL],2,-10.0,10.0f,0.01f,sw); break; case ACCELCORRECT+2: Param_Set_Prompt1("Accel>PIT",&conf.Accel_Ref[PIT],2,-10.0,10.0f,0.01f,sw); break; case ACCELCORRECT+3: Param_Set_Prompt1("Accel>YAW",&conf.Accel_Ref[YAW],3,-10.0,10.0f,0.01f,sw); break; case PIDSET: //PID Setting LCD_printf("PID Setting"); Set_Arrow(1); hmax = 8; break; case PIDSET+1: Param_Set_Prompt1("PID>RoolPitch>kp",&conf.kp[0],2,0.00f,5.00f,0.01f,sw); conf.kp[1] = conf.kp[0]; break; case PIDSET+2: Param_Set_Prompt1("PID>RoolPitch>ki",&conf.ki[0],2,0.00f,5.00f,0.01f,sw); conf.ki[1] = conf.ki[0]; break; case PIDSET+3: Param_Set_Prompt1("PID>RoolPitch>kd",&conf.kd[0],2,0.00f,5.00f,0.01f,sw); conf.kd[1] = conf.kd[0]; break; case PIDSET+4: Param_Set_Prompt1("PID>YAW>kp",&conf.kp[2],2,0.00f,5.00f,0.01f,sw); break; case PIDSET+5: Param_Set_Prompt1("PID>YAW>ki",&conf.ki[2],2,0.00f,5.00f,0.01f,sw); break; case PIDSET+6: Param_Set_Prompt1("PID>YAW>kd",&conf.kd[2],2,0.00f,5.00f,0.01f,sw); break; case PIDSET+7: Param_Set_Prompt1("PID Limit",&conf.PID_Limit,2,0.00f,400.00f,10.00f,sw); break; case PIDSET+8: Param_Set_Prompt1("Integral Limit",&conf.Integral_Limit,3,0.00f,100.00f,1.00f,sw); break; case STICKMIX: //Set Stick Mixing LCD_printf("Set Stick Mixing"); Set_Arrow(1); hmax = 3; break; case STICKMIX+1: //Set Stick Mixing Param_Set_Prompt1("Mixing>Roll",&conf.Stick_Mix[0],2,0.00f,2.00f,0.01f,sw); break; case STICKMIX+2: Param_Set_Prompt1("Mixing>Pitch",&conf.Stick_Mix[1],2,0.00f,2.00f,0.01f,sw); break; case STICKMIX+3: Param_Set_Prompt1("Mixing>Yaw",&conf.Stick_Mix[2],3,0.00f,2.00f,0.01f,sw); break; case DISPPULSE: //Display Pulse Width LCD_printf("Disp Pulse Width"); Set_Arrow(1); hmax = 2; break; case DISPPULSE+1: //Display Pulse Width // DisplayPulseWidth(THR,AIL,ELE,RUD,AUX); ret_mode = 'R'; LCD_locate(0,0); sprintf(str,"TR=%4d,AL=%4d",THR,AIL); LCD_printf(str); LCD_locate(0,1); sprintf(str,"EL=%4d,RD=%4d",ELE,RUD); LCD_printf(str); break; case DISPPULSE+2: //Display Stick Ref ret_mode = 'R'; Get_Stick_Pos(); LCD_locate(0,0); sprintf(str,"TR=%4d,AL=%4d",Stick[COL],Stick[ROL]); LCD_printf(str); LCD_locate(0,1); sprintf(str,"EL=%4d,RD=%4d",Stick[PIT],Stick[YAW]); LCD_printf(str); break; case DISPSENSOR: //Display Sensor Value LCD_printf("Disp Senser"); Set_Arrow(1); hmax = 7; for ( i=0; i<3; i++ ) { pid[i].init(conf.kp[i],conf.ki[i],conf.kd[i]*(float)abs(Stick[GAIN])/50.0 ,conf.PID_Limit,conf.Integral_Limit); Angle[i] = 0; } break; case DISPSENSOR+1: //Gyro Get_Gyro(); LCD_locate(0,0); sprintf(str,"[Gyro]X=%5.1f",Gyro[ROL]); LCD_printf(str); LCD_locate(0,1); sprintf(str,"y=%5.1f,Z=%5.1f",Gyro[PIT],Gyro[YAW]); LCD_printf(str); ret_mode = 'R'; break; case DISPSENSOR+2: //Accelerometer Get_Accel(); if ( conf.Gyro_Dir[3] ==1 ) i2c.Acceleration(&x,&y,&z); else i2c.Acceleration(&y,&x,&z); x -= conf.Accel_Ref[0]; y -= conf.Accel_Ref[1]; z -= conf.Accel_Ref[2]; LCD_locate(0,0); sprintf(str,"[Accel]X=%5.1f",x); LCD_printf(str); LCD_locate(0,1); sprintf(str,"Y=%5.1f,Z=%5.1f",y,z); LCD_printf(str); // Set_Arrow(2); ret_mode = 'R'; break; case DISPSENSOR+3: //angle PWM_Out(false); LCD_locate(0,0); sprintf(str,"[Angle]X=%6.1f",Angle[ROL]); LCD_printf(str); LCD_locate(0,1); sprintf(str,"Y=%6.1f,Z=%5.1f",Angle[PIT],Angle[YAW]); LCD_printf(str); // Set_Arrow(2); ret_mode = 'R'; break; case DISPSENSOR+4: PWM_Out(false); LCD_locate(0,0); sprintf(str,"t=%5d,a=%6.2f",-Stick[YAW]*45/400,Angle[YAW]); LCD_printf(str); LCD_locate(0,1); sprintf(str,"pid=%5d",pid_reg[YAW]); LCD_printf(str); Set_Arrow(2); ret_mode = 'R'; break; case DISPSENSOR+5: // Pressure elaps.reset(); elaps.start(); Get_Pressure(); elaps.stop(); LCD_locate(0,0); sprintf(str,"Press=%9.3f",Press/4096); LCD_printf(str); LCD_locate(0,1); sprintf(str,"Elaps=%6d",elaps.read_us()); LCD_printf(str); // Set_Arrow(2); ret_mode = 'R'; wait(0.05); break; case DISPSENSOR+6: elaps.reset(); elaps.start(); PWM_Out(false); elaps.stop(); i = elaps.read_us(); LCD_locate(0,0); sprintf(str,"ElapsTime=%6d",i); LCD_printf(str); Set_Arrow(2); ret_mode = 'R'; break; case DISPSENSOR+7: //Sensor Calibration CalibrateGyros(); FlashLED(3); LCD_printf("Calibrate completed"); Set_Arrow(3); break; case DISPPWM: //Display PWM Condition LCD_printf("Display PWM "); Set_Arrow(1); hmax = 1; break; case DISPPWM+1: //Display PWM Width ret_mode = 'R'; PWM_Out(false); LCD_locate(0,0); sprintf(str,"M1=%4d,M2=%4d",M1,M2); LCD_printf(str); LCD_locate(0,1); sprintf(str,"M4=%4d,M3=%4d",M4,M3); LCD_printf(str); break; case PARMSET: //パラメーター設定 LCD_printf("Parameter Set"); Set_Arrow(1); hmax = 6; break; case PARMSET+1: Param_Set_Prompt1("LCD>Contrast",&conf.LCD_Contrast,2,0,63,1,sw); break; case PARMSET+2: LCD_locate(0,0); LCD_printf("PWM>Mode"); LCD_locate(0,1); switch ( sw ) { case 'U': case 'D': conf.PWM_Mode *= -1; } if ( conf.PWM_Mode == 1 ) LCD_printf("ESC "); else LCD_printf("Moter"); Set_Arrow(2); break; case PARMSET+3: Param_Set_Prompt1("PWM>Interval",&conf.PWM_Interval,2,Thro_Hi,10000,10,sw); break; case PARMSET+4: Param_Set_Prompt1("Gyro>CutoffFreq",&conf.Cutoff_Freq,2,0.00f,10.0f,0.01f,sw); break; case PARMSET+5: Param_Set_Prompt1("ESC>Low Position",&conf.ESC_Low,2,Pulse_Min,Pulse_Max,1,sw); break; case PARMSET+6: Param_Set_Prompt1("Flight Timer",&conf.Flight_Time,2,0,600,10,sw); break; // case PARMSET+7: // Param_Set_Prompt1("Cont. swap Angle",&conf.Control_Exchange_Angle,3,0.0f,40.0f,1.0f,sw); // break; case CONFSTORE: //E2PROM Store LCD_printf("Config Save"); Set_Arrow(1); hmax = 1; break; case CONFSTORE+1: WriteConfig(); LCD_locate(0,0); sprintf(str,"Config %3dbyte",sizeof(config)); LCD_printf(str); LCD_locate(0,1); LCD_printf("Save Sucssesuful"); Set_Arrow(3); wait(0.5); FlashLED(5); hnum = 0; break; case CONFRESET: //E2PROM reset LCD_printf("Config Reset"); Set_Arrow(1); hmax = 3; break; case CONFRESET+1: LCD_printf("If want to reset"); LCD_locate(0,1); LCD_printf("Aileron right"); Set_Arrow(2); break; case CONFRESET+2: //E2PROM reset conf = init; LCD_printf("Rset sucssesuful"); Set_Arrow(3); break; default: if ( hnum == 0 ) vnum++; } sw = Check_Stick_Dir(ret_mode); //Wait Mode switch ( sw ) { case 'L': hnum--; if ( hnum <= 0 ) hnum = 0; LCD_cls(); //Clear LCD break; case 'R': LCD_cls(); if ( hnum < hmax ) hnum++; break; case 'U': if ( hnum == 0 ) { if ( vnum < vmax ) vnum++; else vnum = 0; LCD_cls(); //Clear LCD } break; case 'D': if ( hnum == 0 ) { if ( vnum > 0 ) vnum--; else vnum = vmax; LCD_cls(); //Clear LCD } break; case 'E': LCD_cls(); //Clear LCD LCD_locate(0,0); LCD_printf("PWM Started"); return; } } } char Check_Stick_Dir(char act) { int i; while ( 1 ) { Get_Stick_Pos(); if ( Stick[YAW] > Stick_Limit ) { i = 0; while ( Stick[YAW] > Stick_Limit && Stick[COL] < 30 ) { if ( i > 2000 ) return 'E'; //wait 2 sec wait(0.001); // wait 1 msec Get_Stick_Pos(); i++; } } if ( Stick[ROL] > Stick_Limit ) { wait(0.03); Get_Stick_Pos(); if ( !(Stick[ROL] > Stick_Limit) ) continue; while ( Stick[ROL] > Stick_Limit ) { Get_Stick_Pos(); } return 'R'; } if ( Stick[ROL] < -Stick_Limit ) { wait(0.03); Get_Stick_Pos(); if ( !(Stick[ROL] < -Stick_Limit) ) continue; while ( Stick[ROL] < -Stick_Limit ) { Get_Stick_Pos(); } return 'L'; } if ( Stick[PIT] < -Stick_Limit ) { wait(0.03); Get_Stick_Pos(); if ( !(Stick[PIT] < -Stick_Limit) ) continue; if ( act == 'R' ) { wait(0.03); return 'D'; } while ( Stick[PIT] < -Stick_Limit ) { Get_Stick_Pos(); } return 'D'; } if ( Stick[PIT] > Stick_Limit ) { wait(0.03); Get_Stick_Pos(); if ( !( Stick[PIT] > Stick_Limit) ) continue; if ( act == 'R' ) { wait(0.03); return 'U'; } while ( Stick[PIT] > Stick_Limit ) { Get_Stick_Pos(); } return 'U'; } if ( act == 'R' ) return ' '; } } void Param_Set_Prompt1(char *hd,int *num,int arrow,int min,int max,int increase,char sw) { ret_mode = 'R'; LCD_locate(0,0); LCD_printf(hd); LCD_locate(0,1); sprintf(str,"%6d",*num); LCD_printf(str); Set_Arrow(arrow); switch ( sw ) { case 'U': *num -= increase; if ( *num <= min ) *num = min; break; case 'D': *num += increase; if ( *num >= max ) *num = max; } } void Param_Set_Prompt1(char *hd,float *num,int arrow,float min,float max,float increase,char sw) { ret_mode = 'R'; LCD_locate(0,0); LCD_printf(hd); LCD_locate(0,1); sprintf(str,"%7.2f",*num); LCD_printf(str); Set_Arrow(arrow); switch ( sw ) { case 'U': *num -= increase; if ( *num <= min ) *num = min; break; case 'D': *num += increase; if ( *num >= max ) *num = max; } } void Set_Arrow(int dir) { LCD_locate(12,1); switch ( dir ) { case 1: LCD_printf(" >>"); break; case 2: LCD_printf("<<>>"); break; case 3: LCD_printf(" <<"); } }