hige dura
RK4_euler
Dependencies: FatFileSystem mbed
Fork of
RK4_euler
by 
hige dura
Revision 7:ec00db826804, committed 2012-11-29
- Comitter:
- higedura
- Date:
- Thu Nov 29 15:22:06 2012 +0000
- Parent:
- 6:07f4aaae5339
- Commit message:
- RK4_euler
Changed in this revision
--- a/ADXL345_I2C.cpp Thu Sep 20 13:00:24 2012 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,421 +0,0 @@
-/**
- * @author Peter Swanson
- * A personal note from me: Jesus Christ has changed my life so much it blows my mind. I say this because
- * today, religion is thought of as something that you do or believe and has about as
- * little impact on a person as their political stance. But for me, God gives me daily
- * strength and has filled my life with the satisfaction that I could never find in any
- * of the other things that I once looked for it in.
- * If your interested, heres verse that changed my life:
- * Rom 8:1-3: "Therefore, there is now no condemnation for those who are in Christ Jesus,
- * because through Christ Jesus, the law of the Spirit who gives life has set
- * me free from the law of sin (which brings...) and death. For what the law
- * was powerless to do in that it was weakened by the flesh, God did by sending
- * His own Son in the likeness of sinful flesh to be a sin offering. And so He
- * condemned sin in the flesh in order that the righteous requirements of the
- * (God's) law might be fully met in us, who live not according to the flesh
- * but according to the Spirit."
- *
- * @section LICENSE
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- * THE SOFTWARE.
- *
- * @section DESCRIPTION
- *
- * ADXL345, triple axis, I2C interface, accelerometer.
- *
- * Datasheet:
- *
- * http://www.analog.com/static/imported-files/data_sheets/ADXL345.pdf
- */
-
-/**
- * Includes
- */
-#include "ADXL345_I2C.h"
-
-//#include "mbed.h"
-
-ADXL345_I2C::ADXL345_I2C(PinName sda, PinName scl) : i2c_(sda, scl) {
-
- //400kHz, allowing us to use the fastest data rates.
- i2c_.frequency(400000);
-// initialize the BW data rate
- char tx[2];
- tx[0] = ADXL345_BW_RATE_REG;
- tx[1] = ADXL345_1600HZ; //value greater than or equal to 0x0A is written into the rate bits (Bit D3 through Bit D0) in the BW_RATE register
- i2c_.write( ADXL345_I2C_WRITE , tx, 2);
-
-//Data format (for +-16g) - This is done by setting Bit D3 of the DATA_FORMAT register (Address 0x31) and writing a value of 0x03 to the range bits (Bit D1 and Bit D0) of the DATA_FORMAT register (Address 0x31).
-
- char rx[2];
- rx[0] = ADXL345_DATA_FORMAT_REG;
- rx[1] = 0x0B;
- // full res and +_16g
- i2c_.write( ADXL345_I2C_WRITE , rx, 2);
-
- // Set Offset - programmed into the OFSX, OFSY, and OFXZ registers, respectively, as 0xFD, 0x03 and 0xFE.
- char x[2];
- x[0] = ADXL345_OFSX_REG ;
- x[1] = 0xFD;
- i2c_.write( ADXL345_I2C_WRITE , x, 2);
- char y[2];
- y[0] = ADXL345_OFSY_REG ;
- y[1] = 0x03;
- i2c_.write( ADXL345_I2C_WRITE , y, 2);
- char z[2];
- z[0] = ADXL345_OFSZ_REG ;
- z[1] = 0xFE;
- i2c_.write( ADXL345_I2C_WRITE , z, 2);
-}
-
-
-char ADXL345_I2C::SingleByteRead(char address){
- char tx = address;
- char output;
- i2c_.write( ADXL345_I2C_WRITE , &tx, 1); //tell it what you want to read
- i2c_.read( ADXL345_I2C_READ , &output, 1); //tell it where to store the data
- return output;
-
-}
-
-
-/*
-***info on the i2c_.write***
-address 8-bit I2C slave address [ addr | 0 ]
-data Pointer to the byte-array data to send
-length Number of bytes to send
-repeated Repeated start, true - do not send stop at end
-returns 0 on success (ack), or non-0 on failure (nack)
-*/
-
-int ADXL345_I2C::SingleByteWrite(char address, char data){
- int ack = 0;
- char tx[2];
- tx[0] = address;
- tx[1] = data;
- return ack | i2c_.write( ADXL345_I2C_WRITE , tx, 2);
-}
-
-
-
-void ADXL345_I2C::multiByteRead(char address, char* output, int size) {
- i2c_.write( ADXL345_I2C_WRITE, &address, 1); //tell it where to read from
- i2c_.read( ADXL345_I2C_READ , output, size); //tell it where to store the data read
-}
-
-
-int ADXL345_I2C::multiByteWrite(char address, char* ptr_data, int size) {
- int ack;
-
- ack = i2c_.write( ADXL345_I2C_WRITE, &address, 1); //tell it where to write to
- return ack | i2c_.write( ADXL345_I2C_READ, ptr_data, size); //tell it what data to write
-
-}
-
-
-void ADXL345_I2C::getOutput(int* readings){
- char buffer[6];
- multiByteRead(ADXL345_DATAX0_REG, buffer, 6);
-
- readings[0] = (int)buffer[1] << 8 | (int)buffer[0];
- readings[1] = (int)buffer[3] << 8 | (int)buffer[2];
- readings[2] = (int)buffer[5] << 8 | (int)buffer[4];
-
-}
-
-
-
-char ADXL345_I2C::getDeviceID() {
- return SingleByteRead(ADXL345_DEVID_REG);
- }
-//
-int ADXL345_I2C::setPowerMode(char mode) {
-
- //Get the current register contents, so we don't clobber the rate value.
- char registerContents = (mode << 4) | SingleByteRead(ADXL345_BW_RATE_REG);
-
- return SingleByteWrite(ADXL345_BW_RATE_REG, registerContents);
-
-}
-
-char ADXL345_I2C::getPowerControl() {
- return SingleByteRead(ADXL345_POWER_CTL_REG);
-}
-
-int ADXL345_I2C::setPowerControl(char settings) {
- return SingleByteWrite(ADXL345_POWER_CTL_REG, settings);
-
-}
-
-
-
-char ADXL345_I2C::getDataFormatControl(void){
-
- return SingleByteRead(ADXL345_DATA_FORMAT_REG);
-}
-
-int ADXL345_I2C::setDataFormatControl(char settings){
-
- return SingleByteWrite(ADXL345_DATA_FORMAT_REG, settings);
-
-}
-
-int ADXL345_I2C::setDataRate(char rate) {
-
- //Get the current register contents, so we don't clobber the power bit.
- char registerContents = SingleByteRead(ADXL345_BW_RATE_REG);
-
- registerContents &= 0x10;
- registerContents |= rate;
-
- return SingleByteWrite(ADXL345_BW_RATE_REG, registerContents);
-
-}
-
-
-char ADXL345_I2C::getOffset(char axis) {
-
- char address = 0;
-
- if (axis == ADXL345_X) {
- address = ADXL345_OFSX_REG;
- } else if (axis == ADXL345_Y) {
- address = ADXL345_OFSY_REG;
- } else if (axis == ADXL345_Z) {
- address = ADXL345_OFSZ_REG;
- }
-
- return SingleByteRead(address);
-}
-
-int ADXL345_I2C::setOffset(char axis, char offset) {
-
- char address = 0;
-
- if (axis == ADXL345_X) {
- address = ADXL345_OFSX_REG;
- } else if (axis == ADXL345_Y) {
- address = ADXL345_OFSY_REG;
- } else if (axis == ADXL345_Z) {
- address = ADXL345_OFSZ_REG;
- }
-
- return SingleByteWrite(address, offset);
-
-}
-
-
-char ADXL345_I2C::getFifoControl(void){
-
- return SingleByteRead(ADXL345_FIFO_CTL);
-
-}
-
-int ADXL345_I2C::setFifoControl(char settings){
- return SingleByteWrite(ADXL345_FIFO_STATUS, settings);
-
-}
-
-char ADXL345_I2C::getFifoStatus(void){
-
- return SingleByteRead(ADXL345_FIFO_STATUS);
-
-}
-
-
-
-char ADXL345_I2C::getTapThreshold(void) {
-
- return SingleByteRead(ADXL345_THRESH_TAP_REG);
-}
-
-int ADXL345_I2C::setTapThreshold(char threshold) {
-
- return SingleByteWrite(ADXL345_THRESH_TAP_REG, threshold);
-
-}
-
-
-float ADXL345_I2C::getTapDuration(void) {
-
- return (float)SingleByteRead(ADXL345_DUR_REG)*625;
-}
-
-int ADXL345_I2C::setTapDuration(short int duration_us) {
-
- short int tapDuration = duration_us / 625;
- char tapChar[2];
- tapChar[0] = (tapDuration & 0x00FF);
- tapChar[1] = (tapDuration >> 8) & 0x00FF;
- return multiByteWrite(ADXL345_DUR_REG, tapChar, 2);
-
-}
-
-float ADXL345_I2C::getTapLatency(void) {
-
- return (float)SingleByteRead(ADXL345_LATENT_REG)*1.25;
-}
-
-int ADXL345_I2C::setTapLatency(short int latency_ms) {
-
- latency_ms = latency_ms / 1.25;
- char latChar[2];
- latChar[0] = (latency_ms & 0x00FF);
- latChar[1] = (latency_ms << 8) & 0xFF00;
- return multiByteWrite(ADXL345_LATENT_REG, latChar, 2);
-
-}
-
-float ADXL345_I2C::getWindowTime(void) {
-
- return (float)SingleByteRead(ADXL345_WINDOW_REG)*1.25;
-}
-
-int ADXL345_I2C::setWindowTime(short int window_ms) {
-
- window_ms = window_ms / 1.25;
- char windowChar[2];
- windowChar[0] = (window_ms & 0x00FF);
- windowChar[1] = ((window_ms << 8) & 0xFF00);
- return multiByteWrite(ADXL345_WINDOW_REG, windowChar, 2);
-
-}
-
-char ADXL345_I2C::getActivityThreshold(void) {
-
- return SingleByteRead(ADXL345_THRESH_ACT_REG);
-}
-
-int ADXL345_I2C::setActivityThreshold(char threshold) {
- return SingleByteWrite(ADXL345_THRESH_ACT_REG, threshold);
-
-}
-
-char ADXL345_I2C::getInactivityThreshold(void) {
- return SingleByteRead(ADXL345_THRESH_INACT_REG);
-
-}
-
-//int FUNCTION(short int * ptr_Output)
-//short int FUNCTION ()
-
-int ADXL345_I2C::setInactivityThreshold(char threshold) {
- return SingleByteWrite(ADXL345_THRESH_INACT_REG, threshold);
-
-}
-
-char ADXL345_I2C::getTimeInactivity(void) {
-
- return SingleByteRead(ADXL345_TIME_INACT_REG);
-
-}
-
-int ADXL345_I2C::setTimeInactivity(char timeInactivity) {
- return SingleByteWrite(ADXL345_TIME_INACT_REG, timeInactivity);
-
-}
-
-char ADXL345_I2C::getActivityInactivityControl(void) {
-
- return SingleByteRead(ADXL345_ACT_INACT_CTL_REG);
-
-}
-
-int ADXL345_I2C::setActivityInactivityControl(char settings) {
- return SingleByteWrite(ADXL345_ACT_INACT_CTL_REG, settings);
-
-}
-
-char ADXL345_I2C::getFreefallThreshold(void) {
-
- return SingleByteRead(ADXL345_THRESH_FF_REG);
-
-}
-
-int ADXL345_I2C::setFreefallThreshold(char threshold) {
- return SingleByteWrite(ADXL345_THRESH_FF_REG, threshold);
-
-}
-
-char ADXL345_I2C::getFreefallTime(void) {
-
- return SingleByteRead(ADXL345_TIME_FF_REG)*5;
-
-}
-
-int ADXL345_I2C::setFreefallTime(short int freefallTime_ms) {
- freefallTime_ms = freefallTime_ms / 5;
- char fallChar[2];
- fallChar[0] = (freefallTime_ms & 0x00FF);
- fallChar[1] = (freefallTime_ms << 8) & 0xFF00;
-
- return multiByteWrite(ADXL345_TIME_FF_REG, fallChar, 2);
-
-}
-
-char ADXL345_I2C::getTapAxisControl(void) {
-
- return SingleByteRead(ADXL345_TAP_AXES_REG);
-
-}
-
-int ADXL345_I2C::setTapAxisControl(char settings) {
- return SingleByteWrite(ADXL345_TAP_AXES_REG, settings);
-
-}
-
-char ADXL345_I2C::getTapSource(void) {
-
- return SingleByteRead(ADXL345_ACT_TAP_STATUS_REG);
-
-}
-
-
-
-char ADXL345_I2C::getInterruptEnableControl(void) {
-
- return SingleByteRead(ADXL345_INT_ENABLE_REG);
-
-}
-
-int ADXL345_I2C::setInterruptEnableControl(char settings) {
- return SingleByteWrite(ADXL345_INT_ENABLE_REG, settings);
-
-}
-
-char ADXL345_I2C::getInterruptMappingControl(void) {
-
- return SingleByteRead(ADXL345_INT_MAP_REG);
-
-}
-
-int ADXL345_I2C::setInterruptMappingControl(char settings) {
- return SingleByteWrite(ADXL345_INT_MAP_REG, settings);
-
-}
-
-char ADXL345_I2C::getInterruptSource(void){
-
- return SingleByteRead(ADXL345_INT_SOURCE_REG);
-
-}
-
-
-
-
--- a/ADXL345_I2C.h Thu Sep 20 13:00:24 2012 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,575 +0,0 @@
-/**
- * @author Peter Swanson
- * A personal note from me: Jesus Christ has changed my life so much it blows my mind. I say this because
- * today, religion is thought of as something that you do or believe and has about as
- * little impact on a person as their political stance. But for me, God gives me daily
- * strength and has filled my life with the satisfaction that I could never find in any
- * of the other things that I once looked for it in.
- * If your interested, heres verse that changed my life:
- * Rom 8:1-3: "Therefore, there is now no condemnation for those who are in Christ Jesus,
- * because through Christ Jesus, the law of the Spirit who gives life has set
- * me free from the law of sin (which brings...) and death. For what the law
- * was powerless to do in that it was weakened by the flesh, God did by sending
- * His own Son in the likeness of sinful flesh to be a sin offering. And so He
- * condemned sin in the flesh in order that the righteous requirements of the
- * (God's) law might be fully met in us, who live not according to the flesh
- * but according to the Spirit."
- *
- * @section LICENSE
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- * THE SOFTWARE.
- *
- * @section DESCRIPTION
- *
- * ADXL345, triple axis, I2C interface, accelerometer.
- *
- * Datasheet:
- *
- * http://www.analog.com/static/imported-files/data_sheets/ADXL345.pdf
- */
-
-
-
-#ifndef ADXL345_I2C_H
-#define ADXL345_I2C_H
-
-/**
- * Includes
- */
-#include "mbed.h"
-
-/**
- * Defines
- */
-//Registers.
-#define ADXL345_DEVID_REG 0x00
-#define ADXL345_THRESH_TAP_REG 0x1D
-#define ADXL345_OFSX_REG 0x1E
-#define ADXL345_OFSY_REG 0x1F
-#define ADXL345_OFSZ_REG 0x20
-#define ADXL345_DUR_REG 0x21
-#define ADXL345_LATENT_REG 0x22
-#define ADXL345_WINDOW_REG 0x23
-#define ADXL345_THRESH_ACT_REG 0x24
-#define ADXL345_THRESH_INACT_REG 0x25
-#define ADXL345_TIME_INACT_REG 0x26
-#define ADXL345_ACT_INACT_CTL_REG 0x27
-#define ADXL345_THRESH_FF_REG 0x28
-#define ADXL345_TIME_FF_REG 0x29
-#define ADXL345_TAP_AXES_REG 0x2A
-#define ADXL345_ACT_TAP_STATUS_REG 0x2B
-#define ADXL345_BW_RATE_REG 0x2C
-#define ADXL345_POWER_CTL_REG 0x2D
-#define ADXL345_INT_ENABLE_REG 0x2E
-#define ADXL345_INT_MAP_REG 0x2F
-#define ADXL345_INT_SOURCE_REG 0x30
-#define ADXL345_DATA_FORMAT_REG 0x31
-#define ADXL345_DATAX0_REG 0x32
-#define ADXL345_DATAX1_REG 0x33
-#define ADXL345_DATAY0_REG 0x34
-#define ADXL345_DATAY1_REG 0x35
-#define ADXL345_DATAZ0_REG 0x36
-#define ADXL345_DATAZ1_REG 0x37
-#define ADXL345_FIFO_CTL 0x38
-#define ADXL345_FIFO_STATUS 0x39
-
-//Data rate codes.
-#define ADXL345_3200HZ 0x0F
-#define ADXL345_1600HZ 0x0E
-#define ADXL345_800HZ 0x0D
-#define ADXL345_400HZ 0x0C
-#define ADXL345_200HZ 0x0B
-#define ADXL345_100HZ 0x0A
-#define ADXL345_50HZ 0x09
-#define ADXL345_25HZ 0x08
-#define ADXL345_12HZ5 0x07
-#define ADXL345_6HZ25 0x06
-
-// read or write bytes
-#define ADXL345_I2C_READ 0xA7
-#define ADXL345_I2C_WRITE 0xA6
-#define ADXL345_I2C_ADDRESS 0x53 //the ADXL345 7-bit address is 0x53 when ALT ADDRESS is low as it is on the sparkfun chip: when ALT ADDRESS is high the address is 0x1D
-
-/////////////when ALT ADDRESS pin is high:
-//#define ADXL345_I2C_READ 0x3B
-//#define ADXL345_I2C_WRITE 0x3A
-//#define ADXL345_I2C_ADDRESS 0x1D
-
-#define ADXL345_X 0x00
-#define ADXL345_Y 0x01
-#define ADXL345_Z 0x02
-
-
-
-// modes
-#define MeasurementMode 0x08
-
-
-
-
-
-
-
-class ADXL345_I2C {
-
-public:
-
- /**
- * Constructor.
- *
- * @param mosi mbed pin to use for SDA line of I2C interface.
- * @param sck mbed pin to use for SCL line of I2C interface.
- */
- ADXL345_I2C(PinName sda, PinName scl);
-
- /**
- * Get the output of all three axes.
- *
- * @param Pointer to a buffer to hold the accelerometer value for the
- * x-axis, y-axis and z-axis [in that order].
- */
- void getOutput(int* readings);
-
- /**
- * Read the device ID register on the device.
- *
- * @return The device ID code [0xE5]
- */
- char getDeviceID(void);
-
-
-
- /**
- * Set the power mode.
- *
- * @param mode 0 -> Normal operation.
- * 1 -> Reduced power operation.
- */
-int setPowerMode(char mode);
-
- /**
- * Set the power control settings.
- *
- * See datasheet for details.
- *
- * @param The control byte to write to the POWER_CTL register.
- */
- int setPowerControl(char settings);
- /**
- * Get the power control settings.
- *
- * See datasheet for details.
- *
- * @return The contents of the POWER_CTL register.
- */
- char getPowerControl(void);
-
-
- /**
- * Get the data format settings.
- *
- * @return The contents of the DATA_FORMAT register.
- */
-
- char getDataFormatControl(void);
-
- /**
- * Set the data format settings.
- *
- * @param settings The control byte to write to the DATA_FORMAT register.
- */
- int setDataFormatControl(char settings);
-
- /**
- * Set the data rate.
- *
- * @param rate The rate code (see #defines or datasheet).
- */
- int setDataRate(char rate);
-
-
- /**
- * Get the current offset for a particular axis.
- *
- * @param axis 0x00 -> X-axis
- * 0x01 -> Y-axis
- * 0x02 -> Z-axis
- * @return The current offset as an 8-bit 2's complement number with scale
- * factor 15.6mg/LSB.
- */
-
- char getOffset(char axis);
-
- /**
- * Set the offset for a particular axis.
- *
- * @param axis 0x00 -> X-axis
- * 0x01 -> Y-axis
- * 0x02 -> Z-axis
- * @param offset The offset as an 8-bit 2's complement number with scale
- * factor 15.6mg/LSB.
- */
- int setOffset(char axis, char offset);
-
-
-
- /**
- * Get the FIFO control settings.
- *
- * @return The contents of the FIFO_CTL register.
- */
- char getFifoControl(void);
-
- /**
- * Set the FIFO control settings.
- *
- * @param The control byte to write to the FIFO_CTL register.
- */
- int setFifoControl(char settings);
-
- /**
- * Get FIFO status.
- *
- * @return The contents of the FIFO_STATUS register.
- */
- char getFifoStatus(void);
-
- /**
- * Read the tap threshold on the device.
- *
- * @return The tap threshold as an 8-bit number with a scale factor of
- * 62.5mg/LSB.
- */
- char getTapThreshold(void);
-
- /**
- * Set the tap threshold.
- *
- * @param The tap threshold as an 8-bit number with a scale factor of
- * 62.5mg/LSB.
- */
- int setTapThreshold(char threshold);
-
- /**
- * Get the tap duration required to trigger an event.
- *
- * @return The max time that an event must be above the tap threshold to
- * qualify as a tap event, in microseconds.
- */
- float getTapDuration(void);
-
- /**
- * Set the tap duration required to trigger an event.
- *
- * @param duration_us The max time that an event must be above the tap
- * threshold to qualify as a tap event, in microseconds.
- * Time will be normalized by the scale factor which is
- * 625us/LSB. A value of 0 disables the single/double
- * tap functions.
- */
- int setTapDuration(short int duration_us);
-
- /**
- * Get the tap latency between the detection of a tap and the time window.
- *
- * @return The wait time from the detection of a tap event to the start of
- * the time window during which a possible second tap event can be
- * detected in milliseconds.
- */
- float getTapLatency(void);
-
- /**
- * Set the tap latency between the detection of a tap and the time window.
- *
- * @param latency_ms The wait time from the detection of a tap event to the
- * start of the time window during which a possible
- * second tap event can be detected in milliseconds.
- * A value of 0 disables the double tap function.
- */
- int setTapLatency(short int latency_ms);
-
- /**
- * Get the time of window between tap latency and a double tap.
- *
- * @return The amount of time after the expiration of the latency time
- * during which a second valid tap can begin, in milliseconds.
- */
- float getWindowTime(void);
-
- /**
- * Set the time of the window between tap latency and a double tap.
- *
- * @param window_ms The amount of time after the expiration of the latency
- * time during which a second valid tap can begin,
- * in milliseconds.
- */
- int setWindowTime(short int window_ms);
-
- /**
- * Get the threshold value for detecting activity.
- *
- * @return The threshold value for detecting activity as an 8-bit number.
- * Scale factor is 62.5mg/LSB.
- */
- char getActivityThreshold(void);
-
- /**
- * Set the threshold value for detecting activity.
- *
- * @param threshold The threshold value for detecting activity as an 8-bit
- * number. Scale factor is 62.5mg/LSB. A value of 0 may
- * result in undesirable behavior if the activity
- * interrupt is enabled.
- */
- int setActivityThreshold(char threshold);
-
- /**
- * Get the threshold value for detecting inactivity.
- *
- * @return The threshold value for detecting inactivity as an 8-bit number.
- * Scale factor is 62.5mg/LSB.
- */
- char getInactivityThreshold(void);
-
- /**
- * Set the threshold value for detecting inactivity.
- *
- * @param threshold The threshold value for detecting inactivity as an
- * 8-bit number. Scale factor is 62.5mg/LSB.
- */
- int setInactivityThreshold(char threshold);
-
- /**
- * Get the time required for inactivity to be declared.
- *
- * @return The amount of time that acceleration must be less than the
- * inactivity threshold for inactivity to be declared, in
- * seconds.
- */
- char getTimeInactivity(void);
-
- /**
- * Set the time required for inactivity to be declared.
- *
- * @param inactivity The amount of time that acceleration must be less than
- * the inactivity threshold for inactivity to be
- * declared, in seconds. A value of 0 results in an
- * interrupt when the output data is less than the
- * threshold inactivity.
- */
- int setTimeInactivity(char timeInactivity);
-
- /**
- * Get the activity/inactivity control settings.
- *
- * D7 D6 D5 D4
- * +-----------+--------------+--------------+--------------+
- * | ACT ac/dc | ACT_X enable | ACT_Y enable | ACT_Z enable |
- * +-----------+--------------+--------------+--------------+
- *
- * D3 D2 D1 D0
- * +-------------+----------------+----------------+----------------+
- * | INACT ac/dc | INACT_X enable | INACT_Y enable | INACT_Z enable |
- * +-------------+----------------+----------------+----------------+
- *
- * See datasheet for details.
- *
- * @return The contents of the ACT_INACT_CTL register.
- */
- char getActivityInactivityControl(void);
-
- /**
- * Set the activity/inactivity control settings.
- *
- * D7 D6 D5 D4
- * +-----------+--------------+--------------+--------------+
- * | ACT ac/dc | ACT_X enable | ACT_Y enable | ACT_Z enable |
- * +-----------+--------------+--------------+--------------+
- *
- * D3 D2 D1 D0
- * +-------------+----------------+----------------+----------------+
- * | INACT ac/dc | INACT_X enable | INACT_Y enable | INACT_Z enable |
- * +-------------+----------------+----------------+----------------+
- *
- * See datasheet for details.
- *
- * @param settings The control byte to write to the ACT_INACT_CTL register.
- */
- int setActivityInactivityControl(char settings);
-
- /**
- * Get the threshold for free fall detection.
- *
- * @return The threshold value for free-fall detection, as an 8-bit number,
- * with scale factor 62.5mg/LSB.
- */
- char getFreefallThreshold(void);
-
- /**
- * Set the threshold for free fall detection.
- *
- * @return The threshold value for free-fall detection, as an 8-bit number,
- * with scale factor 62.5mg/LSB. A value of 0 may result in
- * undesirable behavior if the free-fall interrupt is enabled.
- * Values between 300 mg and 600 mg (0x05 to 0x09) are recommended.
- */
- int setFreefallThreshold(char threshold);
-
- /**
- * Get the time required to generate a free fall interrupt.
- *
- * @return The minimum time that the value of all axes must be less than
- * the freefall threshold to generate a free-fall interrupt, in
- * milliseconds.
- */
- char getFreefallTime(void);
-
- /**
- * Set the time required to generate a free fall interrupt.
- *
- * @return The minimum time that the value of all axes must be less than
- * the freefall threshold to generate a free-fall interrupt, in
- * milliseconds. A value of 0 may result in undesirable behavior
- * if the free-fall interrupt is enabled. Values between 100 ms
- * and 350 ms (0x14 to 0x46) are recommended.
- */
- int setFreefallTime(short int freefallTime_ms);
-
- /**
- * Get the axis tap settings.
- *
- * D3 D2 D1 D0
- * +----------+--------------+--------------+--------------+
- * | Suppress | TAP_X enable | TAP_Y enable | TAP_Z enable |
- * +----------+--------------+--------------+--------------+
- *
- * (D7-D4 are 0s).
- *
- * See datasheet for more details.
- *
- * @return The contents of the TAP_AXES register.
- */
- char getTapAxisControl(void);
-
- /**
- * Set the axis tap settings.
- *
- * D3 D2 D1 D0
- * +----------+--------------+--------------+--------------+
- * | Suppress | TAP_X enable | TAP_Y enable | TAP_Z enable |
- * +----------+--------------+--------------+--------------+
- *
- * (D7-D4 are 0s).
- *
- * See datasheet for more details.
- *
- * @param The control byte to write to the TAP_AXES register.
- */
- int setTapAxisControl(char settings);
-
- /**
- * Get the source of a tap.
- *
- * @return The contents of the ACT_TAP_STATUS register.
- */
- char getTapSource(void);
-
- /**
- * Get the interrupt enable settings.
- *
- * @return The contents of the INT_ENABLE register.
- */
-
- char getInterruptEnableControl(void);
-
- /**
- * Set the interrupt enable settings.
- *
- * @param settings The control byte to write to the INT_ENABLE register.
- */
- int setInterruptEnableControl(char settings);
-
- /**
- * Get the interrupt mapping settings.
- *
- * @return The contents of the INT_MAP register.
- */
- char getInterruptMappingControl(void);
-
- /**
- * Set the interrupt mapping settings.
- *
- * @param settings The control byte to write to the INT_MAP register.
- */
- int setInterruptMappingControl(char settings);
-
- /**
- * Get the interrupt source.
- *
- * @return The contents of the INT_SOURCE register.
- */
- char getInterruptSource(void);
-
-
-private:
-
- I2C i2c_;
-
-
- /**
- * Read one byte from a register on the device.
- *
- * @param: - the address to be read from
- *
- * @return: the value of the data read
- */
- char SingleByteRead(char address);
-
- /**
- * Write one byte to a register on the device.
- *
- * @param:
- - address of the register to write to.
- - the value of the data to store
- */
-
-
- int SingleByteWrite(char address, char data);
-
- /**
- * Read several consecutive bytes on the device and store them in a given location.
- *
- * @param startAddress: The address of the first register to read from.
- * @param ptr_output: a pointer to the location to store the data being read
- * @param size: The number of bytes to read.
- */
- void multiByteRead(char startAddress, char* ptr_output, int size);
-
- /**
- * Write several consecutive bytes on the device.
- *
- * @param startAddress: The address of the first register to write to.
- * @param ptr_data: Pointer to a location which contains the data to write.
- * @param size: The number of bytes to write.
- */
- int multiByteWrite(char startAddress, char* ptr_data, int size);
-
-};
-
-#endif /* ADXL345_I2C_H */
--- a/HMC5883L.cpp Thu Sep 20 13:00:24 2012 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,174 +0,0 @@
-/**
- * @author Jose R. Padron
- * @author Used HMC6352 library developed by Aaron Berk as template
- * @section LICENSE
- *
- * Copyright (c) 2010 ARM Limited
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- * THE SOFTWARE.
- *
- * @section DESCRIPTION
- *
- * Honeywell HMC5883Ldigital compass.
- *
- * Datasheet:
- *
- * http://www.ssec.honeywell.com/magnetic/datasheets/HMC5883L.pdf
- */
-
-/**
- * Includes
- */
-#include "HMC5883L.h"
-
-HMC5883L::HMC5883L(PinName sda, PinName scl) {
-
- i2c_ = new I2C(sda, scl);
- //100KHz, as specified by the datasheet.
- i2c_->frequency(100000);
-
-
-}
-
-
-void HMC5883L::write(int address, int data) {
-
- char tx[2];
-
- tx[0]=address;
- tx[1]=data;
-
- i2c_->write(HMC5883L_I2C_WRITE,tx,2);
-
- wait_ms(100);
-
-}
-
-
-void HMC5883L::setSleepMode() {
-
- write(HMC5883L_MODE, HMC5883L_SLEEP);
-}
-
-void HMC5883L::setDefault(void) {
-
- write(HMC5883L_CONFIG_A,HMC5883L_10HZ_NORMAL);
- write(HMC5883L_CONFIG_B,HMC5883L_1_0GA);
- write(HMC5883L_MODE,HMC5883L_CONTINUOUS);
- wait_ms(100);
-}
-
-
-void HMC5883L::getAddress(char *buffer) {
-
- char rx[3];
- char tx[1];
- tx[0]=HMC5883L_IDENT_A;
-
-
- i2c_->write(HMC5883L_I2C_WRITE, tx,1);
-
- wait_ms(1);
-
- i2c_->read(HMC5883L_I2C_READ,rx,3);
-
- buffer[0]=rx[0];
- buffer[1]=rx[1];
- buffer[2]=rx[2];
-}
-
-
-
-void HMC5883L::setOpMode(int mode, int ConfigA, int ConfigB) {
-
-
- write(HMC5883L_CONFIG_A,ConfigA);
- write(HMC5883L_CONFIG_B,ConfigB);
- write(HMC5883L_MODE,mode);
-
-
-}
-
-
-
-
-void HMC5883L::readData(int* getMag) {
-
-
- char tx[1];
- char rx[2];
-
-
- tx[0]=HMC5883L_X_MSB;
- i2c_->write(HMC5883L_I2C_READ,tx,1);
- i2c_->read(HMC5883L_I2C_READ,rx,2);
- getMag[0]= (int)rx[0]<<8|(int)rx[1];
-
-
- tx[0]=HMC5883L_Y_MSB;
- i2c_->write(HMC5883L_I2C_READ,tx,1);
- i2c_->read(HMC5883L_I2C_READ,rx,2);
- getMag[1]= (int)rx[0]<<8|(int)rx[1];
-
- tx[0]=HMC5883L_Z_MSB;
- i2c_->write(HMC5883L_I2C_READ,tx,1);
- i2c_->read(HMC5883L_I2C_READ,rx,2);
- getMag[2]= (int)rx[0]<<8|(int)rx[1];
-
-}
-
-int HMC5883L::getMx() {
-
- char tx[1];
- char rx[2];
-
-
- tx[0]=HMC5883L_X_MSB;
- i2c_->write(HMC5883L_I2C_READ,tx,1);
- i2c_->read(HMC5883L_I2C_READ,rx,2);
- return ((int)rx[0]<<8|(int)rx[1]);
-
-}
-
-int HMC5883L::getMy() {
-
- char tx[1];
- char rx[2];
-
-
- tx[0]=HMC5883L_Y_MSB;
- i2c_->write(HMC5883L_I2C_READ,tx,1);
- i2c_->read(HMC5883L_I2C_READ,rx,2);
- return ((int)rx[0]<<8|(int)rx[1]);
-
-}
-
-
-int HMC5883L::getMz(){
-
- char tx[1];
- char rx[2];
-
-
- tx[0]=HMC5883L_Z_MSB;
- i2c_->write(HMC5883L_I2C_READ,tx,1);
- i2c_->read(HMC5883L_I2C_READ,rx,2);
- return ((int)rx[0]<<8|(int)rx[1]);
-
-}
\ No newline at end of file
--- a/HMC5883L.h Thu Sep 20 13:00:24 2012 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,219 +0,0 @@
-/**
- * @author Uwe Gartmann
- * @author Used HMC5883L library developed by Jose R. Padron and Aaron Berk as template
- *
- * @section LICENSE
- *
- * Copyright (c) 2010 ARM Limited
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- * THE SOFTWARE.
- *
- * @section DESCRIPTION
- *
- * Honeywell HMC5883L digital compass.
- *
- * Datasheet:
- *
- * http://www.ssec.honeywell.com/magnetic/datasheets/HMC5883L.pdf
- */
-
-#ifndef HMC5883L_H
-#define HMC5883L_H
-
-/**
- * Includes
- */
-#include "mbed.h"
-
-/**
- * Defines
- */
-#define HMC5883L_I2C_ADDRESS 0x1E //7-bit address. 0x3C write, 0x3D read.
-#define HMC5883L_I2C_WRITE 0x3C
-#define HMC5883L_I2C_READ 0x3D
-
-//Values Config A
-#define HMC5883L_0_5HZ_NORMAL 0x00
-#define HMC5883L_0_5HZ_POSITIVE 0x01
-#define HMC5883L_0_5HZ_NEGATIVE 0x02
-
-#define HMC5883L_1HZ_NORMAL 0x04
-#define HMC5883L_1HZ_POSITIVE 0x05
-#define HMC5883L_1HZ_NEGATIVE 0x06
-
-#define HMC5883L_2HZ_NORMAL 0x08
-#define HMC5883L_2HZ_POSITIVE 0x09
-#define HMC5883L_2HZ_NEGATIVE 0x0A
-
-#define HMC5883L_5HZ_NORMAL 0x0C
-#define HMC5883L_5HZ_POSITIVE 0x0D
-#define HMC5883L_5HZ_NEGATIVE 0x0E
-
-#define HMC5883L_10HZ_NORMAL 0x10
-#define HMC5883L_10HZ_POSITIVE 0x11
-#define HMC5883L_10HZ_NEGATIVE 0x12
-
-#define HMC5883L_20HZ_NORMAL 0x14
-#define HMC5883L_20HZ_POSITIVE 0x15
-#define HMC5883L_20HZ_NEGATIVE 0x16
-
-#define HMC5883L_50HZ_NORMAL 0x18
-#define HMC5883L_50HZ_POSITIVE 0x19
-#define HMC5883L_50HZ_NEGATIVE 0x1A
-
-//Values Config B
-#define HMC5883L_0_7GA 0x00
-#define HMC5883L_1_0GA 0x20
-#define HMC5883L_1_5GA 0x40
-#define HMC5883L_2_0GA 0x60
-#define HMC5883L_3_2GA 0x80
-#define HMC5883L_3_8GA 0xA0
-#define HMC5883L_4_5GA 0xC0
-#define HMC5883L_6_5GA 0xE0
-
-//Values MODE
-#define HMC5883L_CONTINUOUS 0x00
-#define HMC5883L_SINGLE 0x01
-#define HMC5883L_IDLE 0x02
-#define HMC5883L_SLEEP 0x03
-
-
-
-#define HMC5883L_CONFIG_A 0x00
-#define HMC5883L_CONFIG_B 0x01
-#define HMC5883L_MODE 0x02
-#define HMC5883L_X_MSB 0x03
-#define HMC5883L_X_LSB 0x04
-#define HMC5883L_Z_MSB 0x05
-#define HMC5883L_Z_LSB 0x06
-#define HMC5883L_Y_MSB 0x07
-#define HMC5883L_Y_LSB 0x08
-#define HMC5883L_STATUS 0x09
-#define HMC5883L_IDENT_A 0x0A
-#define HMC5883L_IDENT_B 0x0B
-#define HMC5883L_IDENT_C 0x0C
-
-
-
-/**
- * Honeywell HMC5883L digital compass.
- */
-class HMC5883L {
-
-public:
-
- /**
- * Constructor.
- *
- * @param sda mbed pin to use for SDA line of I2C interface.
- * @param scl mbed pin to use for SCL line of I2C interface.
- */
- HMC5883L(PinName sda, PinName scl);
-
-
- /**
- * Enter into sleep mode.
- *
- */
- void setSleepMode();
-
-
- /**
- * Set Device in Default Mode.
- * HMC5883L_CONTINUOUS, HMC5883L_10HZ_NORMAL HMC5883L_1_0GA
- */
- void setDefault();
-
-
- /**
- * Read the memory location on the device which contains the address.
- *
- * @param Pointer to a buffer to hold the address value
- * Expected H, 4 and 3.
- */
- void getAddress(char * address);
-
-
-
- /**
- * Set the operation mode.
- *
- * @param mode 0x00 -> Continuous
- * 0x01 -> Single
- * 0x02 -> Idle
- * @param ConfigA values
- * @param ConfigB values
- */
- void setOpMode(int mode, int ConfigA, int ConfigB);
-
- /**
- * Write to on the device.
- *
- * @param address Address to write to.
- * @param data Data to write.
- */
-
- void write(int address, int data);
-
- /**
- * Get the output of all three axes.
- *
- * @param Pointer to a buffer to hold the magnetics value for the
- * x-axis, y-axis and z-axis [in that order].
- */
- void readData(int* getMag);
-
- /**
- * Get the output of X axis.
- *
- * @return x-axis magnetic value
- */
- int getMx();
-
- /**
- * Get the output of Y axis.
- *
- * @return y-axis magnetic value
- */
- int getMy();
-
- /**
- * Get the output of Z axis.
- *
- * @return z-axis magnetic value
- */
- int getMz();
-
-
- /**
- * Get the current operation mode.
- *
- * @return Status register values
- */
- int getStatus(void);
-
-
-
- I2C* i2c_;
-
-
-
-};
-
-#endif /* HMC5883L_H */
\ No newline at end of file
--- a/main.cpp Thu Sep 20 13:00:24 2012 +0000
+++ b/main.cpp Thu Nov 29 15:22:06 2012 +0000
@@ -1,87 +1,55 @@
#include "mbed.h"
#include "ITG3200.h"
-#include "HMC5883L.h"
#define N 3
-#define chan 6
+#define pi 3.14159265
+
+double* RK4( double, double[N], double[N] );
+double* func( double[N], double[N] );
ITG3200 gyro(p9, p10);
-HMC5883L compass(p9, p10);
-InterruptIn ch1(p13); // aileron
-InterruptIn ch2(p14); // elevator
-InterruptIn ch3(p15); // throttle
-InterruptIn ch4(p16); // rudder
-InterruptIn ch5(p17); // switch1
-InterruptIn ch6(p18); // switch2
-AnalogIn sonar(p19); // sonar
-PwmOut ch1_out(p21);
-PwmOut ch2_out(p22);
-PwmOut ch3_out(p23);
-PwmOut ch4_out(p24);
-PwmOut ch5_out(p25);
-PwmOut ch6_out(p26);
Serial pc(USBTX, USBRX);
-Timer t_aile; Timer t_elev; Timer t_thro; Timer t_rudd; Timer t_switch1; Timer t_switch2;
-
-void aile_up(); void elev_up(); void thro_up(); void rudd_up(); void switch1_up(); void switch2_up();
-void aile_down(); void elev_down(); void thro_down(); void rudd_down(); void switch1_down(); void switch2_down();
-
-double pwm_pulse[chan] = {0.001,0.001,0.001,0.001,0.001,0.001}; // aile, elev, thro, rudd, switch1, switch2
-
int main(){
- float dt = 0.1;
- float t = 0;
+ // keisan zyou no kizami haba
+ float dt = 0.01;
+ // zissai ni wait suru zikan
+ float dt_wait = 0.01;
+ // hyouzi you
+ float t = 0;
+
pc.baud(921600);
- double range = 0;
- int getMag[N] = {0}; // Buffer of the compass
- double Gyro [N] = {0};
- int Mag [N] = {0};
+ // x = [ p, q, r ]
+ // y = [ phi, the, psi ]
+ double x_rad[N] = {0};
+ double x_deg[N] = {0};
+ double y_rad[N] = {0};
+ double y_deg[N] = {0};
- double servo[6] = {0.001, 0.001, 0.001, 0.001, 0.001, 0.001};
-
- ch1_out.period(0.02); ch2_out.period(0.02); ch3_out.period(0.02); ch4_out.period(0.02); ch5_out.period(0.02); ch6_out.period(0.02);
- // pulsewidth 0.001~0.002
- ch1_out.pulsewidth(0.001); ch2_out.pulsewidth(0.001); ch3_out.pulsewidth(0.001); ch4_out.pulsewidth(0.001); ch5_out.pulsewidth(0.001); ch6_out.pulsewidth(0.001);
+ double* pybuf; // pointer for gyro
// *** start up ***
gyro.setLpBandwidth(LPFBW_42HZ);
- compass.setDefault();
wait(0.1); // Wait some time for all sensors (Need at least 5ms)
// *** start up ***
- // Reciever function
- ch1.rise(&aile_up); ch1.fall(&aile_down);
- ch2.rise(&elev_up); ch2.fall(&elev_down);
- ch3.rise(&thro_up); ch3.fall(&thro_down);
- ch4.rise(&rudd_up); ch4.fall(&rudd_down);
- ch5.rise(&switch1_up); ch5.fall(&switch1_down);
- ch6.rise(&switch2_up); ch6.fall(&switch2_down);
-
- pc.printf("\n\rHiko-robo!!\n\r");
-
while(1){
- range = sonar;
+ x_deg[0] = (gyro.getGyroX())/14.375+9.4;
+ x_deg[1] = (gyro.getGyroY())/14.375-0.8;
+ x_deg[2] = (gyro.getGyroZ())/14.375-4;
+
+ for ( int i=0;i<N;i++ ){ x_rad[i] = x_deg[i]*pi/180; }
- // Updating accelerometer and compass
- compass.readData(getMag);
-
- Gyro[0] = (gyro.getGyroX())/14.375;
- Gyro[1] = (gyro.getGyroY())/14.375;
- Gyro[2] = (gyro.getGyroZ())/14.375;
- // Calibration YAL 9DOF Compass:X, Y, Z
- Mag[0] = (int16_t)getMag[0];
- Mag[1] = (int16_t)getMag[1];
- Mag[2] = (int16_t)getMag[2];
+ // RK4
+ pybuf = RK4(dt,y_rad,x_rad);
+ for ( int i=0;i<N;i++ ){ y_rad[i] = *pybuf; pybuf = pybuf+1; }
- for( int i=0;i<6;i++ ){ servo[i] = pwm_pulse[i]; }
+ for ( int i=0;i<N;i++ ){ y_deg[i] = y_rad[i]*180/pi; }
- ch1_out.pulsewidth(servo[0]); ch2_out.pulsewidth(servo[1]); ch3_out.pulsewidth(servo[2]); ch4_out.pulsewidth(servo[3]); ch5_out.pulsewidth(servo[4]); ch6_out.pulsewidth(servo[5]);
-
- pc.printf("%7.2f, %7.1f, %7.1f, %7.1f, %f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f\n\r", t, Gyro[0], Gyro[1], Gyro[2], range, pwm_pulse[0]*1000, pwm_pulse[1]*1000, pwm_pulse[2]*1000, pwm_pulse[3]*1000, pwm_pulse[4]*1000, pwm_pulse[5]*1000);
+ pc.printf("%7.2f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f\n\r", t, x_deg[0], x_deg[1], x_deg[2], y_deg[0], y_deg[1], y_deg[2]);
t += dt;
wait(dt);
@@ -90,45 +58,51 @@
}
-void aile_up(){ t_aile.start(); }
-void elev_up(){ t_elev.start(); }
-void thro_up(){ t_thro.start(); }
-void rudd_up(){ t_rudd.start(); }
-void switch1_up(){ t_switch1.start(); }
-void switch2_up(){ t_switch2.start(); }
+double* RK4( double dt, double y[N], double x[N] ){
+
+ double yBuf[N] = {0};
+ double k[N][4] = {0};
+
+ double* p_y = y;
+
+ double* pk1;
+ double* pk2;
+ double* pk3;
+ double* pk4;
+
+ for ( int i=0;i<N;i++){ yBuf[i] = y[i]; }
+ pk1 = func (yBuf,x);
+ for ( int i=0;i<N;i++ ){ k[i][0] = *pk1; pk1 = pk1+1; }
-void aile_down(){
- t_aile.stop();
- pwm_pulse[0] = t_aile.read();
- t_aile.reset();
-}
+ for ( int i=0;i<N;i++){ yBuf[i] = y[i]+0.5*dt*k[i][1]; }
+ pk2 = func (yBuf,x);
+ for ( int i=0;i<N;i++ ){ k[i][1] = *pk2; pk2 = pk2+1; }
+
+ for ( int i=0;i<N;i++){ yBuf[i] = y[i]+0.5*dt*k[i][2]; }
+ pk3 = func (yBuf,x);
+ for ( int i=0;i<N;i++ ){ k[i][2] = *pk3; pk3 = pk3+1; }
-void elev_down(){
- t_elev.stop();
- pwm_pulse[1] = t_elev.read();
- t_elev.reset();
+ for ( int i=0;i<N;i++){ yBuf[i] = y[i]+dt*k[i][3]; }
+ pk4 = func (yBuf,x);
+ for ( int i=0;i<N;i++ ){ k[i][3] = *pk4; pk4 = pk4+1; }
+
+ for ( int i=0;i<N;i++){ y[i] = y[i]+dt*(k[i][0]+2.0*k[i][1]+2.0*k[i][2]+k[i][3])/6.0; }
+
+ return p_y;
+
}
-void thro_down(){
- t_thro.stop();
- pwm_pulse[2] = t_thro.read();
- t_thro.reset();
-}
-
-void rudd_down(){
- t_rudd.stop();
- pwm_pulse[3] = t_rudd.read();
- t_rudd.reset();
-}
+double* func( double y[N], double x[N] ){
-void switch1_down(){
- t_switch1.stop();
- pwm_pulse[4] = t_switch1.read();
- t_switch1.reset();
-}
+ double f[N] = {0};
+ double* p_f = f;
+
+ f[0] = x[0]+x[1]*sin(y[0])*tan(y[1])+x[2]*cos(y[0])*tan(y[1]);
+ f[1] = x[1]*cos(y[0])-x[2]*sin(y[0]);
+ f[2] = x[1]*sin(y[0])/cos(y[1])+x[2]*cos(y[0])/cos(y[1]);
+
+ wait(.000000001);
+
+ return p_f;
-void switch2_down(){
- t_switch2.stop();
- pwm_pulse[5] = t_switch2.read();
- t_switch2.reset();
}
\ No newline at end of file