//Title: quad_trans
//Programmer: Joe MOrrow
//Date: June 14, 2011
//Contact: jmmorrow@mit.edu
//Description: Code for pilot's transmitter (and receiver)

#include <LiquidCrystal.h>
LiquidCrystal lcd(12,11,7,6,5,4);

int throttlePin = A0;
int rollPin = A1;
int pitchPin = A2;
int yawPin = A3;
int ctrlPin = A4;
int killPin = A5;

float throttle;
float roll;
float pitch;
float yaw;
//float throttleOld = 0.0;
//float rollOld = 0.0;
//float pitchOld = 0.0;
//float yawOld = 0.0;
//
//float dT = 0.01;
//float pole = 4;

float deg = 180.0/3.14159;

void setup() {
  
  pinMode(throttlePin, INPUT);
  pinMode(rollPin, INPUT);
  pinMode(pitchPin, INPUT);
  pinMode(yawPin, INPUT);
  pinMode(ctrlPin, INPUT);
  pinMode(killPin, INPUT);
  
  lcd.begin(16,2);
  lcd.clear();
  
  Serial.begin(9600);
  
}

void loop() {
  
//  delay(5);
  int setting1 = digitalRead(killPin);
  float setting2 = digitalRead(ctrlPin);
  
  if (setting1 == HIGH) {
    
    throttle = analogRead(throttlePin);
    throttle = mapf(throttle,216.0,1010.0,400.0,700.0);
   
  } else {

    throttle = 0.0;
    
  }
  
  roll = analogRead(rollPin);
  pitch = analogRead(pitchPin);
  yaw = analogRead(yawPin);
  roll = mapf(roll,167.0,1023.0,-0.1,0.1);
  pitch = mapf(pitch,186.0,942.0,-0.1,0.1);
  yaw = mapf(yaw,1023.0,120.0,-0.1,0.1);
  
//  (throttle*2 + throttleOld/dT)/((1/dT) + 2);
//  (roll*pole + rollOld/dT)/((1/dT) + pole);
//  (pitch*pole + pitchOld/dT)/((1/dT) + pole);
//  (yaw*pole + yawOld/dT)/((1/dT) + pole);
    
//allocate 5 characters per float
  Serial.print(";");
  Serial.print(throttle/100,3);
  Serial.print(",");
  if (roll < 0.0) {
    Serial.print(roll*10,2);
  } else {
    Serial.print(roll*10,3);
  }
  Serial.print(",");
  if (pitch < 0.0) {
    Serial.print(pitch*10,2);
  } else {
    Serial.print(pitch*10,3);
  }
  Serial.print(",");
  if (yaw < 0.0) {
    Serial.print(yaw*10,2);
  } else {
    Serial.print(yaw*10,3);
  }
  Serial.print(",");
  Serial.print(setting2,3);
  Serial.print(",");
  Serial.print(setting2,3);
  Serial.println();

  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print(throttle/10.0,2);
  if (roll >= 0.0) {
    lcd.setCursor(1,1);
  } else {
    lcd.setCursor(0,1);
  }
  lcd.print(roll*180.0/3.14159,2);
  if (pitch >= 0.0) {
    lcd.setCursor(11,0);
  } else {
    lcd.setCursor(10,0);
  }
  lcd.print(pitch*10*110,2);
  if (yaw >= 0.0) {
    lcd.setCursor(11,1);
  } else {
    lcd.setCursor(10,1);
  }
  lcd.print(yaw*10*110,2);
  
//  throttleOld = throttle;
//  rollOld = roll;
//  pitchOld = pitch;
//  yawOld = yaw;
  
//  delay(10);
  
}
  
  /////////////////////////////////////////////////////

float mapf(float x, float in_min, float in_max, float out_min, float out_max)
{
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}