Panusorn Chinsakuljaroen
/
BTL432kc
imu for l432kc
Revision 2:01ca44dd3908, committed 2018-12-04
- Comitter:
- sunninety1
- Date:
- Tue Dec 04 20:24:04 2018 +0000
- Parent:
- 1:f8ed70330216
- Child:
- 3:4a1bc31c360f
- Commit message:
- kk
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MPU6050.cpp Tue Dec 04 20:24:04 2018 +0000 @@ -0,0 +1,275 @@ +/** + * Includes + */ +#include "MPU6050.h" + +MPU6050::MPU6050(PinName sda, PinName scl) : connection(sda, scl) { + this->setSleepMode(false); + + //Initializations: + currentGyroRange = 0; + currentAcceleroRange=0; + MPU6050_ADDRESS=0x68; + //connection.frequency(400000); //mesures ne fonctionnent pas + // test I2C connexion and update MPU6050_ADDRESS + isFindMPU6050=I2C_finder(); + // if MPU6050 not found stop the program !!!!! + if(!isFindMPU6050) + { + DigitalOut myled(LED1); + printf("MPU6050 doesn't exist\n\r"); + printf("Check Connexion SCL and SDA and Power supply of your sensor\n\r"); + while(1) { + myled = 1; + wait(0.2); + myled = 0; + wait(0.2); + } + } +} + +//-------------------------------------------------- +//-------------------General------------------------ +//-------------------------------------------------- + +bool MPU6050::I2C_finder(){ + int i,isOK=1; + char data; + bool isFindSlave=false; + printf("test I2C\n\r"); + for(i=1;i<127;i++) + { wait(0.001); + isOK=connection.read(i*2,&data,1,false); + if(isOK==0){ + printf("slave I2C find : 0X%X\n\r",i); + if(i==MPU6050_ADDRESS || i==MPU6050_ADDRESS+1){ + printf("MPU6050 has been found : 0x%X\n\r",i); + MPU6050_ADDRESS=i; // on met l'adresse du MPU6050 = 0x68 ou 0x69 + isFindSlave=true; + } + } + } + return isFindSlave; +} + +void MPU6050::write(char address, char data) { + char temp[2]; + temp[0]=address; + temp[1]=data; + + connection.write(MPU6050_ADDRESS * 2,temp,2); +} + +char MPU6050::read(char address) { + char retval; + connection.write(MPU6050_ADDRESS * 2, &address, 1, true); + connection.read(MPU6050_ADDRESS * 2, &retval, 1); + return retval; +} + +void MPU6050::read(char address, char *data, int length) { + connection.write(MPU6050_ADDRESS * 2, &address, 1, true); + connection.read(MPU6050_ADDRESS * 2, data, length); +} + +void MPU6050::setSleepMode(bool state) { + char temp; + temp = this->read(MPU6050_PWR_MGMT_1_REG); + if (state == true) + temp |= 1<<MPU6050_SLP_BIT; + if (state == false) + temp &= ~(1<<MPU6050_SLP_BIT); + this->write(MPU6050_PWR_MGMT_1_REG, temp); +} + +bool MPU6050::testConnection( void ) { + char temp=0; + temp = this->read(MPU6050_WHO_AM_I_REG); + printf(" WHO AM I : 0X%X\n\r",temp); + return (temp == (MPU6050_ADDRESS & 0xFE)); +} + +void MPU6050::setBW(char BW) { + char temp; + BW=BW & 0x07; + temp = this->read(MPU6050_CONFIG_REG); + temp &= 0xF8; + temp = temp + BW; + this->write(MPU6050_CONFIG_REG, temp); +} + +void MPU6050::setI2CBypass(bool state) { + char temp; + temp = this->read(MPU6050_INT_PIN_CFG); + if (state == true) + temp |= 1<<MPU6050_BYPASS_BIT; + if (state == false) + temp &= ~(1<<MPU6050_BYPASS_BIT); + this->write(MPU6050_INT_PIN_CFG, temp); +} + +//-------------------------------------------------- +//----------------Accelerometer--------------------- +//-------------------------------------------------- + +void MPU6050::setAcceleroRange( char range ) { + char temp; + range = range & 0x03; + currentAcceleroRange = range; + + temp = this->read(MPU6050_ACCELERO_CONFIG_REG); + temp &= ~(3<<3); + temp = temp + (range<<3); + this->write(MPU6050_ACCELERO_CONFIG_REG, temp); +} + +int MPU6050::getAcceleroRawX( void ) { + short retval; + char data[2]; + this->read(MPU6050_ACCEL_XOUT_H_REG, data, 2); + retval = (data[0]<<8) + data[1]; + return (int)retval; +} + +int MPU6050::getAcceleroRawY( void ) { + short retval; + char data[2]; + this->read(MPU6050_ACCEL_YOUT_H_REG, data, 2); + retval = (data[0]<<8) + data[1]; + return (int)retval; +} + +int MPU6050::getAcceleroRawZ( void ) { + short retval; + char data[2]; + this->read(MPU6050_ACCEL_ZOUT_H_REG, data, 2); + retval = (data[0]<<8) + data[1]; + return (int)retval; +} + +void MPU6050::getAcceleroRaw( int *data ) { + char temp[6]; + this->read(MPU6050_ACCEL_XOUT_H_REG, temp, 6); + data[0] = (int)(short)((temp[0]<<8) + temp[1]); + data[1] = (int)(short)((temp[2]<<8) + temp[3]); + data[2] = (int)(short)((temp[4]<<8) + temp[5]); +} + +void MPU6050::getAccelero( float *data ) { + int temp[3]; + this->getAcceleroRaw(temp); + if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_2G) { + data[0]=(float)temp[0] / 16384.0 * 9.81; + data[1]=(float)temp[1] / 16384.0 * 9.81; + data[2]=(float)temp[2] / 16384.0 * 9.81; + } + if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_4G){ + data[0]=(float)temp[0] / 8192.0 * 9.81; + data[1]=(float)temp[1] / 8192.0 * 9.81; + data[2]=(float)temp[2] / 8192.0 * 9.81; + } + if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_8G){ + data[0]=(float)temp[0] / 4096.0 * 9.81; + data[1]=(float)temp[1] / 4096.0 * 9.81; + data[2]=(float)temp[2] / 4096.0 * 9.81; + } + if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_16G){ + data[0]=(float)temp[0] / 2048.0 * 9.81; + data[1]=(float)temp[1] / 2048.0 * 9.81; + data[2]=(float)temp[2] / 2048.0 * 9.81; + } + + #ifdef DOUBLE_ACCELERO + data[0]*=2; + data[1]*=2; + data[2]*=2; + #endif +} + +//-------------------------------------------------- +//------------------Gyroscope----------------------- +//-------------------------------------------------- +void MPU6050::setGyroRange( char range ) { + char temp; + currentGyroRange = range; + range = range & 0x03; + temp = this->read(MPU6050_GYRO_CONFIG_REG); + temp &= ~(3<<3); + temp = temp + range<<3; + this->write(MPU6050_GYRO_CONFIG_REG, temp); +} + +int MPU6050::getGyroRawX( void ) { + short retval; + char data[2]; + this->read(MPU6050_GYRO_XOUT_H_REG, data, 2); + retval = (data[0]<<8) + data[1]; + return (int)retval; +} + +int MPU6050::getGyroRawY( void ) { + short retval; + char data[2]; + this->read(MPU6050_GYRO_YOUT_H_REG, data, 2); + retval = (data[0]<<8) + data[1]; + return (int)retval; +} + +int MPU6050::getGyroRawZ( void ) { + short retval; + char data[2]; + this->read(MPU6050_GYRO_ZOUT_H_REG, data, 2); + retval = (data[0]<<8) + data[1]; + return (int)retval; +} + +void MPU6050::getGyroRaw( int *data ) { + char temp[6]; + this->read(MPU6050_GYRO_XOUT_H_REG, temp, 6); + data[0] = (int)(short)((temp[0]<<8) + temp[1]); + data[1] = (int)(short)((temp[2]<<8) + temp[3]); + data[2] = (int)(short)((temp[4]<<8) + temp[5]); +} + +void MPU6050::getGyro( float *data ) { + int temp[3]; + this->getGyroRaw(temp); + if (currentGyroRange == MPU6050_GYRO_RANGE_250) { + data[0]=(float)temp[0] / 7505.7; + data[1]=(float)temp[1] / 7505.7; + data[2]=(float)temp[2] / 7505.7; + } + if (currentGyroRange == MPU6050_GYRO_RANGE_500){ + data[0]=(float)temp[0] / 3752.9; + data[1]=(float)temp[1] / 3752.9; + data[2]=(float)temp[2] / 3752.9; + } + if (currentGyroRange == MPU6050_GYRO_RANGE_1000){ + data[0]=(float)temp[0] / 1879.3;; + data[1]=(float)temp[1] / 1879.3; + data[2]=(float)temp[2] / 1879.3; + } + if (currentGyroRange == MPU6050_GYRO_RANGE_2000){ + data[0]=(float)temp[0] / 939.7; + data[1]=(float)temp[1] / 939.7; + data[2]=(float)temp[2] / 939.7; + } +} +//-------------------------------------------------- +//-------------------Temperature-------------------- +//-------------------------------------------------- +int MPU6050::getTempRaw( void ) { + short retval; + char data[2]; + this->read(MPU6050_TEMP_H_REG, data, 2); + retval = (data[0]<<8) + data[1]; + return (int)retval; +} + +float MPU6050::getTemp( void ) { + float retval; + retval=(float)this->getTempRaw(); + retval=(retval+521.0)/340.0+35.0; + return retval; +} +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MPU6050.h Tue Dec 04 20:24:04 2018 +0000 @@ -0,0 +1,282 @@ +/*Use #define MPU6050_ES before you include this file if you have an engineering sample (older EVBs will have them), to find out if you have one, check your accelerometer output. +If it is half of what you expected, and you still are on the correct planet, you got an engineering sample +*/ + + +#ifndef MPU6050_H +#define MPU6050_H + +/** + * Includes + */ +#include "mbed.h" + + +/** + * Defines */ +//modif prog by salvat +#ifndef _MPU6050_ADDRESS + #define _MPU6050_ADDRESS + + +#endif + +#ifdef MPU6050_ES + #define DOUBLE_ACCELERO +#endif + +/** + * Registers + */ + #define MPU6050_CONFIG_REG 0x1A + #define MPU6050_GYRO_CONFIG_REG 0x1B + #define MPU6050_ACCELERO_CONFIG_REG 0x1C + + #define MPU6050_INT_PIN_CFG 0x37 + + #define MPU6050_ACCEL_XOUT_H_REG 0x3B + #define MPU6050_ACCEL_YOUT_H_REG 0x3D + #define MPU6050_ACCEL_ZOUT_H_REG 0x3F + + #define MPU6050_TEMP_H_REG 0x41 + + #define MPU6050_GYRO_XOUT_H_REG 0x43 + #define MPU6050_GYRO_YOUT_H_REG 0x45 + #define MPU6050_GYRO_ZOUT_H_REG 0x47 + + + + #define MPU6050_PWR_MGMT_1_REG 0x6B + #define MPU6050_WHO_AM_I_REG 0x75 + + + + /** + * Definitions + */ +#define MPU6050_SLP_BIT 6 +#define MPU6050_BYPASS_BIT 1 + +#define MPU6050_BW_256 0 +#define MPU6050_BW_188 1 +#define MPU6050_BW_98 2 +#define MPU6050_BW_42 3 +#define MPU6050_BW_20 4 +#define MPU6050_BW_10 5 +#define MPU6050_BW_5 6 + +#define MPU6050_ACCELERO_RANGE_2G 0 +#define MPU6050_ACCELERO_RANGE_4G 1 +#define MPU6050_ACCELERO_RANGE_8G 2 +#define MPU6050_ACCELERO_RANGE_16G 3 + +#define MPU6050_GYRO_RANGE_250 0 +#define MPU6050_GYRO_RANGE_500 1 +#define MPU6050_GYRO_RANGE_1000 2 +#define MPU6050_GYRO_RANGE_2000 3 + + +/** MPU6050 IMU library. + * + * Example: + * @code + * Later, maybe + * @endcode + */ +class MPU6050 { + public: + /** + * Constructor. + * + * Sleep mode of MPU6050 is immediatly disabled + * + * @param sda - mbed pin to use for the SDA I2C line. + * @param scl - mbed pin to use for the SCL I2C line. + */ + MPU6050(PinName sda, PinName scl); + + + /** + * Tests the I2C connection by reading the WHO_AM_I register. + * + * @return True for a working connection, false for an error + */ + bool testConnection( void ); + + /** + * Sets the bandwidth of the digital low-pass filter + * + * Macros: MPU6050_BW_256 - MPU6050_BW_188 - MPU6050_BW_98 - MPU6050_BW_42 - MPU6050_BW_20 - MPU6050_BW_10 - MPU6050_BW_5 + * Last number is the gyro's BW in Hz (accelero BW is virtually identical) + * + * @param BW - The three bits that set the bandwidth (use the predefined macros) + */ + void setBW( char BW ); + + /** + * Sets the auxiliary I2C bus in bypass mode to read the sensors behind the MPU6050 (useful for eval board, otherwise just connect them to primary I2C bus) + * + * @param state - Enables/disables the I2C bypass mode + */ + void setI2CBypass ( bool state ); + + /** + * Sets the Accelero full-scale range + * + * Macros: MPU6050_ACCELERO_RANGE_2G - MPU6050_ACCELERO_RANGE_4G - MPU6050_ACCELERO_RANGE_8G - MPU6050_ACCELERO_RANGE_16G + * + * @param range - The two bits that set the full-scale range (use the predefined macros) + */ + void setAcceleroRange(char range); + + /** + * Reads the accelero x-axis. + * + * @return 16-bit signed integer x-axis accelero data + */ + int getAcceleroRawX( void ); + + /** + * Reads the accelero y-axis. + * + * @return 16-bit signed integer y-axis accelero data + */ + int getAcceleroRawY( void ); + + /** + * Reads the accelero z-axis. + * + * @return 16-bit signed integer z-axis accelero data + */ + int getAcceleroRawZ( void ); + + /** + * Reads all accelero data. + * + * @param data - pointer to signed integer array with length three: data[0] = X, data[1] = Y, data[2] = Z + */ + void getAcceleroRaw( int *data ); + + /** + * Reads all accelero data, gives the acceleration in m/s2 + * + * Function uses the last setup value of the full scale range, if you manually set in another range, this won't work. + * + * @param data - pointer to float array with length three: data[0] = X, data[1] = Y, data[2] = Z + */ + void getAccelero( float *data ); + + /** + * Sets the Gyro full-scale range + * + * Macros: MPU6050_GYRO_RANGE_250 - MPU6050_GYRO_RANGE_500 - MPU6050_GYRO_RANGE_1000 - MPU6050_GYRO_RANGE_2000 + * + * @param range - The two bits that set the full-scale range (use the predefined macros) + */ + void setGyroRange(char range); + + /** + * Reads the gyro x-axis. + * + * @return 16-bit signed integer x-axis gyro data + */ + int getGyroRawX( void ); + + /** + * Reads the gyro y-axis. + * + * @return 16-bit signed integer y-axis gyro data + */ + int getGyroRawY( void ); + + /** + * Reads the gyro z-axis. + * + * @return 16-bit signed integer z-axis gyro data + */ + int getGyroRawZ( void ); + + /** + * Reads all gyro data. + * + * @param data - pointer to signed integer array with length three: data[0] = X, data[1] = Y, data[2] = Z + */ + void getGyroRaw( int *data ); + + /** + * Reads all gyro data, gives the gyro in rad/s + * + * Function uses the last setup value of the full scale range, if you manually set in another range, this won't work. + * + * @param data - pointer to float array with length three: data[0] = X, data[1] = Y, data[2] = Z + */ + void getGyro( float *data); + + /** + * Reads temperature data. + * + * @return 16 bit signed integer with the raw temperature register value + */ + int getTempRaw( void ); + + /** + * Returns current temperature + * + * @returns float with the current temperature + */ + float getTemp( void ); + + /** + * Sets the sleep mode of the MPU6050 + * + * @param state - true for sleeping, false for wake up + */ + void setSleepMode( bool state ); + + + /** + * Writes data to the device, could be private, but public is handy so you can transmit directly to the MPU. + * + * @param adress - register address to write to + * @param data - data to write + */ + void write( char address, char data); + + /** + * Read data from the device, could be private, but public is handy so you can transmit directly to the MPU. + * + * @param adress - register address to write to + * @return - data from the register specified by RA + */ + char read( char adress); + + /** + * Read multtiple regigsters from the device, more efficient than using multiple normal reads. + * + * @param adress - register address to write to + * @param length - number of bytes to read + * @param data - pointer where the data needs to be written to + */ + void read( char adress, char *data, int length); + + /** + * Find all I2C devices and print all devices I2C + update MPU6050_ADDRESS (choice between 0x68 and 0X69) + * + */ + bool I2C_finder(); + + private: + I2C connection; + char currentAcceleroRange; + char currentGyroRange; +// add by salvat : permet de trouver l'adresse du MPU6050 dans testConnexion + char MPU6050_ADDRESS;// address pin low (GND), default for InvenSense evaluation board + bool isFindMPU6050; + + + +}; + + + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Tue Dec 04 20:24:04 2018 +0000 @@ -0,0 +1,150 @@ + + +#include "mbed.h" +#include "MPU6050.h" +#include "math.h" +MPU6050 mpu(D4,D5); +Serial pc(USBTX, USBRX); // tx, rx +Timer t; +Serial serial(PA_9,PA_10); +char data[30]; + +//DigitalIn res(USER_BUTTON); + +float ark[3]; + float gy[3]; + + int timestamp=0; + float Now,lastupdate; +float cali_ax,cali_ay,cali_az,cali_gx,cali_gy,cali_gz; + static float v[3]={0,0,0}; + +float PI = 3.14159265358979323846f; +void MPUcalibate(){ + static float axx=0,ayy=0,azz=0,gxx=0,gyy=0,gzz=0; + int x=0; + while(x<=9999) { + mpu.getAccelero(ark); + mpu.getGyro(gy); + axx+=ark[0]; + ayy+=ark[1]; + azz+=ark[2]; + gxx+=gy[0]; + gyy+=gy[1]; + gzz+=gy[2]; + x+=1; + wait(0.0001); + } + cali_ax=axx/10000.0f; + cali_ay=ayy/10000.0f; + cali_az=azz/10000.0f; + cali_gx=gxx/10000.0f; + cali_gy=gyy/10000.0f; + cali_gz=gzz/10000.0f; + //pc.printf("%.10f %.10f %.10f\n",cali_ax,cali_ay,cali_az-9.80665); + //pc.printf("%.10f %.10f %.10f\n",cali_ax-9.80665,cali_ay,cali_az); + //pc.printf("%.10f %.10f %.10f\n",cali_ax,cali_ay-9.80665,cali_az); + //pc.printf("%f %f %f\n",cali_gx,cali_gy,cali_gz); + } +void AccelToVelo(float ax,float ay,float az,float dt){ + v[0]+=(ax*dt/129); //(-156.10655212402344+150.74868774414062); + v[1]+=(ay*dt/180); //(-208.04095458984375+194.97883605957031); + v[2]+=(az*dt/6); //(151.52879333496094-144.20004272460938); + } +/*float CalculateAngle(float ax,float ay,float az,float gx,float gy,float gz){ + float pitch; + gx=0; + gy=0; + gz=0; + static float fillter_pitch=0; + fillter_pitch=(fillter_pitch*0.8)+(pitch*0.2); + pitch=atan((ax)/sqrt((ay)*(ay)+(az)*(az))*(180/PI)); + //pc.printf("%f\n",pitch); + return fillter_pitch; + }*/ +int main() +{ + pc.baud(115200); + static float calibate_acc[3]={0.6465099045, 0.14739338841, -0.2634094528}; + static float calibate_gy[3]={-0.022125, 0.005254 ,-0.009761}; + mpu.getAccelero(ark); + mpu.getGyro(gy); + //CalculateAngle(ark[0],ark[1],ark[2],0,0,0); + static float fillter_pitch=0; + int co=0; + float pitch; + float delay=0.0001,current=0.0,deltat; + t.start(); + + wait(0.5); + + //MPUcalibate(); + //cali_err=atan((-(ark[1]-cali_ay))/(ark[2]-cali_az)); + //cali_p=atan2(((ark[2]-cali_az)),(ark[0]-cali_ax)); + //cali_p=atan((ark[0]-cali_ax)/sqrt((ark[1]-cali_ay)*(ark[1]-cali_ay)+(ark[2]-cali_az)*(ark[2]-cali_az))); + //pc.printf("cali P %f \n",cali_err); + //wait(1); + /* fillter_pitch2=(fillter_pitch2*0.75)+(fillter_pitch*0.25); + fillter_pitch3=(fillter_pitch3*0.75)+(fillter_pitch2*0.25); + fillter_pitch4=(fillter_pitch4*0.75)+(fillter_pitch3*0.25); + fillter_pitch5=(fillter_pitch5*0.75)+(fillter_pitch4*0.25); + fillter_pitch6=(fillter_pitch6*0.75)+(fillter_pitch5*0.25); + fillter_pitch7=(fillter_pitch7*0.75)+(fillter_pitch6*0.25); + fillter_pitch8=(fillter_pitch8*0.75)+(fillter_pitch7*0.25); + fillter_pitch9=(fillter_pitch9*0.75)+(fillter_pitch8*0.25); + fillter_pitch10=(fillter_pitch10*0.75)+(fillter_pitch9*0.25);*/ + while(1) { + Now = t.read(); + deltat=Now-current; + if (deltat>0.1){ + mpu.getAccelero(ark); + mpu.getGyro(gy); + ark[0] -= calibate_acc[0]; + ark[1] -= calibate_acc[1]; + ark[2] -= calibate_acc[2]; + gy[0] -= calibate_gy[0]; + gy[1] -= calibate_gy[1]; + gy[2] -= calibate_gy[2]; + float g=sqrt((ark[0]*ark[0])+(ark[1]*ark[1])+(ark[2]*ark[2])); + while(co<1000){ + AccelToVelo((ark[0]),ark[1],ark[2]-9.8065,deltat); + co+=1; + } + co=0; + ark[0]+=0.5; + float gra=sqrt((ark[0]*ark[0])+(ark[1]*ark[1])+(ark[2]*ark[2])); + pc.printf("Q AX %f AY %f AZ %f G %f \n",ark[0],ark[1],ark[2],gra); + if(serial.writeable()) { + serial.printf("%f %f %f %f \n",ark[0],ark[1],ark[2],gra); + //serial.printf("------------------------ \n"); + //serial.printf(" Gyx:%f_Gyy:%f_Gyz:%f \n",gy[0],gy[1],gy[2]); + wait_ms(500); // lag to debounce the button + } + //pc.printf("Q GyX %f GyY %f GyZ %f \n",gy[0],gy[1],gy[2]); + //pc.printf("Q Vx %f Vy %f Vz %f \n",v[0],v[1],v[2]); + //pc.printf("%.14f\n",v[1]/100); + pc.printf("Q AX %f AY %f AZ %f GyX %f GyY %f GyZ %f \n",ark[0]+0.5,ark[1],ark[2],gy[0],gy[1],gy[2]); + //timestamp+=1; + //pc.printf("%f\n",atan((-(ark[1]-cali_ay))/(ark[2]-cali_az))); + //pc.printf("Ax %f\n",(atan2((ark[2]-cali_az),(ark[0]-cali_ax))-cali_p)*(180/PI)); + //pitch=atan((ark[0])/sqrt((ark[1])*(ark[1])+(ark[2])*ark[2]))*(180/PI); + //CalculateAngle(ark[0],ark[1],ark[2],0,0,0); + //pc.printf("%lf\n",CalculateAngle(ark[0],ark[1],ark[2],0,0,0)); + pitch=atan(ark[0]/sqrt(pow(ark[1],2)+pow(ark[2],2)))*(180/PI); + //fillter_pitch=(fillter_pitch*0.75)+(pitch*0.25); + // fillter_pitch2=(fillter_pitch2*0.75)+(fillter_pitch*0.25); + // pc.printf("%f\n",fillter_pitch); + //pc.printf("Ax %f\n",(atan((ark[0])/sqrt((ark[1]-cali_ay)*(ark[1]-cali_ay)+(ark[2]-cali_az)*(ark[2]-cali_az)))-cali_p)*(180/PI)); + // pc.printf("%.10f %.10f %.10f\n",ark[0],ark[1],ark[2]); + //pc.printf("%f %f %f\n",gy[0],gy[1],gy[2]); + //pc.printf("%.2f\n",t.read()); + wait(0.02); + /*if (res==0) { + //lastupdate=t.read()-Now; + //Now=lastupdate; + t.reset(); + }*/ +current=Now; +} + } +}
--- a/test.cpp Tue Dec 04 04:38:15 2018 +0000 +++ b/test.cpp Tue Dec 04 20:24:04 2018 +0000 @@ -1,5 +1,5 @@ - +/* #include "mbed.h" Serial serial(D8, D2); @@ -17,7 +17,7 @@ while(1) {} } - +*/ /* #include "mbed.h"