// Arduino Eldiven Simulator Kumandası
// V0.6 wo/IRQ Alfa - Zafer ŞAHİN
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
#define CH_ANL 4 // kanal sayısı - ANALOG ( potansiyometre, v.b. )
#define CH_DGT 4 // kanal sayısı - DIGITAL ( anahtar v.b. )
#define CENTER_VAL 1500 // varsayılan servo merkez sinyali
#define PWM_H_LEN 500 // varsayılan pwm sinyal genişliğinin yarısı, yani maksimum sinyal = CENTER_VAL + PWM_H_LEN, minimum sinyal = CENTER_VAL - PWM_H_LEN
#define PPM_FRLEN 22500 // mikrosaniye cinsinden PPM çerçeve uzunluğu (1ms = 1000µs)
#define PPM_PULSELEN 300 // PPM başlangıç darbesinin uzunluğu
#define ONSTATE 0 // darbelerin kutuplaması, 0 negatif, 1 pozitif
#define SIGPIN 8 // PPM sinyali için arduino'nun çıkış pini
#define T_LIMIT 255 // EPA ve TRIM için limit değer
#define TRIM_ENABLE 0 // TRIM ayarlamasının açık olup olmadığını belirtir. 0 kapalı, >0 açık.
#define EPA_ENABLE 0 // EPA ayarlamasının açık olup olmadığını belirtir. 0 kapalı, >0 açık.
#define PRINT_RAW_DATA 0
#define LED_PIN 13
const int CHANNELS = CH_ANL + CH_DGT; // toplam kanal sayısı - ANALOG + DIJITAL
int pwm_max = CENTER_VAL + PWM_H_LEN; // PWM servo sinyali için üst limit
int pwm_min = CENTER_VAL - PWM_H_LEN; // PWM servo sinyali için alt limit
bool blinkState = false;
MPU6050 mpu;
bool dmpReady = false; // set true if DMP init was successful
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer
Quaternion q; // [w, x, y, z] quaternion container
VectorInt16 aa; // [x, y, z] accel sensor measurements
VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
VectorFloat gravity; // [x, y, z] gravity vector
float euler[3]; // [psi, theta, phi] Euler angle container
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
float throttle=500.0;
float roll,pitch,yaw;
unsigned long timer = 0;
int yawMin = -180;
int yawMax = 180;
int yawLim[] = {0,0};
int pitchMin = -86;
int pitchMax = 89;
int pitchLim[] = {0,0};
int rollMin = -86;
int rollMax = 89;
int rollLim[] = {0,0};
int throttleMin = 900;
int throttleMax = 100;
int ppm_val[CHANNELS];
int CHN[CH_ANL] = {1000,1000,1000,1000};
int ypr_offset[CH_ANL] = {0,0,0,0};
int ypr_m[CH_ANL] = {0,0,0,0};
int ypr_range[CH_ANL] = {(yawMax-yawMin),(pitchMax-pitchMin),(rollMax-rollMin),0};
unsigned long tPPMCurrent, tPPMPrev;
bool bCalibrate = false;
bool dummy = false;
void setup() {
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
tPPMCurrent = micros();
tPPMPrev = tPPMCurrent;
mpu.initialize();
devStatus = mpu.dmpInitialize();
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(178:coolxf:; // 1688 factory default for my test chip
if (devStatus == 0) {
mpu.setDMPEnabled(true);
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
dummy=true;
}
pinMode(SIGPIN, OUTPUT);
pinMode(LED_PIN, OUTPUT);
digitalWrite(SIGPIN, !ONSTATE);
digitalWrite(LED_PIN, blinkState);
}
void loop() {
tPPMCurrent = micros();
if (tPPMCurrent < tPPMPrev) tPPMPrev = tPPMCurrent; // Against micros() overflow.
if( !bCalibrate && ( millis() > 15000)){
ypr_offset[0] = yaw - ((yawMin + yawMax) / 2);
ypr_offset[1] = pitch - ((pitchMin + pitchMax) / 2);
ypr_offset[2] = roll - ((rollMin + rollMax) / 2);
yawMin += ypr_offset[0];
yawMax += ypr_offset[0];
pitchMin += ypr_offset[1];
pitchMax += ypr_offset[1];
rollMin += ypr_offset[2];
rollMax += ypr_offset[2];
digitalWrite(LED_PIN, !blinkState);
bCalibrate = true;
}
mpuIntStatus = mpu.getIntStatus();
fifoCount = mpu.getFIFOCount();
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
mpu.resetFIFO();
} else if (mpuIntStatus & 0x02) {
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
mpu.getFIFOBytes(fifoBuffer, packetSize);
fifoCount -= packetSize;
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
yaw = ypr[0] * 180/M_PI;
pitch = ypr[1] * 180/M_PI;
roll = ypr[2] * 180/M_PI;
if(yaw < yawMin){
CHN[0] = map(yaw+ypr_range[0],yawMin,yawMax,0,1000);
} else if(yaw > yawMax){
CHN[0] = map(yaw-ypr_range[0],yawMin,yawMax,0,1000);
} else {
CHN[0] = map(yaw,yawMin,yawMax,0,1000);
}
if(pitch < pitchMin){
CHN[1] = map(pitch+ypr_range[1],pitchMin,pitchMax,0,1000);
} else if(pitch > pitchMax){
CHN[1] = map(pitch-ypr_range[1],pitchMin,pitchMax,0,1000);
} else {
CHN[1] = map(pitch,pitchMin,pitchMax,0,1000);
}
if(roll < rollMin){
CHN[2] = map(roll+ypr_range[2],rollMin,rollMax,0,1000);
} else if(roll > rollMax){
CHN[2] = map(roll-ypr_range[2],rollMin,rollMax,0,1000);
} else {
CHN[2] = map(roll,rollMin,rollMax,0,1000);
}
CHN[3] = map(throttle,throttleMin,throttleMax,0,1000);
for (int i = 0; i < CHANNELS; ++i) {
if (i<CH_ANL){
ppm_val[i] = CHN[i] + 1000;
} else {
ppm_val[i] = 1500;
}
}
}
if (tPPMCurrent - tPPMPrev > PPM_FRLEN) {
tPPMPrev = tPPMCurrent;
createPPMFrame();
}
}
void createPPMFrame(){
for (int i = 0; i < CHANNELS; ++i) {
digitalWrite(SIGPIN, ONSTATE);
delayMicroseconds(PPM_PULSELEN);
digitalWrite(SIGPIN, !ONSTATE);
delayMicroseconds(ppm_val[i] - PPM_PULSELEN);
}
digitalWrite(SIGPIN, ONSTATE);
delayMicroseconds(PPM_PULSELEN);
digitalWrite(SIGPIN, !ONSTATE);
}
