syouichi imamori
/
MulticopterQuadX
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Diff: config.cpp
- Revision:
- 2:59ac9df97701
- Parent:
- 0:cca1c4e84da4
- Child:
- 3:27407c4984cf
--- a/config.cpp Tue Jul 16 06:37:28 2013 +0000
+++ b/config.cpp Fri Nov 15 20:53:36 2013 +0000
@@ -1,81 +1,97 @@
#include "mbed.h"
#include "I2cPeripherals.h"
-//#include "I2cLCD.h"
#include "InterruptIn.h"
-//#include "ITG3200.h"
-//#include "LPS331AP.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(const char *,int *,int,int,int,int,char);
-void Param_Set_Prompt1(const char *,float *,int,float,float,float,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 int CH[5];
-extern int M[6];
-extern int Gyro[3];
-extern int Accel[3];
-extern int Gyro_Ref[3];
-extern int Stick[5];
-extern float Press;
+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;
-#ifdef SERIAL_LCD
-SerialLcd *lcdptr;
-void SetUpPrompt(config& conf,SerialLcd& lcd)
-#else
-I2cPeripherals *lcdptr;
-void SetUpPrompt(config& conf,I2cPeripherals& lcd)
-#endif
+void SetUpPrompt(config& conf,I2cPeripherals& i2c)
{
-
- lcdptr = &lcd;
- lcd.cls();
+ float x,y,z;
+ LCD_cls();
mode = 0;
vnum = 0;
hnum = 0;
- vmax = 11;
-
+ vmax = 12;
+
while( 1 ) {
- FlashLED(1);
+// FlashLED(1);
ret_mode = 'W';
mode = vnum * 10 + hnum;
-
+
switch ( mode ) {
case 0:
- lcd.locate(0,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");
+ LCD_printf(str);
+ LCD_locate(4,1);
+ LCD_printf("By AZUKITEN");
hmax = 0;
break;
- case 10: //Calibrate Transmitter
- lcd.printf("Calibrate Transmitter");
+ case CALIBURATE: //Calibrate Transmitter
+ LCD_printf("Calibrate");
Set_Arrow(1);
hmax = 1;
break;
- case 11: //Calibrate Transmitter
- lcd.printf("Start Calibrate");
+ case CALIBURATE+1: //Calibrate Transmitter
+ LCD_printf("Start Calibrate");
for(i=0; i<4; i++) {
conf.Stick_Ref[i] = 0;
}
@@ -90,300 +106,349 @@
for(i=0; i<4; i++) {
conf.Stick_Ref[i] = conf.Stick_Ref[i]/16;
}
- lcd.cls(); //Clear LCD
- lcd.printf("Calibrate Completed");
+ CalibrateGyros();
+ CalibrateAccel();
+ LCD_cls(); //Clear LCD
+ LCD_printf("Calibrate Completed");
Set_Arrow(3);
FlashLED(5);
+ hnum = 0;
break;
- case 20: //Set Gyro Gain
- lcd.printf("Set Gyro Gain");
+ case GYROGAIN: //Set Gyro Gain
+ LCD_printf("Set Gyro Gain");
Set_Arrow(1);
hmax = 4;
break;
- case 21: //Set Gyro Gain Roll
+ 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);
+ Param_Set_Prompt1("GyroGain>Roll",&conf.Gyro_Gain[3],2,-1.00f,1.00f,0.01f,sw);
break;
- case 22:
+ 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 23:
+ 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 24:
+ case GYROGAIN+4:
// ret_mode = 'R';
- lcd.printf("GyroGain>setting");
- lcd.locate(0,1);
+ 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");
+ LCD_printf("Controller");
else
- lcd.printf("Transmitter");
+ LCD_printf("Transmitter");
Set_Arrow(3);
break;
- case 30: //Set Gyro Direction
- lcd.printf("Gyro Direction");
+ case GYRODIR: //Set Gyro Direction
+ LCD_printf("Gyro Direction");
Set_Arrow(1);
hmax = 4;
break;
- case 31: //Set Gyro Direction Roll
- case 32:
- case 33:
- case 34:
- // ret_mode = 'R';
- idx = mode - 31;
- if ( mode == 34 )
- lcd.printf("Gyro>Swap X-Y");
+ 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(steering[idx]);
+ LCD_printf("Gyro>Dir>");
+ LCD_locate(9,0);
+ LCD_printf((char*)steering[idx]);
}
- lcd.locate(0,1);
+ LCD_locate(0,1);
switch ( sw ) {
case 'U':
case 'D':
conf.Gyro_Dir[idx] *= -1;
}
if ( conf.Gyro_Dir[idx] == 1 )
- lcd.printf("Normal ");
+ LCD_printf("Normal ");
else
- lcd.printf("Reverse");
- if ( mode == 34 )
+ LCD_printf("Reverse");
+ if ( mode == (GYRODIR+4) )
Set_Arrow(3);
else
Set_Arrow(2);
break;
- case 40: //Set Accelerometer
- lcd.printf("Acceleration");
- lcd.locate(5,1);
- lcd.printf("Gain");
+ case ACCELCORRECT:
+ LCD_printf("Acceleration");
+ LCD_locate(2,1);
+ LCD_printf("correction");
Set_Arrow(1);
hmax = 3;
break;
- case 41:
- Param_Set_Prompt1("Accel>Roll",&conf.Accel_Gain[0],2,0.00f,1.00f,0.01f,sw);
+ 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 42: //Set Accelerometer
- Param_Set_Prompt1("Accel>Pith",&conf.Accel_Gain[1],2,0.00f,1.00f,0.01f,sw);
+ 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 43: //Set Accelerometer
- ret_mode = 'R';
- Param_Set_Prompt1("Accel>Yaw",&conf.Accel_Gain[2],3,0.00f,1.00f,0.01f,sw);
- break;
-
- case 50: //Set Stick Mixing
- lcd.printf("Set Stick Mixing");
+ 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 51: //Set Stick Mixing
+ 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 52:
+ case STICKMIX+2:
Param_Set_Prompt1("Mixing>Pitch",&conf.Stick_Mix[1],2,0.00f,2.00f,0.01f,sw);
break;
- case 53:
+ case STICKMIX+3:
Param_Set_Prompt1("Mixing>Yaw",&conf.Stick_Mix[2],3,0.00f,2.00f,0.01f,sw);
break;
- case 60: //Display Pulse Width
- lcd.printf("Disp Pulse Width");
+ case DISPPULSE: //Display Pulse Width
+ LCD_printf("Disp Pulse Width");
Set_Arrow(1);
hmax = 2;
break;
- case 61: //Display Pulse Width
+ case DISPPULSE+1: //Display Pulse Width
// DisplayPulseWidth(THR,AIL,ELE,RUD,AUX);
- ret_mode = 'R';
- lcd.locate(0,0);
+ ret_mode = 'R';
+ LCD_locate(0,0);
sprintf(str,"TR=%4d,AL=%4d",THR,AIL);
- lcd.printf(str);
- lcd.locate(0,1);
+ LCD_printf(str);
+ LCD_locate(0,1);
sprintf(str,"EL=%4d,RD=%4d",ELE,RUD);
- lcd.printf(str);
+ LCD_printf(str);
break;
- case 62: //Display Stick Ref
-// DisplayPulseWidth(Stick[COL],Stick[ROL],Stick[PIT],Stick[YAW],Stick[GAIN]);
- ret_mode = 'R';
+ case DISPPULSE+2: //Display Stick Ref
+ ret_mode = 'R';
Get_Stick_Pos();
- lcd.locate(0,0);
+ LCD_locate(0,0);
sprintf(str,"TR=%4d,AL=%4d",Stick[COL],Stick[ROL]);
- lcd.printf(str);
- lcd.locate(0,1);
+ LCD_printf(str);
+ LCD_locate(0,1);
sprintf(str,"EL=%4d,RD=%4d",Stick[PIT],Stick[YAW]);
- lcd.printf(str);
- break;
- case 70: //Display Sensor Value
- lcd.printf("Disp Senser");
- Set_Arrow(1);
- hmax = 5;
+ LCD_printf(str);
break;
- case 71: //Gyro
+ 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=%5d",Gyro[ROL]);
- lcd.printf(str);
- wait(0.02);
- lcd.locate(0,1);
- sprintf(str,"y=%5d,Z=%5d",Gyro[PIT],Gyro[YAW]);
- lcd.printf(str);
- wait(0.02);
+ 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 72: //Accelerometer
+ case DISPSENSOR+2: //Accelerometer
Get_Accel();
- lcd.locate(0,0);
- sprintf(str,"Accel//X=%5d",Accel[ROL]);
- lcd.printf(str);
- lcd.locate(0,1);
- sprintf(str,"Y=%5d,Z=%5d",Accel[PIT],Accel[YAW]);
- lcd.printf(str);
+ 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 73: // Pressure
+ 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,"Pressure=%9.3f",Press/4096);
- lcd.printf(str);
- lcd.locate(0,1);
+ 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);
+ LCD_printf(str);
// Set_Arrow(2);
ret_mode = 'R';
+ wait(0.05);
break;
- case 74:
+ case DISPSENSOR+6:
elaps.reset();
elaps.start();
PWM_Out(false);
elaps.stop();
- lcd.locate(0,0);
- sprintf(str,"ElapsTime=%5d",elaps.read_us());
- lcd.printf(str);
+ i = elaps.read_us();
+ LCD_locate(0,0);
+ sprintf(str,"ElapsTime=%6d",i);
+ LCD_printf(str);
Set_Arrow(2);
ret_mode = 'R';
break;
- case 75: //Sensor Calibration
-// CalibrateAccel();
+ case DISPSENSOR+7: //Sensor Calibration
CalibrateGyros();
- CalibrateAccel();
- lcd.printf("Calibrate completed");
+ FlashLED(3);
+ LCD_printf("Calibrate completed");
Set_Arrow(3);
break;
- case 80: //Display PMW Condition
- lcd.printf("Display PMW ");
+ case DISPPWM: //Display PWM Condition
+ LCD_printf("Display PWM ");
Set_Arrow(1);
hmax = 1;
break;
- case 81: //Display PMW Width
+ case DISPPWM+1: //Display PWM Width
+ ret_mode = 'R';
PWM_Out(false);
-// DisplayInt(Motor_HD[0],M1,M2,M4,M3);
- ret_mode = 'R';
- lcd.locate(0,0);
+ LCD_locate(0,0);
sprintf(str,"M1=%4d,M2=%4d",M1,M2);
- lcd.printf(str);
- lcd.locate(0,1);
+ LCD_printf(str);
+ LCD_locate(0,1);
sprintf(str,"M4=%4d,M3=%4d",M4,M3);
- lcd.printf(str);
+ LCD_printf(str);
break;
- case 90: //パラメーター設定
- lcd.printf("Parameter Set");
+ case PARMSET: //パラメーター設定
+ LCD_printf("Parameter Set");
Set_Arrow(1);
- hmax = 4;
+ hmax = 6;
break;
- case 91:
+ case PARMSET+1:
Param_Set_Prompt1("LCD>Contrast",&conf.LCD_Contrast,2,0,63,1,sw);
break;
- case 92:
- lcd.locate(0,0);
- lcd.printf("PWM>Mode");
- lcd.locate(0,1);
+ 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 ");
+ LCD_printf("ESC ");
else
- lcd.printf("Moter");
+ LCD_printf("Moter");
Set_Arrow(2);
break;
- case 93:
+ case PARMSET+3:
Param_Set_Prompt1("PWM>Interval",&conf.PWM_Interval,2,Thro_Hi,10000,10,sw);
-// for ( i=0;i<4;i++ ) pwm[i].period_us(conf.PWM_Interval);
+ 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 94:
- lcd.locate(0,0);
- lcd.printf("Gyro>Type");
- lcd.locate(0,1);
- switch ( sw ) {
- case 'U':
- case 'D':
- if ( conf.Gyro_Type == _L3GD20 )
- conf.Gyro_Type = _ITG3200;
- else
- conf.Gyro_Type = _L3GD20;
- }
- if ( conf.Gyro_Type == _L3GD20 )
- lcd.printf("L3GD20");
- else
- lcd.printf("ITG3200");
- Set_Arrow(2);
+ case PARMSET+6:
+ Param_Set_Prompt1("Flight Timer",&conf.Flight_Time,2,0,600,10,sw);
break;
- case 100: //E2PROM Store
- lcd.printf("Config Save");
+// 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 101:
+ case CONFSTORE+1:
WriteConfig();
- FlashLED(5);
- lcd.locate(0,0);
+ LCD_locate(0,0);
sprintf(str,"Config %3dbyte",sizeof(config));
- lcd.printf(str);
- lcd.locate(0,1);
- lcd.printf("Save Sucssesuful");
+ LCD_printf(str);
+ LCD_locate(0,1);
+ LCD_printf("Save Sucssesuful");
Set_Arrow(3);
- wait(2);
- break;
- case 110: //ESC Set up
- lcd.locate(0,0);
- lcd.printf("ESC Set UP");
- Set_Arrow(1);
- hmax = 1;
+ wait(0.5);
+ FlashLED(5);
+ hnum = 0;
break;
- case 111:
- if ( conf.PWM_Mode == -1 ) {
- hnum = 0;
- break;
- }
- lcd.locate(0,0);
- lcd.printf("Setup Start");
- wait(1.5);
- lcd.cls(); //Clear LCD
- lcd.locate(0,0);
- lcd.printf("Please power-off");
- lcd.locate(1,0);
- lcd.printf(" after Setup");
-
- ESC_SetUp();
+ 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++;
@@ -396,30 +461,30 @@
case 'L':
hnum--;
if ( hnum <= 0 ) hnum = 0;
- lcd.cls(); //Clear LCD
+ LCD_cls(); //Clear LCD
break;
case 'R':
- lcd.cls();
+ LCD_cls();
if ( hnum < hmax ) hnum++;
break;
case 'U':
if ( hnum == 0 ) {
if ( vnum < vmax ) vnum++;
else vnum = 0;
- lcd.cls(); //Clear LCD
+ LCD_cls(); //Clear LCD
}
break;
case 'D':
if ( hnum == 0 ) {
if ( vnum > 0 ) vnum--;
else vnum = vmax;
- lcd.cls(); //Clear LCD
+ LCD_cls(); //Clear LCD
}
break;
case 'E':
- lcd.cls(); //Clear LCD
- lcd.locate(0,0);
- lcd.printf("PWM Started");
+ LCD_cls(); //Clear LCD
+ LCD_locate(0,0);
+ LCD_printf("PWM Started");
return;
}
}
@@ -431,8 +496,6 @@
int i;
while ( 1 ) {
Get_Stick_Pos();
-// DisplayInt(Stick[0],Stick[1],Stick[2],Stick[3]);
-// wait(0.2);
if ( Stick[YAW] > Stick_Limit ) {
i = 0;
while ( Stick[YAW] > Stick_Limit && Stick[COL] < 30 ) {
@@ -491,13 +554,14 @@
}
}
-void Param_Set_Prompt1(const char *hd,int *num,int arrow,int min,int max,int increase,char sw)
+void Param_Set_Prompt1(char *hd,int *num,int arrow,int min,int max,int increase,char sw)
{
ret_mode = 'R';
- lcdptr->locate(0,0);
- lcdptr->printf(hd);
- lcdptr->locate(0,1);
- lcdptr->printf("%d",*num);
+ 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':
@@ -511,14 +575,14 @@
*num = max;
}
}
-void Param_Set_Prompt1(const char *hd,float *num,int arrow,float min,float max,float increase,char sw)
+void Param_Set_Prompt1(char *hd,float *num,int arrow,float min,float max,float increase,char sw)
{
ret_mode = 'R';
- lcdptr->locate(0,0);
- lcdptr->printf(hd);
- lcdptr->locate(0,1);
+ LCD_locate(0,0);
+ LCD_printf(hd);
+ LCD_locate(0,1);
sprintf(str,"%7.2f",*num);
- lcdptr->printf(str);
+ LCD_printf(str);
Set_Arrow(arrow);
switch ( sw ) {
case 'U':
@@ -535,16 +599,16 @@
void Set_Arrow(int dir)
{
- lcdptr->locate(12,1);
+ LCD_locate(12,1);
switch ( dir ) {
case 1:
- lcdptr->printf(" >>");
+ LCD_printf(" >>");
break;
case 2:
- lcdptr->printf("<<>>");
+ LCD_printf("<<>>");
break;
case 3:
- lcdptr->printf(" <<");
+ LCD_printf(" <<");
}
}
@@ -553,3 +617,37 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+