syouichi imamori
Quad X Type Multicopter
Revision 2:59ac9df97701, committed 2013-11-15
- Comitter:
- komaida424
- Date:
- Fri Nov 15 20:53:36 2013 +0000
- Parent:
- 1:caafe8935da6
- Child:
- 3:27407c4984cf
- Commit message:
- ver.1.25
Changed in this revision
--- a/I2cPeripherals/I2cPeripherals.cpp Tue Jul 16 06:37:28 2013 +0000
+++ b/I2cPeripherals/I2cPeripherals.cpp Fri Nov 15 20:53:36 2013 +0000
@@ -1,128 +1,197 @@
#include "mbed.h"
#include "I2cPeripherals.h"
-
+//Serial pc(USBTX, USBRX);
I2cPeripherals::I2cPeripherals(PinName sda, PinName scl): _i2c(sda,scl)
-{
+{
+
_i2c.frequency(400000);
- LCD_contrast = 60;
+// LCD_contrast = 60;
+ wait(0.5);
+ LCD_addr = 0;
+ Gyro_addr = 0;
+ Accel_addr = 0;
+ barometor_addr = 0;
+ find();
+ start();
+
}
-void I2cPeripherals::start(char I2cAddr,int contrast)
+void I2cPeripherals::start(int contrast)
{
- LCD_contrast = contrast;
- start(I2cAddr);
+ char tx[2];
+// find(); // I2C interface sensor detect
+#ifdef ST7032
+ LCD_addr = 0x7c;
+ LCD_data = 0x40;
+ tx[0] = 0x00;
+ tx[1] = 0x38;
+ if ( _i2c.write(LCD_addr,tx,2) == 0 ) {
+ tx[1] = 0x39;
+ _i2c.write(LCD_addr,tx,2);
+ tx[1] = 0x14; //1F
+ _i2c.write(LCD_addr,tx,2);
+ tx[1] = 0x70 | (contrast & 0x0F);
+ _i2c.write(LCD_addr,tx,2);
+ tx[1] = 0x5C | ((contrast >> 4) & 0x03);
+ _i2c.write(LCD_addr,tx,2);
+ tx[1] = 0x6A;
+ _i2c.write(LCD_addr,tx,2);
+ wait(0.3);
+ tx[1] = 0x0C;
+ _i2c.write(LCD_addr,tx,2); // display on
+ tx[1] = 0x06;
+ _i2c.write(LCD_addr,tx,2);
+ // tx[0] = 0x40;
+// tx[1] = '*';
+// _i2c.write(LCD_addr,tx,2); // Display clear
+ return;
+ }
+#endif
+#ifdef ACM1602
+ LCD_addr = 0xA0;
+ LCD_data = 0x80;
+ tx[0] = 0x00;
+ tx[1] = 0x01;
+ if ( _i2c.write(LCD_addr,tx,2) == 0 ) {
+ wait(0.005);
+ tx[1] = 0x38;
+ _i2c.write(LCD_addr,tx,2);
+ wait(0.005);
+ tx[1] = 0x0F;
+ _i2c.write(LCD_addr,tx,2);
+ wait(0.005);
+ tx[1] = 0x06;
+ _i2c.write(LCD_addr,tx,2);
+ wait(0.005);
+ return;
+ }
+#endif
+ LCD_addr = 0;
}
-void I2cPeripherals::start(char I2cAddr)
+void I2cPeripherals::find()
{
char tx[2];
+ char rx[1];
+ SensorInf sens[10] = { 0xD0,0x68,0x00, //ITG3200
+ 0xD2,0x68,0x00,
+ 0xD0,0x68,0x75, //MPU6050
+ 0xD2,0x68,0x75,
+ 0xD4,0xD4,0x0F, //L3GD20
+ 0xD6,0xD4,0x0F,
+ 0xA6,0xE5,0x00, //ADXL345
+ 0x3A,0xE5,0x00,
+ 0xB8,0xBB,0x0F, //LPS331
+ 0xBA,0xBB,0x0F
+ };
- switch (I2cAddr) {
- case ST7032_ADDR: //I2C LCD
- LCD_addr = I2cAddr;
- LCD_data = 0x40;
- tx[0] = 0x00;
- tx[1] = 0x38;
- _i2c.write(I2cAddr,tx,2);
- tx[1] = 0x39;
- _i2c.write(I2cAddr,tx,2);
- tx[1] = 0x1F;
- _i2c.write(I2cAddr,tx,2);
- tx[1] = 0x70 | (LCD_contrast & 0x0F);
- _i2c.write(I2cAddr,tx,2);
- tx[1] = 0x5C | ((LCD_contrast >> 4) & 0x03);
- _i2c.write(I2cAddr,tx,2);
- tx[1] = 0x6A;
- _i2c.write(I2cAddr,tx,2);
- wait(0.3);
- tx[1] = 0x38;
- _i2c.write(I2cAddr,tx,2); // function set
- tx[1] = 0x0C;
- _i2c.write(I2cAddr,tx,2); // Display On
- break;
- case ACM1602_ADDR: //I2C LCD
- LCD_addr = I2cAddr;
- LCD_data = 0x80;
- wait(0.015);
- tx[0] = 0x00;
- tx[1] = 0x01;
- _i2c.write(I2cAddr,tx,2);
- wait(0.005);
- tx[1] = 0x38;
- _i2c.write(I2cAddr,tx,2);
- wait(0.005);
- tx[1] = 0x0F;
- _i2c.write(I2cAddr,tx,2);
- wait(0.005);
- tx[1] = 0x06;
- _i2c.write(I2cAddr,tx,2);
- wait(0.005);
- break;
+ for ( int num = 0; num < 10; num++ ) {
+ tx[0]= sens[num].whoaddr;
+ if ( _i2c.write(sens[num].addr,tx,1,true) != 0 ) continue;
+ _i2c.read (sens[num].addr,rx,1);
+ if ( (rx[0]&0x7E) != (sens[num].who&0x7E) ) continue;
- case ITG3200_ADDR0: //gyro
- case ITG3200_ADDR1:
- Gyro_addr = I2cAddr;
- Gyro_data = 0x1D;
- tx[0] = 0x16;
- tx[1] = 0x18; // 0x1D
- _i2c.write(I2cAddr,tx,2);
- tx[0] = 0x3E;
- tx[1] = 0x01;
- _i2c.write(I2cAddr,tx,2);
- wait_ms(100);
- break;
-
- case L3GD20_ADDR0: //gyro
- case L3GD20_ADDR1:
- Gyro_addr = I2cAddr;
- Gyro_data = 0x28;
- tx[0] = 0x20;
- tx[1] = 0xFF;
- _i2c.write(I2cAddr,tx,2);
- tx[0] = 0x21;
- tx[1] = 0x09;
- _i2c.write(I2cAddr,tx,2);
- tx[0] = 0x23;
- tx[1] = 0xF0;
- _i2c.write(I2cAddr,tx,2);
- break;
-
- case LDXL345_ADDR0: //accelametor
- case LDXL345_ADDR1:
- Accel_addr = I2cAddr;
- Accel_data = 0x32;
- tx[0] = 0x2D;
- tx[1] = 0x00;
- _i2c.write(I2cAddr,tx,2);
- tx[0] = 0x31;
- tx[1] = 0x08; //full range 08:2g 09:4g 0A:8g 0B:16g
- _i2c.write(I2cAddr,tx,2);
- tx[0] = 0x2C;
- tx[1] = 0x0C; //out rate 0xC:400Hz 0xD:800Hz
- _i2c.write(I2cAddr,tx,2);
- tx[0] = 0x2D;
- tx[1] = 0x08;
- _i2c.write(I2cAddr,tx,2);
- break;
-
- case LPS331AP_ADDR0: //barometor
- case LPS331AP_ADDR1:
- barometor_addr = I2cAddr;
- barometor_data = 0x2B;
- tx[0] = 0x10; //RES_CNF
- tx[1] = 0x19;
- _i2c.write(I2cAddr,tx,2);
- tx[0] = 0x20; // CTL_REG1
- tx[1] = 0x80; // power on
- _i2c.write(I2cAddr,tx,2);
- tx[0]= 0x21; // CTL_REG2
- tx[1]= 0x01; // one shot start
- _i2c.write(I2cAddr,tx,2);
- break;
- }
+ switch ( num ) {
+#ifdef ITG3200
+ case 0: //ITG3200
+ case 1:
+ Gyro_addr = sens[num].addr;
+ Gyro_data = 0x1D;
+ tx[0] = 0x16;
+ tx[1] = 0x18; // 0x1D
+ _i2c.write(sens[num].addr,tx,2);
+ tx[0] = 0x3E;
+ tx[1] = 0x01;
+ _i2c.write(sens[num].addr,tx,2);
+ wait_ms(10);
+ break;
+#endif
+#ifdef MPU6050
+ case 2: //MPU6050
+ case 3:
+ Gyro_addr = sens[num].addr;
+ Gyro_data = 0x43;
+ Accel_addr = sens[num].addr;
+ Accel_data = 0x3B;
+ tx[0] = 0x6B;
+ tx[1] = 0x00; // PWR on
+ _i2c.write(sens[num].addr,tx,2);
+ tx[0] = 0x1A; // DLPF(Digitel low pass filter)
+ tx[1] = 0x02; // set 0 to 7
+ _i2c.write(sens[num].addr,tx,2);
+ wait(0.01);
+ tx[0] = 0x1B;
+ tx[1] = 0x18; // +-2000deg
+ _i2c.write(sens[num].addr,tx,2);
+ wait(0.01);
+ tx[0] = 0x1C;
+ tx[1] = 0x18; // +-16g
+ _i2c.write(sens[num].addr,tx,2);
+ wait_ms(10);
+ break;
+#endif
+#ifdef L3GD20
+ case 4: //L3GD20
+ case 5:
+ Gyro_addr = sens[num].addr;
+ Gyro_data = 0x28;
+ tx[0] = 0x20;
+ tx[1] = 0xBF; //rate 400Hz 0x8F-0xBF
+ _i2c.write(sens[num].addr,tx,2);
+ tx[0] = 0x21;
+ tx[1] = 0x09;
+ _i2c.write(sens[num].addr,tx,2);
+ tx[0] = 0x23;
+ tx[1] = 0xF0;
+ _i2c.write(sens[num].addr,tx,2);
+ tx[0] = 0x24;
+ tx[1] = 0x10;
+ _i2c.write(sens[num].addr,tx,2);
+ break;
+#endif
+#ifdef ADXL345
+ case 6: //ADXL345
+ case 7:
+ Accel_addr = sens[num].addr;
+ Accel_data = 0x32;
+ tx[0] = 0x2D;
+ tx[1] = 0x00;
+ _i2c.write(sens[num].addr,tx,2);
+ tx[0] = 0x31;
+ tx[1] = 0x0B; //full range 08:2g 09:4g 0A:8g 0B:16g
+ _i2c.write(sens[num].addr,tx,2);
+ tx[0] = 0x2C;
+ tx[1] = 0x09; //out rate 0xC:400Hz 0xD:800Hz
+ _i2c.write(sens[num].addr,tx,2);
+ tx[0] = 0x2D;
+ tx[1] = 0x08;
+ _i2c.write(sens[num].addr,tx,2);
+ break;
+#endif
+#ifdef LPS331AP
+ case 8: //barometor
+ case 9:
+ barometor_addr = sens[num].addr;
+ barometor_data = 0x2B;
+ tx[0] = 0x10; //RES_CNF
+ tx[1] = 0x7A;
+ _i2c.write(sens[num].addr,tx,2);
+ tx[0] = 0x20; // CTL_REG1
+ tx[1] = 0xB0; // power on
+ _i2c.write(sens[num].addr,tx,2);
+// tx[0]= 0x21; // CTL_REG2
+// tx[1]= 0x01; // one shot start
+// _i2c.write(sens[num].addr,tx,2);
+ wait(1);
+ break;
+#endif
+ }
+ }
}
-int I2cPeripherals::_putc(int value) {
+int I2cPeripherals::_putc(int value)
+{
+ if ( LCD_addr == 0 ) return value;
_i2c.start();
_i2c.write(LCD_addr);
_i2c.write(LCD_data);
@@ -131,12 +200,32 @@
return value;
}
-int I2cPeripherals::_getc() {
+int I2cPeripherals::_getc()
+{
return -1;
}
+void I2cPeripherals::write_reg(char I2cAddr,char reg_addr,char* data,int len)
+{
+ char tx[17];
+ if ( len >16 ) len = 16;
+ tx[0] = reg_addr;
+ if ( len > 1 ) tx[0] |= 0x80;
+ for (int i=0; i<len; i++) tx[i+1] = data[i];
+ _i2c.write(I2cAddr,tx,len+1);
+}
+
+void I2cPeripherals::read_reg(char I2cAddr,char reg_addr,char* data,int len)
+{
+ char tx[1];
+ tx[0] = reg_addr | 0x80;
+ _i2c.write(I2cAddr,tx,1,true);
+ _i2c.read(I2cAddr,data,len);
+}
+
void I2cPeripherals::cls()
{
+ if ( LCD_addr == 0 ) return;
char tx[2] = { 0x00, 0x01 };
_i2c.write(LCD_addr,tx,2);
}
@@ -144,89 +233,130 @@
void I2cPeripherals::locate(int clm,int row)
{
char tx[2];
-
+
+ if ( LCD_addr == 0 ) return;
tx[0] = 0x00;
tx[1] = 0x80 + (row * 0x40) + clm;
_i2c.write(LCD_addr,tx,2);
-
+
}
-int I2cPeripherals::angular(int *x,int *y,int *z)
-{
+int I2cPeripherals::angular(float *x,float *y,float *z)
+{
char rx[6];
char tx[1];
+ float i = 0;
tx[0] = Gyro_data | 0x80;
if ( _i2c.write(Gyro_addr,tx,1,true) != 0 ) {
- x=y=z=0;
- return false;
+ *x=*y=*z=0;
+ return Gyro_addr;
+ }
+ switch ( Gyro_addr ) {
+ case 0xD0:
+ case 0xD2:
+ i = 0.06098;
+ break;
+ case 0xD4:
+ case 0xD6:
+ i = 0.06957;
}
_i2c.read(Gyro_addr,rx,6);
- *x = short(rx[0] << 8 | (uint8_t)rx[1]);
- *y = short(rx[2] << 8 | (uint8_t)rx[3]);
- *z = short(rx[4] << 8 | (uint8_t)rx[5]);
+ *x = ( short(rx[0] << 8 | (uint8_t)rx[1]) ) * i;
+ *y = ( short(rx[2] << 8 | (uint8_t)rx[3]) ) * i;
+ *z = ( short(rx[4] << 8 | (uint8_t)rx[5]) ) * i;
return true;
}
-int I2cPeripherals::Acceleration(int *x,int *y,int *z)
+
+int I2cPeripherals::Acceleration(float *x,float *y,float *z)
{
- //AXDL345 Data Read
+ //AXDL345 Data Read
char rx[6];
char tx[1];
tx[0] = Accel_data | 0x80;
if ( _i2c.write(Accel_addr,tx,1,true) != 0 ) {
- x=y=z=0;
+ *x=*y=0;
+ *z=9.8;
return false;
}
_i2c.read(Accel_addr,rx,6);
- *y = -short(rx[1] << 8 | (uint8_t)rx[0]);
- *x = short(rx[3] << 8 | (uint8_t)rx[2]);
- *z = short(rx[5] << 8 | (uint8_t)rx[4]);
+ switch ( Accel_addr ) {
+ case 0xD0:
+ case 0xD2:
+#ifdef MPU6050
+ *y = ( -(short(rx[0] << 8 | (uint8_t)rx[1])) ) * 0.0048;
+ *x = ( short(rx[2] << 8 | (uint8_t)rx[3]) ) * 0.0048;
+ *z = ( short(rx[4] << 8 | (uint8_t)rx[5]) ) * 0.0048;
+#endif
+ break;
+ case 0xA6:
+ case 0x3A:
+#ifdef ADXL345
+ *y = ( -(short(rx[1] << 8 | (uint8_t)rx[0])) ) * 0.04;
+ *x = ( short(rx[3] << 8 | (uint8_t)rx[2]) ) * 0.04;
+ *z = ( short(rx[5] << 8 | (uint8_t)rx[4]) ) * 0.04;
+#endif
+ }
return true;
}
-
+
int I2cPeripherals::pressure()
{
char tx[2];
char rx[3];
- int press;
-
- tx[0]= 0x21;
- rx[0] = 0;
+ int press = 0;
+
+ tx[0]= 0x28 | 0x80;
if ( _i2c.write(barometor_addr,tx,1,true) != 0 )
return 0;
- _i2c.read (barometor_addr,rx,1);
- while( rx[0]&0x01 ) {
- wait(0.0001);
- _i2c.write(barometor_addr,tx,1,true);
- _i2c.read (barometor_addr,rx,1);
- }
-
- tx[0]= 0x28 | 0x80;
- _i2c.write(barometor_addr,tx,1,true);
- _i2c.read (barometor_addr,rx,3);
-
- tx[0]= 0x21; // CTL_REG2
- tx[1]= 0x01; // one shot start
- _i2c.write(barometor_addr, tx, 2);
-
+ _i2c.read (barometor_addr,rx,3);
press =int( rx[2]<<16 | rx[1]<<8 | rx[0] );
return press;
}
-
-//Update-Methode
int I2cPeripherals::temperature()
{
char tx[1];
char rx[2];
int temp;
-
+
tx[0]= 0x2B | 0x80;
if ( _i2c.write(barometor_addr,tx,1,true) != 0 )
return 0;
- _i2c.read (barometor_addr,rx,2);
-
+ _i2c.read (barometor_addr,rx,2);
+
temp = short(rx[1]<<8 | rx[0]);
return temp;
-};
+}
+
+void I2cPeripherals::i2c_write(int i2caddr,char* tx,int len)
+{
+ char rx[1];
+ rx[0] = 0;
+ while( 1 ) {
+ _i2c.write(i2caddr,tx,len);
+ _i2c.write(i2caddr,tx,1,true);
+ _i2c.read (i2caddr,rx,1);
+ if ( tx[1] == rx[0] ) return;
+ }
+}
+
+int I2cPeripherals::GetAddr(int type)
+{
+ switch ( type ) {
+ case GYRO_ADDR:
+ return Gyro_addr;
+ case ACCEL_ADDR:
+ return Accel_addr;
+ case LCD_ADDR:
+ return LCD_addr;
+ case BARO_ADDR:
+ return barometor_addr;
+ }
+ return 0;
+}
+;
+
+
+
--- a/I2cPeripherals/I2cPeripherals.h Tue Jul 16 06:37:28 2013 +0000
+++ b/I2cPeripherals/I2cPeripherals.h Fri Nov 15 20:53:36 2013 +0000
@@ -4,33 +4,38 @@
#include "mbed.h"
#include "stdarg.h"
-#define LPS331AP_ADDR0 0xB8
-#define LPS331AP_ADDR1 0xBA //select
-#define L3GD20_ADDR0 0xD4
-#define L3GD20_ADDR1 0xD6 //select
-#define ITG3200_ADDR0 0xD0 //select
-#define ITG3200_ADDR1 0xB2
-#define ACM1602_ADDR 0xA0
-#define ST7032_ADDR 0x7C
-#define LDXL345_ADDR0 0xA6 //select
-#define LDXL345_ADDR1 0x3A
+#define LPS331AP // baro
+#define L3GD20 // gyro
+#define MPU6050 // gyro and accelameter
+#define ITG3200 // gyro
+#define ACM1602 // I2C LCD
+#define ST7032 // I2C LCD
+#define ADXL345 // accelameter
+#define GYRO_ADDR 0
+#define ACCEL_ADDR 1
+#define LCD_ADDR 3
+#define BARO_ADDR 4
class I2cPeripherals : public Stream
{
public:
- I2cPeripherals(PinName sda, PinName scl);
+ I2cPeripherals(PinName sda=p28, PinName scl=p27);
- void start(char);
- void start(char,int);
+ void start(int contrast=60);
+ void write_reg(char,char,char*,int);
+ void read_reg(char,char,char*,int);
void cls();
void locate(int,int);
- int angular(int*,int*,int*);
- int Acceleration(int*,int*,int*);
+ int GetAddr(int);
+ int angular(float*,float*,float*);
+ int Acceleration(float*,float*,float*);
int pressure();
int temperature();
private:
virtual int _putc(int value);
virtual int _getc();
+ void i2c_write(int,char*,int);
+ void find();
I2C _i2c;
int LCD_addr;
char LCD_cmd;
@@ -42,5 +47,10 @@
int barometor_addr;
char barometor_data;
int LCD_contrast;
+ struct SensorInf {
+ char addr;
+ char who; //WHO_AMI
+ char whoaddr; //WHO_AMI resister address
+ };
};
#endif
\ No newline at end of file
--- a/IAP.lib Tue Jul 16 06:37:28 2013 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://mbed.org/users/okano/code/IAP/#ff906ad52cf9
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/IAP/IAP.cpp Fri Nov 15 20:53:36 2013 +0000
@@ -0,0 +1,272 @@
+/** IAP : internal Flash memory access library
+ *
+ * The internal Flash memory access is described in the LPC1768 and LPC11U24 usermanual.
+ * http://www.nxp.com/documents/user_manual/UM10360.pdf
+ * http://www.nxp.com/documents/user_manual/UM10462.pdf
+ *
+ * LPC1768 --
+ * Chapter 2: "LPC17xx Memory map"
+ * Chapter 32: "LPC17xx Flash memory interface and programming"
+ * refering Rev. 01 - 4 January 2010
+ *
+ * LPC11U24 --
+ * Chapter 2: "LPC11Uxx Memory mapping"
+ * Chapter 20: "LPC11Uxx Flash programming firmware"
+ * refering Rev. 03 - 16 July 2012
+ *
+ * Released under the MIT License: http://mbed.org/license/mit
+ *
+ * revision 1.0 09-Mar-2010 1st release
+ * revision 1.1 12-Mar-2010 chaged: to make possible to reserve flash area for user
+ * it can be set by USER_FLASH_AREA_START and USER_FLASH_AREA_SIZE in IAP.h
+ * revision 2.0 26-Nov-2012 LPC11U24 code added
+ * revision 2.1 26-Nov-2012 EEPROM access code imported from Suga koubou san's (http://mbed.org/users/okini3939/) library
+ * http://mbed.org/users/okini3939/code/M0_EEPROM_test/
+ */
+
+#include "mbed.h"
+#include "IAP.h"
+
+#define USER_FLASH_AREA_START_STR( x ) STR( x )
+#define STR( x ) #x
+
+//unsigned char user_area[ USER_FLASH_AREA_SIZE ] __attribute__((section( ".ARM.__at_" USER_FLASH_AREA_START_STR( USER_FLASH_AREA_START ) ), zero_init));
+
+
+/*
+ * Reserve of flash area is explained by Igor. Please refer next URL
+ * http://mbed.org/users/okano/notebook/iap-in-application-programming-internal-flash-eras/?page=1#comment-271
+ */
+
+//unsigned char user_area[ size ] __attribute__((section(".ARM.__at_0x78000"), zero_init));
+
+/*
+ * IAP command codes
+ * Table 589. "IAP Command Summary", Chapter 8. "IAP commands", usermanual
+ */
+
+enum command_code
+ {
+ IAPCommand_Prepare_sector_for_write_operation = 50,
+ IAPCommand_Copy_RAM_to_Flash,
+ IAPCommand_Erase_sector,
+ IAPCommand_Blank_check_sector,
+ IAPCommand_Read_part_ID,
+ IAPCommand_Read_Boot_Code_version,
+ IAPCommand_Compare,
+ IAPCommand_Reinvoke_ISP,
+ IAPCommand_Read_device_serial_number,
+#if defined(TARGET_LPC11U24)
+ IAPCommand_EEPROM_Write = 61,
+ IAPCommand_EEPROM_Read,
+#endif
+ };
+
+
+/** Read part identification number
+ *
+ * @return device ID
+ * @see read_serial()
+ */
+
+int IAP::read_ID( void ) {
+ IAP_command[ 0 ] = IAPCommand_Read_part_ID;
+
+ iap_entry( IAP_command, IAP_result );
+
+ // return ( (int)IAP_result[ 0 ] );
+ return ( (int)IAP_result[ 1 ] ); // to return the number itself (this command always returns CMD_SUCCESS)
+}
+
+
+/** Read device serial number
+ *
+ * @return device serial number
+ * @see read_ID()
+ */
+
+int IAP::read_serial( void ) {
+ IAP_command[ 0 ] = IAPCommand_Read_device_serial_number;
+
+ iap_entry( IAP_command, IAP_result );
+
+ // return ( (int)IAP_result[ 0 ] );
+ return ( (int)IAP_result[ 1 ] ); // to return the number itself (this command always returns CMD_SUCCESS)
+}
+
+
+/** Blank check sector(s)
+ *
+ * @param start a Start Sector Number
+ * @param end an End Sector Number (should be greater than or equal to start sector number).
+ * @return error code: CMD_SUCCESS | BUSY | SECTOR_NOT_BLANK | INVALID_SECTOR
+ */
+
+int IAP::blank_check( int start, int end ) {
+ IAP_command[ 0 ] = IAPCommand_Blank_check_sector;
+ IAP_command[ 1 ] = (unsigned int)start; // Start Sector Number
+ IAP_command[ 2 ] = (unsigned int)end; // End Sector Number (should be greater than or equal to start sector number)
+
+ iap_entry( IAP_command, IAP_result );
+
+ return ( (int)IAP_result[ 0 ] );
+}
+
+
+/** Erase Sector(s)
+ *
+ * @param start a Start Sector Number
+ * @param end an End Sector Number (should be greater than or equal to start sector number).
+ * @return error code: CMD_SUCCESS | BUSY | SECTOR_NOT_PREPARED_FOR_WRITE_OPERATION | INVALID_SECTOR
+ */
+
+int IAP::erase( int start, int end ) {
+ IAP_command[ 0 ] = IAPCommand_Erase_sector;
+ IAP_command[ 1 ] = (unsigned int)start; // Start Sector Number
+ IAP_command[ 2 ] = (unsigned int)end; // End Sector Number (should be greater than or equal to start sector number)
+ IAP_command[ 3 ] = cclk_kHz; // CPU Clock Frequency (CCLK) in kHz
+
+ iap_entry( IAP_command, IAP_result );
+
+ return ( (int)IAP_result[ 0 ] );
+}
+
+
+/** Prepare sector(s) for write operation
+ *
+ * @param start a Start Sector Number
+ * @param end an End Sector Number (should be greater than or equal to start sector number).
+ * @return error code: CMD_SUCCESS | BUSY | INVALID_SECTOR
+ */
+
+int IAP::prepare( int start, int end ) {
+ IAP_command[ 0 ] = IAPCommand_Prepare_sector_for_write_operation;
+ IAP_command[ 1 ] = (unsigned int)start; // Start Sector Number
+ IAP_command[ 2 ] = (unsigned int)end; // End Sector Number (should be greater than or equal to start sector number).
+
+ iap_entry( IAP_command, IAP_result );
+
+ return ( (int)IAP_result[ 0 ] );
+}
+
+
+/** Copy RAM to Flash
+ *
+ * @param source_addr Source RAM address from which data bytes are to be read. This address should be a word boundary.
+ * @param target_addr Destination flash address where data bytes are to be written. This address should be a 256 byte boundary.
+ * @param size Number of bytes to be written. Should be 256 | 512 | 1024 | 4096.
+ * @return error code: CMD_SUCCESS | SRC_ADDR_ERROR (Address not a word boundary) | DST_ADDR_ERROR (Address not on correct boundary) | SRC_ADDR_NOT_MAPPED | DST_ADDR_NOT_MAPPED | COUNT_ERROR (Byte count is not 256 | 512 | 1024 | 4096) | SECTOR_NOT_PREPARED_FOR_WRITE_OPERATION | BUSY
+ */
+
+int IAP::write( char *source_addr, char *target_addr, int size ) {
+ IAP_command[ 0 ] = IAPCommand_Copy_RAM_to_Flash;
+ IAP_command[ 1 ] = (unsigned int)target_addr; // Destination flash address where data bytes are to be written. This address should be a 256 byte boundary.
+ IAP_command[ 2 ] = (unsigned int)source_addr; // Source RAM address from which data bytes are to be read. This address should be a word boundary.
+ IAP_command[ 3 ] = size; // Number of bytes to be written. Should be 256 | 512 | 1024 | 4096.
+ IAP_command[ 4 ] = cclk_kHz; // CPU Clock Frequency (CCLK) in kHz.
+
+ iap_entry( IAP_command, IAP_result );
+
+ return ( (int)IAP_result[ 0 ] );
+}
+
+
+/** Compare <address1> <address2> <no of bytes>
+ *
+ * @param source_addr Starting flash or RAM address of data bytes to be compared. This address should be a word boundary.
+ * @param target_addr Starting flash or RAM address of data bytes to be compared. This address should be a word boundary.
+ * @param size Number of bytes to be compared; should be a multiple of 4.
+ * @return error code: CMD_SUCCESS | COMPARE_ERROR | COUNT_ERROR (Byte count is not a multiple of 4) | ADDR_ERROR | ADDR_NOT_MAPPED
+ */
+
+int IAP::compare( char *source_addr, char *target_addr, int size ) {
+ IAP_command[ 0 ] = IAPCommand_Compare;
+ IAP_command[ 1 ] = (unsigned int)target_addr; // Starting flash or RAM address of data bytes to be compared. This address should be a word boundary.
+ IAP_command[ 2 ] = (unsigned int)source_addr; // Starting flash or RAM address of data bytes to be compared. This address should be a word boundary.
+ IAP_command[ 3 ] = size; // Number of bytes to be compared; should be a multiple of 4.
+
+ iap_entry( IAP_command, IAP_result );
+
+ return ( (int)IAP_result[ 0 ] );
+}
+
+/** Compare <address1> <address2> <no of bytes>
+ *
+ * @param source_addr Starting flash or RAM address of data bytes to be compared. This address should be a word boundary.
+ * @param target_addr Starting flash or RAM address of data bytes to be compared. This address should be a word boundary.
+ * @param size Number of bytes to be compared; should be a multiple of 4.
+ * @return error code: CMD_SUCCESS | COMPARE_ERROR | COUNT_ERROR (Byte count is not a multiple of 4) | ADDR_ERROR | ADDR_NOT_MAPPED
+ */
+
+int IAP::read_BootVer(void) {
+ IAP_command[0] = IAPCommand_Read_Boot_Code_version;
+ IAP_result[1] = 0; // not sure if in high or low bits.
+ iap_entry(IAP_command, IAP_result);
+ return ((int)IAP_result[1]);
+}
+
+/** Get user reserved flash start address
+ *
+ * @return start address of user reserved flash memory
+ * @see reserved_flash_area_size()
+ */
+
+char * IAP::reserved_flash_area_start( void )
+{
+ return ( (char *)USER_FLASH_AREA_START );
+}
+
+
+/** Get user reserved flash size
+ *
+ * @return size of user reserved flash memory
+ * @see reserved_flash_area_start()
+ */
+
+int IAP::reserved_flash_area_size( void )
+{
+ return ( USER_FLASH_AREA_SIZE );
+}
+
+#if defined(TARGET_LPC11U24)
+/** Copy RAM to EEPROM (LPC11U24)
+ *
+ * @param source_addr Source RAM address from which data bytes are to be read.
+ * @param target_addr Destination EEPROM address where data bytes are to be written.
+ * @param size Number of bytes to be written.
+ * @return error code: CMD_SUCCESS | SRC_ADDR_NOT_MAPPED | DST_ADDR_NOT_MAPPED
+ * Remark: The top 64 bytes of the EEPROM memory are reserved and cannot be written to.
+ */
+int IAP::write_eeprom( char *source_addr, char *target_addr, int size ) {
+ IAP_command[ 0 ] = IAPCommand_EEPROM_Write;
+ IAP_command[ 1 ] = (unsigned int)target_addr; // Destination EEPROM address where data bytes are to be written. This address should be a 256 byte boundary.
+ IAP_command[ 2 ] = (unsigned int)source_addr; // Source RAM address from which data bytes are to be read. This address should be a word boundary.
+ IAP_command[ 3 ] = size; // Number of bytes to be written. Should be 256 | 512 | 1024 | 4096.
+ IAP_command[ 4 ] = cclk_kHz; // CPU Clock Frequency (CCLK) in kHz.
+
+ iap_entry( IAP_command, IAP_result );
+
+ return ( (int)IAP_result[ 0 ] );
+}
+
+/** Copy EEPROM to RAM (LPC11U24)
+ *
+ * @param source_addr Source EEPROM address from which data bytes are to be read.
+ * @param target_addr Destination RAM address where data bytes are to be written.
+ * @param size Number of bytes to be written.
+ * @return error code: CMD_SUCCESS | SRC_ADDR_NOT_MAPPED | DST_ADDR_NOT_MAPPED
+ * Remark: The top 64 bytes of the EEPROM memory are reserved and cannot be written to.
+ */
+int IAP::read_eeprom( char *source_addr, char *target_addr, int size ) {
+ IAP_command[ 0 ] = IAPCommand_EEPROM_Read;
+ IAP_command[ 1 ] = (unsigned int)source_addr; // Source EEPROM address from which data bytes are to be read. This address should be a word boundary.
+ IAP_command[ 2 ] = (unsigned int)target_addr; // Destination RAM address where data bytes are to be written. This address should be a 256 byte boundary.
+ IAP_command[ 3 ] = size; // Number of bytes to be written. Should be 256 | 512 | 1024 | 4096.
+ IAP_command[ 4 ] = cclk_kHz; // CPU Clock Frequency (CCLK) in kHz.
+
+ iap_entry( IAP_command, IAP_result );
+
+ return ( (int)IAP_result[ 0 ] );
+}
+#endif
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/IAP/IAP.h Fri Nov 15 20:53:36 2013 +0000
@@ -0,0 +1,287 @@
+/** IAP : internal Flash memory access library
+ *
+ * The internal Flash memory access is described in the LPC1768 and LPC11U24 usermanual.
+ * http://www.nxp.com/documents/user_manual/UM10360.pdf
+ * http://www.nxp.com/documents/user_manual/UM10462.pdf
+ *
+ * LPC1768 --
+ * Chapter 2: "LPC17xx Memory map"
+ * Chapter 32: "LPC17xx Flash memory interface and programming"
+ * refering Rev. 01 - 4 January 2010
+ *
+ * LPC11U24 --
+ * Chapter 2: "LPC11Uxx Memory mapping"
+ * Chapter 20: "LPC11Uxx Flash programming firmware"
+ * refering Rev. 03 - 16 July 2012
+ *
+ * Released under the MIT License: http://mbed.org/license/mit
+ *
+ * revision 1.0 09-Mar-2010 1st release
+ * revision 1.1 12-Mar-2010 chaged: to make possible to reserve flash area for user
+ * it can be set by USER_FLASH_AREA_START and USER_FLASH_AREA_SIZE in IAP.h
+ * revision 2.0 26-Nov.2012 LPC11U24 code added
+ * revision 2.1 26-Nov-2012 EEPROM access code imported from Suga koubou san's (http://mbed.org/users/okini3939/) library
+ * http://mbed.org/users/okini3939/code/M0_EEPROM_test/
+ */
+
+#ifndef MBED_IAP
+#define MBED_IAP
+
+#include "mbed.h"
+
+#if defined(TARGET_LPC1768)
+
+#define USER_FLASH_AREA_START FLASH_SECTOR_29
+//#define USER_FLASH_AREA_SIZE (FLASH_SECTOR_SIZE_16_TO_29 * 1)
+#define USER_FLASH_AREA_SIZE (FLASH_SECTOR_SIZE_16_TO_29 * 1)
+
+/*
+ * memory map information is available in next URL also.
+ * http://mbed.org/projects/libraries/svn/mbed/trunk/LPC1768/LPC17xx.h
+ */
+
+/** Table for start adress of sectors
+ *
+ * LPC1768 internal flash memory sector numbers and addresses
+ *
+ * LPC1768 flash memory are and sector number/size
+ * Table 568 "Sectors in a LPC17xx device", Section 5. "Sector numbers", usermanual
+ *
+ * 0x00000000 - 0x0007FFFF flash (29 sectors)
+ *
+ * Sector0: 0x00000000 - 0x00000FFF 4K
+ * Sector1: 0x00001000 - 0x00001FFF 4K
+ * Sector2: 0x00002000 - 0x00002FFF 4K
+ * Sector3: 0x00003000 - 0x00003FFF 4K
+ * Sector4: 0x00004000 - 0x00004FFF 4K
+ * Sector5: 0x00005000 - 0x00005FFF 4K
+ * Sector6: 0x00006000 - 0x00006FFF 4K
+ * Sector7: 0x00007000 - 0x00007FFF 4K
+ * Sector8: 0x00008000 - 0x00008FFF 4K
+ * Sector9: 0x00009000 - 0x00009FFF 4K
+ * Sector10: 0x0000A000 - 0x0000AFFF 4K
+ * Sector11: 0x0000B000 - 0x0000BFFF 4K
+ * Sector12: 0x0000C000 - 0x0000CFFF 4K
+ * Sector13: 0x0000D000 - 0x0000DFFF 4K
+ * Sector14: 0x0000E000 - 0x0000EFFF 4K
+ * Sector15: 0x0000F000 - 0x0000FFFF 4K
+ *
+ * Sector16: 0x00010000 - 0x00017FFF 32K
+ * Sector17: 0x00018000 - 0x0001FFFF 32K
+ * Sector18: 0x00020000 - 0x00027FFF 32K
+ * Sector19: 0x00028000 - 0x0002FFFF 32K
+ * Sector20: 0x00030000 - 0x00037FFF 32K
+ * Sector21: 0x00038000 - 0x0003FFFF 32K
+ * Sector22: 0x00040000 - 0x00047FFF 32K
+ * Sector23: 0x00048000 - 0x0004FFFF 32K
+ * Sector24: 0x00050000 - 0x00057FFF 32K
+ * Sector25: 0x00058000 - 0x0005FFFF 32K
+ * Sector26: 0x00060000 - 0x00067FFF 32K
+ * Sector27: 0x00068000 - 0x0006FFFF 32K
+ * Sector28: 0x00070000 - 0x00077FFF 32K
+ * Sector29: 0x00078000 - 0x0007FFFF 32K
+ */
+
+#define FLASH_SECTOR_0 0x00000000
+#define FLASH_SECTOR_1 0x00001000
+#define FLASH_SECTOR_2 0x00002000
+#define FLASH_SECTOR_3 0x00003000
+#define FLASH_SECTOR_4 0x00004000
+#define FLASH_SECTOR_5 0x00005000
+#define FLASH_SECTOR_6 0x00006000
+#define FLASH_SECTOR_7 0x00007000
+#define FLASH_SECTOR_8 0x00008000
+#define FLASH_SECTOR_9 0x00009000
+#define FLASH_SECTOR_10 0x0000A000
+#define FLASH_SECTOR_11 0x0000B000
+#define FLASH_SECTOR_12 0x0000C000
+#define FLASH_SECTOR_13 0x0000D000
+#define FLASH_SECTOR_14 0x0000E000
+#define FLASH_SECTOR_15 0x0000F000
+#define FLASH_SECTOR_16 0x00010000
+#define FLASH_SECTOR_17 0x00018000
+#define FLASH_SECTOR_18 0x00020000
+#define FLASH_SECTOR_19 0x00028000
+#define FLASH_SECTOR_20 0x00030000
+#define FLASH_SECTOR_21 0x00038000
+#define FLASH_SECTOR_22 0x00040000
+#define FLASH_SECTOR_23 0x00048000
+#define FLASH_SECTOR_24 0x00050000
+#define FLASH_SECTOR_25 0x00058000
+#define FLASH_SECTOR_26 0x00060000
+#define FLASH_SECTOR_27 0x00068000
+#define FLASH_SECTOR_28 0x00070000
+#define FLASH_SECTOR_29 0x00078000
+#define FLASH_SECTOR_SIZE_0_TO_15 ( 4 * 1024)
+#define FLASH_SECTOR_SIZE_16_TO_29 (32 * 1024)
+
+static char * sector_start_adress[] = {
+ (char *)FLASH_SECTOR_0,
+ (char *)FLASH_SECTOR_1,
+ (char *)FLASH_SECTOR_2,
+ (char *)FLASH_SECTOR_3,
+ (char *)FLASH_SECTOR_4,
+ (char *)FLASH_SECTOR_5,
+ (char *)FLASH_SECTOR_6,
+ (char *)FLASH_SECTOR_7,
+ (char *)FLASH_SECTOR_8,
+ (char *)FLASH_SECTOR_9,
+ (char *)FLASH_SECTOR_10,
+ (char *)FLASH_SECTOR_11,
+ (char *)FLASH_SECTOR_12,
+ (char *)FLASH_SECTOR_13,
+ (char *)FLASH_SECTOR_14,
+ (char *)FLASH_SECTOR_15,
+ (char *)FLASH_SECTOR_16,
+ (char *)FLASH_SECTOR_17,
+ (char *)FLASH_SECTOR_18,
+ (char *)FLASH_SECTOR_19,
+ (char *)FLASH_SECTOR_20,
+ (char *)FLASH_SECTOR_21,
+ (char *)FLASH_SECTOR_22,
+ (char *)FLASH_SECTOR_23,
+ (char *)FLASH_SECTOR_24,
+ (char *)FLASH_SECTOR_25,
+ (char *)FLASH_SECTOR_26,
+ (char *)FLASH_SECTOR_27,
+ (char *)FLASH_SECTOR_28,
+ (char *)FLASH_SECTOR_29
+};
+
+#elif defined(TARGET_LPC11U24) || defined(TARGET_LPC1114)
+
+#define USER_FLASH_AREA_START FLASH_SECTOR_7
+#define USER_FLASH_AREA_SIZE (FLASH_SECTOR_SIZE * 1)
+
+/** Table for start adress of sectors
+ *
+ * LPC11U24 internal flash memory sector numbers and addresses
+ *
+ * LPC11U24 flash memory are and sector number/size
+ * Table 334 "LPC11U1x/2x flash sectors", Section 20. "Sector numbers", usermanual
+ *
+ * 0x00000000 - 0x00007FFF flash (8 sectors)
+ *
+ * Sector0: 0x00000000 - 0x00000FFF 4K
+ * Sector1: 0x00001000 - 0x00001FFF 4K
+ * Sector2: 0x00002000 - 0x00002FFF 4K
+ * Sector3: 0x00003000 - 0x00003FFF 4K
+ * Sector4: 0x00004000 - 0x00004FFF 4K
+ * Sector5: 0x00005000 - 0x00005FFF 4K
+ * Sector6: 0x00006000 - 0x00006FFF 4K
+ * Sector7: 0x00007000 - 0x00007FFF 4K
+ */
+
+#define FLASH_SECTOR_0 0x00000000
+#define FLASH_SECTOR_1 0x00001000
+#define FLASH_SECTOR_2 0x00002000
+#define FLASH_SECTOR_3 0x00003000
+#define FLASH_SECTOR_4 0x00004000
+#define FLASH_SECTOR_5 0x00005000
+#define FLASH_SECTOR_6 0x00006000
+#define FLASH_SECTOR_7 0x00007000
+#define FLASH_SECTOR_SIZE (4 * 1024)
+
+static char * sector_start_adress[] = {
+ (char *)FLASH_SECTOR_0,
+ (char *)FLASH_SECTOR_1,
+ (char *)FLASH_SECTOR_2,
+ (char *)FLASH_SECTOR_3,
+ (char *)FLASH_SECTOR_4,
+ (char *)FLASH_SECTOR_5,
+ (char *)FLASH_SECTOR_6,
+ (char *)FLASH_SECTOR_7,
+};
+
+#endif
+
+
+/** Error code by IAP routine
+ *
+ * Table 588 "ISP Return Codes Summary", Section 7.15 "ISP Return Codes", usermanual
+ */
+
+enum error_code
+ {
+ CMD_SUCCESS,
+ INVALID_COMMAND,
+ SRC_ADDR_ERROR,
+ DST_ADDR_ERROR,
+ SRC_ADDR_NOT_MAPPED,
+ DST_ADDR_NOT_MAPPED,
+ COUNT_ERROR,
+ INVALID_SECTOR,
+ SECTOR_NOT_BLANK,
+ SECTOR_NOT_PREPARED_FOR_WRITE_OPERATION,
+ COMPARE_ERROR,
+ BUSY,
+ PARAM_ERROR,
+ ADDR_ERROR,
+ ADDR_NOT_MAPPED,
+ CMD_LOCKED,
+ INVALID_CODE,
+ INVALID_BAUD_RATE,
+ INVALID_STOP_BIT,
+ CODE_READ_PROTECTION_ENABLED
+ };
+
+
+
+/*
+ * IAP routine entry
+ *
+ * "IAP commands"
+ */
+
+#define IAP_LOCATION 0x1fff1ff1
+typedef void (*IAP_call)(unsigned int [], unsigned int []);
+
+/** IAP class
+ *
+ * Interface for internal flash memory access
+ */
+
+
+class IAP {
+public:
+
+ /*
+ * SystemCoreClock ??? :
+ * http://mbed.org/forum/mbed/topic/229/
+ * http://mbed.org/users/simon/programs/SystemCoreClock/16mhsh/
+ */
+
+
+ /** Constructor for IAP
+ *
+ */
+
+ IAP() : iap_entry( reinterpret_cast<IAP_call>(IAP_LOCATION) ), cclk_kHz( SystemCoreClock / 1000 ) {}
+ int read_ID( void );
+ int read_serial( void );
+ int blank_check( int start, int end );
+ int erase( int start, int end );
+ int prepare( int start, int end );
+ int write( char *source_addr, char *target_addr, int size );
+ int compare( char *source_addr, char *target_addr, int size );
+ int read_BootVer( void );
+
+ char *reserved_flash_area_start( void );
+ int reserved_flash_area_size( void );
+
+#if defined(TARGET_LPC11U24)
+ int write_eeprom( char *source_addr, char *target_addr, int size );
+ int read_eeprom( char *source_addr, char *target_addr, int size );
+#endif
+
+private:
+ IAP_call iap_entry;
+ unsigned int IAP_command[ 5 ];
+ unsigned int IAP_result[ 5 ];
+ int cclk_kHz;
+
+ //int cpu_clock( void );
+}
+;
+
+#endif // #ifndef MBED_IAP
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/PID/PID.cpp Fri Nov 15 20:53:36 2013 +0000
@@ -0,0 +1,52 @@
+#include "mbed.h"
+#include "SerialLcd.h"
+#include "PID.h"
+
+PID::PID()
+{
+ interval = 0.05;
+ integral_limit = 50;
+ pid_limit = 300;
+ kp = 0.5;
+ ki = 0.5;
+ kd = 0.2;
+ integral = 0;
+ old = 0;
+}
+
+void PID::init(float KP,float KI,float KD,float PID_LIM,float INTEGRAL_LIM)
+{
+// interval = INTERVAL;
+ pid_limit = PID_LIM;
+ integral_limit = INTEGRAL_LIM;
+ kp = KP;
+ ki = KI;
+ kd = KD;
+ integral = 0;
+ old = 0;
+}
+
+float PID::calc(float value,float target,float interval)
+{
+ cur = value - target;
+// if ( (old >= 0 && cur <= 0) || (old <= 0 && cur >= 0) || fabsf(cur) < 1 )
+// integral = 0;
+ integral += ( cur + old ) * interval * 0.5f;
+ if ( integral < -integral_limit ) integral = -integral_limit;
+ if ( integral > integral_limit ) integral = integral_limit;
+ float pid = kp * cur + ki * integral + kd * ( cur - old ) / interval;
+ if ( pid < -pid_limit ) pid = -pid_limit;
+ if ( pid > pid_limit ) pid = pid_limit;
+ old = cur;
+ return pid;
+}
+
+void PID::reset()
+{
+ integral = 0;
+ old = 0;
+}
+;
+
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/PID/PID.h Fri Nov 15 20:53:36 2013 +0000
@@ -0,0 +1,22 @@
+#ifndef MBED_PID_H
+#define MBED_PID_H
+
+#include "mbed.h"
+#include "stdarg.h"
+
+class PID
+{
+public:
+ PID();
+ void init(float KP,float KI,float KD,float PID_LIM,float INTGRAL_LIM);
+ float calc(float value,float target,float interval);
+ void reset();
+private:
+ float integral;
+ float interval;
+ float cur,old;
+ float kp,ki,kd;
+ float integral_limit;
+ float pid_limit;
+};
+#endif
\ No newline at end of file
--- a/PulseWidthCounter.lib Tue Jul 16 06:37:28 2013 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://mbed.org/users/komaida424/code/PulseWidthCounter/#d37d6388c179
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/PulseWidthCounter/PulseWidthCounter.cpp Fri Nov 15 20:53:36 2013 +0000
@@ -0,0 +1,28 @@
+#include "mbed.h"
+#include "InterruptIn.h"
+#include "PulseWidthCounter.h"
+
+PulseWidthCounter::PulseWidthCounter(PinName _interrupt,bool positive) : interrupt(_interrupt) //constructa
+{
+ if ( positive )
+ { interrupt.rise(this,&PulseWidthCounter::start);
+ interrupt.fall(this,&PulseWidthCounter::stop);
+ }
+ else
+ { interrupt.fall(this,&PulseWidthCounter::start);
+ interrupt.rise(this,&PulseWidthCounter::stop);
+ }
+}
+
+void PulseWidthCounter::start()
+{
+ _time.reset();
+ _time.start();
+}
+
+void PulseWidthCounter::stop()
+{
+ _time.stop();
+ count = _time.read_us();
+}
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/PulseWidthCounter/PulseWidthCounter.h Fri Nov 15 20:53:36 2013 +0000
@@ -0,0 +1,22 @@
+#ifndef PULSEWIDTHCOUNTER_H
+#define PULSEWIDTHCOUNTER_H
+#define POSITIVE true
+#define NEGATIVE false
+
+#include "mbed.h"
+
+class PulseWidthCounter
+{
+private:
+ Timer _time;
+ void start();
+ void stop();
+ InterruptIn interrupt;
+
+public:
+ // Timer time;
+ PulseWidthCounter(PinName,bool positive=true);
+
+ int count;
+};
+#endif
\ No newline at end of file
--- a/SerialLcd.lib Tue Jul 16 06:37:28 2013 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://mbed.org/users/komaida424/code/SerialLcd/#f30bad3f815d
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/SerialLcd/SerialLcd.cpp Fri Nov 15 20:53:36 2013 +0000
@@ -0,0 +1,31 @@
+#include "mbed.h"
+#include "SerialLcd.h"
+
+SerialLcd::SerialLcd(PinName TX): _lcd(TX,NC)
+{
+// LCD_contrast = 60;
+}
+
+int SerialLcd::_putc(int value) {
+ _lcd.putc(value);
+ return value;
+}
+
+int SerialLcd::_getc() {
+ return -1;
+}
+
+void SerialLcd::cls()
+{
+ _lcd.putc(0xFE);
+ _lcd.putc(0x01);
+ wait(0.01);
+}
+
+void SerialLcd::locate(int clm,int row)
+{
+ _lcd.putc(0xFE);
+ _lcd.putc( 0x80 + (row * 0x40) + clm );
+ wait(0.01);
+}
+;
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/SerialLcd/SerialLcd.h Fri Nov 15 20:53:36 2013 +0000
@@ -0,0 +1,23 @@
+#ifndef MBED_SERIALLCD_H
+#define MBED_SERIALLCD_H
+
+#include "mbed.h"
+#include "stdarg.h"
+
+class SerialLcd : public Stream
+{
+public:
+ SerialLcd(PinName);
+
+ void cls();
+ void locate(int,int);
+private:
+ virtual int _putc(int value);
+ virtual int _getc();
+ Serial _lcd;
+ int LCD_addr;
+ char LCD_cmd;
+ char LCD_data;
+ int LCD_contrast;
+};
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/SoftPWM/SoftPWM.cpp Fri Nov 15 20:53:36 2013 +0000
@@ -0,0 +1,99 @@
+#include "mbed.h"
+#include "InterruptIn.h"
+#include "SoftPWM.h"
+
+SoftPWM::SoftPWM(PinName _outpin,bool _positive) : pulse(_outpin) //constructa
+{
+ if ( _positive )
+ pulse = 0;
+ else
+ pulse = 1;
+ positive = _positive;
+ interval = 0.02;
+ width = 0;
+ start();
+}
+
+float SoftPWM::read()
+{
+ if ( width <= 0.0 ) return 0.0;
+ if ( width > 1.0 ) return 1.0;
+ return width / interval;
+}
+
+void SoftPWM::write(float duty)
+{
+ width = interval * duty;
+ if ( duty <= 0.0 ) width = 0.0;
+ if ( duty > 1.0 ) width = interval;
+}
+
+void SoftPWM::start()
+{
+ _ticker.attach(this,&SoftPWM::TickerInterrapt,interval);
+}
+
+void SoftPWM::stop()
+{
+ _ticker.detach();
+ if ( positive )
+ pulse = 0;
+ else
+ pulse = 1;
+ wait(width);
+}
+
+void SoftPWM::period(float _period)
+{
+ interval = _period;
+ start();
+}
+
+void SoftPWM::period_ms(int _period)
+{
+ period((float)_period / 1000);
+ start();
+}
+
+void SoftPWM::period_us(int _period)
+{
+ period((float)_period / 1000000);
+ start();
+}
+
+void SoftPWM::pulsewidth(float _width)
+{
+ width = _width;
+ if ( width < 0.0 ) width = 0.0;
+}
+
+void SoftPWM::pulsewidth_ms(int _width)
+{
+ pulsewidth((float)_width / 1000);
+}
+
+void SoftPWM::pulsewidth_us(int _width)
+{
+ pulsewidth((float)_width / 1000000);
+}
+
+void SoftPWM::TickerInterrapt()
+{
+ if ( width <= 0 ) return;
+ _timeout.attach(this,&SoftPWM::end,width);
+ if ( positive )
+ pulse = 1;
+ else
+ pulse = 0;
+}
+
+void SoftPWM::end()
+{
+ if ( positive )
+ pulse = 0;
+ else
+ pulse = 1;
+// _timeout.detach();
+}
+;
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/SoftPWM/SoftPWM.h Fri Nov 15 20:53:36 2013 +0000
@@ -0,0 +1,45 @@
+#ifndef SoftPWM_H
+#define SoftPWM_H
+#define POSITIVE true
+#define NEGATIVE false
+
+#include "mbed.h"
+
+class SoftPWM
+{
+private:
+ Timeout _timeout;
+ Ticker _ticker;
+ void end();
+ DigitalOut pulse;
+ bool positive;
+ void TickerInterrapt();
+ float width;
+ float interval;
+public:
+ SoftPWM(PinName,bool mode=true);
+// void attach_us(int);
+ void start();
+ void write(float);
+ float read();
+ void pulsewidth(float);
+ void pulsewidth_ms(int);
+ void pulsewidth_us(int);
+ void period(float);
+ void period_ms(int);
+ void period_us(int);
+ void stop();
+ operator float() {
+ if ( width <= 0.0 ) return 0.0;
+ if ( width > 1.0 ) return 1.0;
+ return width / interval;
+ }
+ SoftPWM& operator=(float duty) {
+ width = interval * duty;
+ if ( duty <= 0.0 ) width = 0.0;
+ if ( duty > 1.0 ) width = interval;
+ return *this;
+ }
+
+};
+#endif
\ No newline at end of file
--- 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 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
--- a/config.h Tue Jul 16 06:37:28 2013 +0000
+++ b/config.h Fri Nov 15 20:53:36 2013 +0000
@@ -1,17 +1,18 @@
#ifndef CONFIG_H
#define CONFIG_H
-#define SERIAL_LCD
+//#define SERIAL_LCD
+//#define SOFT_PWM
//#define LPCXpresso
//#define LocalFileOut
#define TX_TYPE 0 // 0:FM 1:IR
-#define Thro_Min 25
+#define Thro_Zero 30
#define Thro_Lo 75
#define Thro_Hi 950
#define Pulse_Min 1000
#define Pulse_Max 2000
-#define Stick_Limit 150
+#define Stick_Limit 350
#define M1 M[0]
#define M2 M[1]
#define M3 M[2]
@@ -25,46 +26,53 @@
#define AUX CH[4]
#define _ITG3200 0x00
#define _L3GD20 0x01
+#define TICK_TIME 0.05
+#define GYRO_ADJUST 3
//enum PortNum{Port_THR=1,Port_AIL=3,Port_ELE=5,Port_RUD=7,Port_AUX=6};
enum IRNum{IR_THR=1,IR_AIL,IR_ELE,IR_RUD,IR_AUX}; //Input Pulse Sequence
enum channel{ROL=0,PIT,YAW,COL,GAIN};
-
struct config {
float Revision;
int Struct_Size;
+ char StartMode; //'c':config mode 'f':flight mode
int Stick_Ref[5]; //Stick Neutral Position
float Stick_Mix[3]; //Mixing ratio of stick operation
signed char Gyro_Dir[4];
float Gyro_Gain[6];
signed char Accel_Dir[4];
+ float Accel_Ref[3];
float Accel_Gain[3];
int Gyro_Gain_Setting;
- int Gyro_Divider;
- int Gyro_LPF_Value;
- int Gyro_SamplRate_Divider;
+ float Control_Exchange_Angle;
+ float Cutoff_Freq;
+ int Flight_Time;
int LCD_Contrast;
int PWM_Mode;
- float Gyro_Stick_offset_effect;
+ int ESC_Low;
int PWM_Interval;
// int Accel_Rang;
// int Accel_Rate;
// int PWM_Resolution;
- char StartMode;
- char Gyro_Type;
+ char Angle_Control;
+ float kp[3];
+ float ki[3];
+ float kd[3];
+ float PID_Limit;
+ float Integral_Limit;
public:
config() {
- Revision = 1.03;
+ Revision = 1.25;
Struct_Size = sizeof(config);
Stick_Ref[0] = 1500;
Stick_Ref[1] = 1500;
Stick_Ref[2] = 1500;
Stick_Ref[3] = 1100;
Stick_Ref[4] = 1500;
- Stick_Mix[0] = 0.5;
- Stick_Mix[1] = 0.5;
- Stick_Mix[2] = 1.0;
+ Stick_Mix[0] = 0.3;
+ Stick_Mix[1] = 0.3;
+ Stick_Mix[2] = 0.65;
Gyro_Dir[0] = 1;
Gyro_Dir[1] = 1;
Gyro_Dir[2] = 1;
@@ -79,22 +87,36 @@
Accel_Dir[1] = 1;
Accel_Dir[2] = 1;
Accel_Dir[3] = 1;
+ Accel_Ref[0] = 0;
+ Accel_Ref[1] = 0;
+ Accel_Ref[2] = 0;
Accel_Gain[0] = 0.50;
Accel_Gain[1] = 0.50;
Accel_Gain[2] = 0.50;
Gyro_Gain_Setting = -1;
- Gyro_Divider=7;
- Gyro_LPF_Value=0;
- Gyro_SamplRate_Divider=1;
+ Cutoff_Freq=0.15;
+ Flight_Time=360;
LCD_Contrast = 60;
PWM_Mode = 1;
- Gyro_Stick_offset_effect = 0.00;
+ ESC_Low= 1025;
PWM_Interval = 3000;
// Accel_Rang = 2;
// Accel_Rate = 14;
// PWM_Resolution = 0;
StartMode = 'C';
- Gyro_Type = _ITG3200;
+ Angle_Control = 'H'; // H:horizn A:angle
+ kp[0] = 1.5;
+ kp[1] = 1.5;
+ kp[2] = 1.5;
+ ki[0] = 0.7;
+ ki[1] = 0.7;
+ ki[2] = 0.7;
+ kd[0] = 1.2;
+ kd[1] = 1.2;
+ kd[2] = 1.2;
+ PID_Limit = 300;
+ Integral_Limit = 30;
+ Control_Exchange_Angle = 15;
}
};
#endif
@@ -104,3 +126,30 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
--- a/main.cpp Tue Jul 16 06:37:28 2013 +0000
+++ b/main.cpp Fri Nov 15 20:53:36 2013 +0000
@@ -1,74 +1,139 @@
#include "mbed.h"
+#include "math.h"
#include "I2cPeripherals.h"
-//#include "I2cLCD.h"
#include "InterruptIn.h"
-//#include "ITG3200.h"
-//#include "L3GD20.h"
#include "config.h"
#include "PulseWidthCounter.h"
#include "string"
#include "SerialLcd.h"
#include "IAP.h"
-//LPC1768 Flash Memory read/write
-#define MEM_SIZE 256
-#define TARGET_SECTOR 29 // use sector 29 as target sector if it is on LPC1768
-IAP iap;
+#include "PID.h"
+#include "SoftPWM.h"
+
+//Serial pc(USBTX, USBRX);
+
+//Gravity at Earth's surface in m/s/s
+#define g0 9.812865328
+//Convert from radians to degrees.
+#define toDegrees(x) (x * 57.2957795)
+//Convert from degrees to radians.
+#define toRadians(x) (x * 0.01745329252)
+//ITG-3200 sensitivity is 14.375 LSB/(degrees/sec).
+#define GYROSCOPE_GAIN (1 / 14.375)
+//Full scale resolution on the ADXL345 is 4mg/LSB.
+#define ACCELEROMETER_GAIN (0.004 * g0)
+//Updating filter at 40Hz.
+#define FILTER_RATE 0.05
+//At rest the gyroscope is centred around 0 and goes between about
+//-5 and 5 counts. As 1 degrees/sec is ~15 LSB, error is roughly
+//5/15 = 0.3 degrees/sec.
-#ifdef LPCXpresso
-DigitalOut led1(P0_22);
-#define led2 led1
-#else
+#ifdef TARGET_LPC1114 // LPC1114
+// PulseOut pwm[4] = { dp1,dp2,dp18,dp24 );
+ #define LED1 dp28
+ #define p5 dp4
+ #define p6 dp9
+ #define p7 dp10
+ #define p8 dp11
+ #define p9 dp25
+ #define p10 dp26
+ #define p13 dp16
+ #define p21 dp1
+ #define p22 dp2
+ #define p23 dp18
+ #define p24 dp24
+ #define p27 dp27
+ #define p28 dp5
+#else //LPC1768
+ #ifdef LPCXpresso
+ #define LED1 P0_22
+ #endif
+#endif
+DigitalInOut pwmpin[] = { p21,p22,p23,p24 };
DigitalOut led1(LED1);
DigitalOut led2(LED2);
+InterruptIn ch1(p5);
+PulseWidthCounter ch[5] = { p6,p7,p8,p9,p10 };
+#if defined(SOFT_PWM) || defined(TARGET_LPC1114)
+SoftPWM pwm[4] = { p21,p22,p23,p24 };
+#else
+PwmOut pwm[4] = { p21,p22,p23,p24 };
#endif
-InterruptIn ch1(p5);
-PulseWidthCounter ch[5] = { PulseWidthCounter(p6,POSITIVE),
- PulseWidthCounter(p7,POSITIVE),
- PulseWidthCounter(p8,POSITIVE),
- PulseWidthCounter(p9,POSITIVE),
- PulseWidthCounter(p10,POSITIVE)
- };
-PwmOut pwm[4] = { p21,p22,p23,p24 };
+SoftPWM buzz(p26);
Timer CurTime;
-Timer ElapTime;
+//Timer ElapTime;
+Timer CycleTime;
+Timer FlyghtTime;
+//Ticker tick;
+//Ticker tick100ms;
+//Ticker mixTime;
I2cPeripherals i2c(p28,p27); //sda scl
-#ifdef SERIAL_LCD
SerialLcd lcd(p13);
-#endif
config conf;
-int StartTime;
-int Channel = 0;
-int CH[5];
-int M[6];
-int Gyro[3];
-int Accel[3];
-int Gyro_Ref[3];
-int Accel_Ref[3];
-int Stick[5];
+PID pid[3];
+#ifdef TARGET_LPC1114
+//LPC1114 Flash Memory read/write
+ #define MEM_SIZE 256
+ #define TARGET_SECTOR 7 // use sector 29 as target sector if it is on LPC1768
+#else
+//LPC1768 Flash Memory read/write
+ #define MEM_SIZE 256
+ #define TARGET_SECTOR 29 // use sector 29 as target sector if it is on LPC1768
+#endif
+#ifndef LocalFileOut
+IAP iap;
+#endif
+float TotalTime = 0;;
+int channel = 0;
+//int intrupt_cnt = 0;
+//int intrupt_cnt2 = 0;
+volatile int CH[5];
+volatile int M[6];
+volatile float Gyro[3];
+volatile float Accel[3];
+volatile float Angle[3];
+volatile float Gyro_Ref[3];
+volatile float Gyro_Save[3];
+volatile int Stick[5];
+volatile int Stick_Save[3];
+//int Stick_Max[3];
float Press;
char InPulseMode; //Receiver Signal Type 's':Serial, 'P':Parallel
+//volatile bool tick_flag;
+//volatile bool buzz_flag;
+float interval;
+int pid_reg[3];
+int loop_cnt;
void initialize();
void FlashLED(int );
void SetUp();
-#ifdef SERIAL_LCD
-void SetUpPrompt(config&,SerialLcd&);
-#else
void SetUpPrompt(config&,I2cPeripherals&);
-#endif
void PWM_Out(bool);
void Get_Stick_Pos();
void CalibrateGyros(void);
void CalibrateAccel(void);
void Get_Gyro();
-void Get_Accel();
+bool Get_Accel();
+void Get_Angle(float);
void ReadConfig();
void WriteConfig();
+void Interrupt_Check(bool&,int &,DigitalOut &);
void ESC_SetUp(void);
-
+void Flight_SetUp();
+//void IMUfilter_Reset(void);
+void LCD_printf(char *);
+void LCD_cls();
+void LCD_locate(int,int);
+/*
+void tick_interrupt()
+{
+ tick_flag = true;
+}
+*/
void PulseCheck()
{
- Channel++;
+ channel++;
}
void PulseAnalysis() //Interrupt Pin5
@@ -77,10 +142,10 @@
int PulseWidth = CurTime.read_us();
CurTime.reset();
CurTime.start();
- if ( PulseWidth > 3000 ) Channel = 0; //reset pulse count
+ if ( PulseWidth > 3000 ) channel = 0; //reset pulse count
else {
if ( PulseWidth > Pulse_Min && PulseWidth < Pulse_Max ) {
- switch( Channel ) {
+ switch( channel ) {
case IR_THR:
THR = PulseWidth;
break;
@@ -99,49 +164,51 @@
}
}
}
- Channel++;
+ channel++;
}
int main()
{
- int i,j=0;
+ int i=0;
+ wait(0.5);
initialize();
- wait(0.5);
+
Get_Stick_Pos();
- while ( Stick[COL] > 30 || conf.StartMode == 'C' ) //Shrottol Low
+ while ( Stick[COL] > Thro_Zero || conf.StartMode == 'C' ) //Shrottol Low
{
- if ( Stick[COL] > 350 || conf.StartMode == 'C' ) // Shrottle High
+ if ( Stick[COL] > 890 && -Stick[YAW] < Stick_Limit ) // Shrottle High
+ ESC_SetUp();
+ if ( Stick[COL] > 890 || conf.StartMode == 'C' ) // Shrottle High
{
-#ifdef SERIAL_LCD
- SetUpPrompt(conf,lcd);
-#else
+ loop_cnt = 0;
SetUpPrompt(conf,i2c);
-#endif
- for ( i=0;i<4;i++ ) pwm[i].period_us(conf.PWM_Interval);
break;
}
FlashLED(3);
- wait(1);
+ wait(0.3);
Get_Stick_Pos();
}
- led2 = 1;
- ElapTime.start();
-
+ Flight_SetUp();
while (1)
{
- if ( Stick[COL] < 30 )
+ if ( Stick[COL] < Thro_Zero )
{
i = 0;
- ElapTime.stop();
- while ( Stick[YAW] < -Stick_Limit && Stick[COL] < 30 )
+ while ( Stick[YAW] < -Stick_Limit && Stick[COL] < Thro_Zero ) //Rudder Left
{
if ( i > 100 ) //wait 2 sec
{
+ FlyghtTime.stop();
+ if ( Stick[PIT] < -Stick_Limit ) { //Elevetor Down
+ loop_cnt = 0;
+ FlashLED(5);
+ for ( i=0;i<4;i++ ) pwm[i].pulsewidth_us(Pulse_Min);
+ i2c.start(conf.LCD_Contrast);
+ SetUpPrompt(conf,i2c);
+ }
CalibrateGyros();
- CalibrateAccel();
- FlashLED(6);
- ElapTime.start();
+ Flight_SetUp();
break;
}
wait(0.01); // wait 10 msec
@@ -149,12 +216,6 @@
i++;
}
}
- j++;
- if (j>100) { j=0; led2 = !led2;}
- ElapTime.stop();
- wait(float(conf.PWM_Interval-ElapTime.read_us()-2)/1000000);
- ElapTime.reset();
- ElapTime.start();
PWM_Out(true);
}
@@ -162,38 +223,33 @@
void initialize()
{
-#ifndef SERIAL_LCD
- i2c.start(ST7032_ADDR,conf.LCD_Contrast);
-#endif
+ buzz.period_us(500);
ReadConfig(); //config.inf file read
-// CurTime.start();
- Channel = 0;
+
+ i2c.start(conf.LCD_Contrast);
+ channel = 0;
ch1.rise(&PulseCheck); //input pulse count
- wait(0.1);
- if ( Channel > 30 ) {
+ wait(0.2);
+ if ( channel > 50 ) {
ch1.rise(&PulseAnalysis);
InPulseMode = 'S';
}
else InPulseMode = 'P';
- if ( conf.Gyro_Type == _ITG3200 )
- i2c.start(ITG3200_ADDR0);
- else
- i2c.start(L3GD20_ADDR1);
- CalibrateGyros();
- i2c.start(LDXL345_ADDR0);
- CalibrateGyros();
- CalibrateAccel();
- i2c.start(LPS331AP_ADDR1);
+ led1 = 0;
+ CycleTime.start();
for ( int i=0;i<4;i++ ) pwm[i].period_us(conf.PWM_Interval);
+ FlashLED(3);
}
void FlashLED(int cnt)
{
for ( int i = 0 ; i < cnt ; i++ ) {
led1 = !led1;
- wait(0.05);
+ buzz = 0.5f;
+ wait(0.1);
led1 = !led1;
- wait(0.05);
+ buzz = 0.0f;
+ wait(0.1);
}
}
@@ -227,7 +283,8 @@
config *conf_ptr;
if ( sizeof(config) > 255 ) {
- i2c.printf("config size over");
+ LCD_printf("config size over");
+ wait(3);
return;
}
rc = iap.blank_check( TARGET_SECTOR, TARGET_SECTOR );
@@ -237,14 +294,10 @@
conf_ptr = (config*)sector_start_adress[TARGET_SECTOR];
if ( conf_ptr->Revision == conf.Revision && conf_ptr->Struct_Size == sizeof(config) ) {
for ( i=0;i<sizeof(config);i++ ) recv[i] = send[i];
-// i2c.printf("config read OK");
-// wait(1);
return;
}
}
WriteConfig();
-// i2c.printf("config write OK");
-// wait(1);
#endif
}
@@ -260,7 +313,8 @@
char *send;
int i;
if ( sizeof(config) > 255 ) {
- printf("config size over");
+ LCD_printf("config size over");
+ wait(3);
return;
}
send = (char*)&conf;
@@ -268,8 +322,10 @@
for ( i=sizeof(config);i<MEM_SIZE;i++ ) mem[i] = 0x00;
iap.prepare( TARGET_SECTOR, TARGET_SECTOR );
iap.erase( TARGET_SECTOR, TARGET_SECTOR );
+//pc.printf("erase\n");
iap.prepare( TARGET_SECTOR, TARGET_SECTOR );
iap.write( mem, sector_start_adress[ TARGET_SECTOR ], MEM_SIZE );
+//pc.printf("write\n");
#endif
}
@@ -278,6 +334,10 @@
if ( InPulseMode == 'P' ) {
for (int i=0;i<5;i++) CH[i] = ch[i].count;
}
+// Stick_Save[ROL] = Stick[ROL];
+// Stick_Save[PIT] = Stick[PIT];
+// Stick_Save[YAW] = Stick[YAW];
+
Stick[ROL] = AIL - conf.Stick_Ref[ROL];
Stick[PIT] = ELE - conf.Stick_Ref[PIT];
Stick[YAW] = RUD - conf.Stick_Ref[YAW];
@@ -287,23 +347,61 @@
void Get_Gyro()
{
- int x,y,z;
- if ( conf.Gyro_Dir[3] ==1 ) i2c.angular(&x,&y,&z);
- else i2c.angular(&y,&x,&z);
- Gyro[ROL] = ( x - Gyro_Ref[0] ) / 5;
- Gyro[PIT] = ( y - Gyro_Ref[1] ) / 5;
- Gyro[YAW] = ( z - Gyro_Ref[2] ) / 5;
+ float x,y,z;
+ bool err;
+ Gyro_Save[ROL] = Gyro[ROL];
+ Gyro_Save[PIT] = Gyro[PIT];
+ Gyro_Save[YAW] = Gyro[YAW];
+
+ if ( conf.Gyro_Dir[3] ==1 ) err=i2c.angular(&x,&y,&z);
+ else err=i2c.angular(&y,&x,&z);
+ if ( err == false ) return;
+ Gyro[ROL] = x - Gyro_Ref[0] ;
+ Gyro[PIT] = y - Gyro_Ref[1] ;
+ Gyro[YAW] = z - Gyro_Ref[2] ;
+
+ if ( fabs(Gyro[ROL]) > 300.0 ) Gyro[ROL] = Gyro_Save[ROL];
+ if ( fabs(Gyro[PIT]) > 300.0 ) Gyro[PIT] = Gyro_Save[PIT];
+ if ( fabs(Gyro[YAW]) > 300.0 ) Gyro[YAW] = Gyro_Save[YAW];
}
-void Get_Accel()
+bool Get_Accel()
+{
+ float x,y,z;
+ bool err;
+ if ( conf.Gyro_Dir[3] ==1 ) err=i2c.Acceleration(&x,&y,&z);
+ else err=i2c.Acceleration(&y,&x,&z);
+ if ( err == false ) return false;
+ float wx = x - conf.Accel_Ref[0];
+ float wy = y - conf.Accel_Ref[1];
+ float wz = z - conf.Accel_Ref[2];
+ Accel[ROL] = atan(wx/sqrt( pow(wy,2)+pow(wz,2)))*180/3.14;
+ Accel[PIT] = atan(wy/sqrt( pow(wx,2)+pow(wz,2)))*180/3.14;
+ Accel[YAW] = atan(sqrt( pow(wx,2)+pow(wy,2))/wz)*180/3.14;
+ return true;
+}
+
+void Get_Angle(float interval)
{
- int x,y,z;
- if ( conf.Gyro_Dir[3] ==1 ) i2c.Acceleration(&x,&y,&z);
- else i2c.Acceleration(&y,&x,&z);
- Accel[ROL] = ( x - Accel_Ref[0] );
- Accel[PIT] = ( y - Accel_Ref[1] );
- Accel[YAW] = ( z - Accel_Ref[2] );
+ Get_Gyro();
+ Get_Accel();
+ float x,y,z;
+
+ x = ( (Gyro[ROL] + Gyro_Save[ROL]) ) * 0.5;
+ y = ( (Gyro[PIT] + Gyro_Save[PIT]) ) * 0.5;
+ z = ( (Gyro[YAW] + Gyro_Save[YAW]) ) * 0.5;
+ if ( Get_Accel() == true ) {
+ float i = 3.14 * 2 * conf.Cutoff_Freq * interval;
+ Angle[ROL] += -Angle[ROL] * i + Accel[ROL] * i + x * interval;
+ Angle[PIT] += -Angle[PIT] * i + Accel[PIT] * i + y * interval;
+ }
+ else {
+ Angle[ROL] += x * interval;
+ Angle[PIT] += y * interval;
+ }
+ Angle[YAW] += z * interval;
}
+
void Get_Pressure()
{
Press = i2c.pressure();
@@ -311,36 +409,41 @@
void CalibrateGyros(void)
{
- int i,j,x,y,z;
- int k[3]={0,0,0};
+ int i;
+ float x,y,z;
+ float k[3]={0,0,0};
wait(1);
+ Angle[0]=Angle[1]=Angle[2]=0;
for(i=0; i<16; i++) {
- if ( conf.Gyro_Dir[3] ==1 ) i2c.angular(&x,&y,&z);
- else i2c.angular(&y,&x,&z);
+ if ( conf.Gyro_Dir[3] ==1 ) i2c.angular(&x,&y,&z);
+ else i2c.angular(&y,&x,&z);
k[0] += x;
k[1] += y;
k[2] += z;
- wait(0.005);
+ wait(0.01);
}
- for( j=0; j<3; j++ ) Gyro_Ref[j] = k[j]/16;
- FlashLED(3);
+ for( i=0; i<3; i++ ) Gyro_Ref[i] = k[i]/16;
+// FlashLED(3);
}
void CalibrateAccel(void)
{
- int i,j,x,y,z;
- int k[3]={0,0,0};
- wait(1);
+ int i;
+ float x,y,z;
+ float k[3]={0,0,0};
+ conf.Accel_Ref[0]=conf.Accel_Ref[1]=conf.Accel_Ref[2]=0;
for(i=0; i<16; i++) {
- if ( conf.Gyro_Dir[3] ==1 ) i2c.Acceleration(&x,&y,&z);
- else i2c.Acceleration(&y,&x,&z);
+ if ( conf.Gyro_Dir[3] ==1 ) i2c.Acceleration(&x,&y,&z);
+ else i2c.Acceleration(&y,&x,&z);
k[0] += x;
k[1] += y;
k[2] += z;
- wait(0.005);
+ wait(0.01);
}
- for( j=0; j<3; j++ ) Accel_Ref[j] = k[j]/16;
- FlashLED(3);
+ conf.Accel_Ref[0] = k[0]/16;
+ conf.Accel_Ref[1] = k[1]/16;
+ conf.Accel_Ref[2] = k[2]/16-9.8;
+// FlashLED(3);
}
void PWM_Out(bool mode)
@@ -349,20 +452,50 @@
int i;
float gain;
-// wait(0.002);
Get_Stick_Pos();
- Get_Gyro();
-// Get_Accel();
-
M1 = M2 = M3 = M4 = Stick[COL];
- for ( i=0;i<3;i++ )
- {
+ interval = CycleTime.read();
+ CycleTime.reset();
+ if ( interval > 0.1 ) return;
+ TotalTime += interval;
+ if ( TotalTime > 0.5 ) {
+ led1 = !led1;
+ if ( ( !buzz ) && ( (float)conf.Flight_Time < FlyghtTime.read() ) ) buzz=0.5f;
+ else buzz=0.0;
+ TotalTime = 0;
+ }
+
+ Get_Angle(interval);
+
+ for ( i=0;i<3;i++ ) {
+
// Stick Angle Mixing
if ( conf.Gyro_Gain_Setting == 1 ) gain = conf.Gyro_Gain[i] * conf.Gyro_Dir[i];
else gain = ( (float)abs(Stick[GAIN])/100 + conf.Gyro_Gain[i+3] ) * conf.Gyro_Dir[i];
- reg[i] = ( Stick[i] * conf.Stick_Mix[i] ) + ( Gyro[i] * gain );
-// if ( Stick[GAIN] < 0 )
-// reg[i] += Accel[i] * conf.Accel_Gain[i];
+ if ( Stick[GAIN] > 0
+// || i == YAW
+// || (fabsf)(Angle[i]) > conf.Control_Exchange_Angle
+ ) {
+ reg[i] = Stick[i] * conf.Stick_Mix[i];
+ reg[i] += Gyro[i] * gain * GYRO_ADJUST;
+ }
+ else {
+ if ( (i == YAW) || (i2c.GetAddr(ACCEL_ADDR)==0) ) {
+ if ( abs(Stick_Save[i]) <= abs(Stick[i]>>2) ) {
+ Stick_Save[i] = Stick[i]>>2;
+ }
+ else {
+// pc.printf("PID RESET.%3d",i);wait(1);
+ Stick_Save[i] = 0;
+ pid[i].reset();
+ Angle[i] = 0;
+ }
+ }
+ reg[i] = Stick[i] * conf.Stick_Mix[i];
+ reg[i] += pid[i].calc(Angle[i],0,interval);
+// reg[i] = pid[i].calc(Angle[i],-(float)Stick[i]*60/400,interval);
+ }
+ pid_reg[i] = reg[i];
}
//Calculate Roll Pulse Width
M1 += reg[ROL];
@@ -384,25 +517,69 @@
for ( i=0;i<4;i++ )
{
+// if ( Stick[COL] > 150 && M[i] < 150 ) M[i] = 150;
if ( M[i] > Thro_Hi ) M[i] = Thro_Hi;
if ( M[i] < Thro_Lo ) M[i] = Thro_Lo; // this is the motor idle level
}
-
- if (Stick[COL] < 20 ) M1=M2=M3=M4=0;
- if ( mode )
- for ( i=0;i<4;i++ ) pwm[i].pulsewidth_us(conf.Stick_Ref[COL]+M[i]);
-
+
+ if (Stick[COL] < Thro_Zero ) M1=M2=M3=M4=0;
+
+ if ( mode ) {
+ for ( i=0;i<4;i++ ) {
+ while ( !pwmpin[i] );
+ if ( conf.PWM_Mode == 1 )
+ pwm[i].pulsewidth_us(conf.ESC_Low+M[i]);
+ else pwm[i].pulsewidth_us(M[i]);
+ }
+ }
+
}
void ESC_SetUp(void) {
while(1) {
Get_Stick_Pos();
- for ( int i=0;i<4;i++ ) pwm[i].pulsewidth_us(Stick[COL]);
- wait(0.01);
+ for ( int i=0;i<4;i++ ) pwm[i].pulsewidth_us(conf.Stick_Ref[COL]+Stick[COL]);
+ wait(0.015);
}
-};
+}
+
+void Flight_SetUp(void)
+{
+ int i;
+ for ( i=0;i<4;i++ ) pwm[i].pulsewidth_us(0);
+ for ( i=0;i<4;i++ ) pwm[i].period_us(conf.PWM_Interval);
+ for ( i=0;i<4;i++ ) pwm[i].pulsewidth_us(Pulse_Min);
+ for ( i=0;i<4;i++ ) pid[i].init(conf.kp[i],conf.ki[i],conf.kd[i]*(float)abs(Stick[GAIN])*0.02
+ ,conf.PID_Limit,conf.Integral_Limit);
+ Angle[ROL]=Angle[PIT]=Angle[YAW]=0;
+ loop_cnt = 0;
+ FlyghtTime.start();
+ CycleTime.start();
+ FlashLED(5);
+}
+
+void LCD_locate(int clm,int row)
+{
+ lcd.locate(clm,row);
+ i2c.locate(clm,row);
+}
+
+void LCD_cls()
+{
+ lcd.cls();
+ i2c.cls();
+}
+
+void LCD_printf(char* str)
+{
+ lcd.printf(str);
+ i2c.printf(str);
+}
+;
+
+
--- a/mbed.bld Tue Jul 16 06:37:28 2013 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://mbed.org/users/mbed_official/code/mbed/builds/5e5da4a5990b \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.lib Fri Nov 15 20:53:36 2013 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/#f37f3b9c9f0b