From 907408e31db94de5efa8dcf694b5e121d3dd5406 Mon Sep 17 00:00:00 2001
From: jancoow <jancokock@gmail.com>
Date: Mon, 23 May 2016 22:30:59 +0200
Subject: [PATCH] Joystick offset calculation

---
 Controller/Controller.ino | 23 ++++++++++++-----------
 1 file changed, 12 insertions(+), 11 deletions(-)

diff --git a/Controller/Controller.ino b/Controller/Controller.ino
index 1d4d43f..2b730cc 100644
--- a/Controller/Controller.ino
+++ b/Controller/Controller.ino
@@ -34,18 +34,13 @@ struct SwitchData{
 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
 
 // orientation/motion vars
 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
 
 volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
@@ -53,6 +48,9 @@ void dmpDataReady() {
     mpuInterrupt = true;
 }
 
+//Joystick offsets
+int joystickoffset_x, joystickoffset_y;
+
 //Data buffer for sensors
 struct MPU6050Data mpu6050data = {0};
 struct JoystickData joystickdata = {0};
@@ -66,7 +64,7 @@ WiFiUDP Udp;
 
 void setup(void){
   Serial.begin(115200);
-  Serial.println();
+  Serial.println("Starting controller..");
   WiFi.mode(WIFI_STA);
   WiFi.disconnect();  
   EEPROM.begin(512);
@@ -79,11 +77,14 @@ void setup(void){
   pinMode(13,INPUT);
   
   //clearEeprom();
-  connectLastBasestation();
-  //searchBasestation();
+  //connectLastBasestation();
+  searchBasestation();
 
   delay(1000);
   ads.begin();
+
+  joystickoffset_x = ads.readADC_SingleEnded(0);
+  joystickoffset_y = ads.readADC_SingleEnded(1);
   
   mpu6050setup();
 }
@@ -91,7 +92,7 @@ void setup(void){
 void mpu6050setup(){
   mpu.initialize(); 
   Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
-  devStatus = mpu.dmpInitialize();
+  int devStatus = mpu.dmpInitialize();
   
   mpu.setXGyroOffset(62);
   mpu.setYGyroOffset(25);
@@ -159,8 +160,8 @@ void loop(void){
 }
 
 void readJoystick(struct JoystickData *joystickdata){
-  joystickdata->x = ads.readADC_SingleEnded(0);
-  joystickdata->y = ads.readADC_SingleEnded(1);
+  joystickdata->x = ads.readADC_SingleEnded(0) - joystickoffset_x;
+  joystickdata->y = ads.readADC_SingleEnded(1) - joystickoffset_y;
 }
 
 void readFifoMpu6050(struct MPU6050Data *mpu6050data){