//*!!Sensor,    S1,             kCompass, sensorCustom,      ,                   !!*//
//*!!Sensor,    S2,          touchSensor, sensorTouch,      ,                    !!*//
//*!!                                                                            !!*//
//*!!Start automatically generated configuration code.                           !!*//
const tSensors kCompass             = (tSensors) S1;   //sensorCustom       //*!!!!*//
const tSensors touchSensor          = (tSensors) S2;   //sensorTouch        //*!!!!*//
//*!!CLICK to edit 'wizard' created sensor & motor configuration.                !!*//
//
// This program is started when the touch sensor is pushed (by a servo). It then moves the rudder to turn the airplane
// 90 degress to the left, at which point it returns to straight-ahead flight. This is a navigation-only autopilot;
// it would have to be accompanied by a stabalization system, such as the FMA Co-Pilot, to maintain level flight
// autonomously.
//
// This program is best used to set up a plane for autonomous flight and test the mechanical assemblies. For true
// autonomous flight a GPS receiver would be used instead of the compass sensor and true waypoint navigation would
// be included. These functions are included in a different program.
//
// Created by Chris Anderson, DIYDrones.com, 2007
// Published under a Creative Commons Attribution licence
//
//////////////////////////////////////////////////////////////////////////////////////////
//
#include "Compass Sensor Driver.c"

// Variables and constants

	int CompassHeading;
	int InitialCompassHeading;
	int TargetHeading;
	int turn					= 90; // change to whatever turn amount you want
	bool DeflectLeft = false;
	bool DeflectRight = false;
	bool LoopStop = false;
	bool AutoPilotON;
	int ncounter;
	int workingtotal = 0;
	int currentPosition = 0;

// Subroutines

task CheckAutoPilot()
{
 while(true)
 {
  if (SensorValue(touchSensor) == 1) AutoPilotON=true;
  else AutoPilotON=false;
  wait1Msec(1000);
 }
}


void Initialize()
{
	SensorType[kCompass]    = sensorI2CCustomStd;
	SensorSubType[kCompass] = subTypeHiTechnicCompass;
	StartTask(taskCompassDeviceDriver);
	nxtDisplayTextLine(4, "Autopilot Initialized");  // Display the text on line number 4 of 8 on the LCD
}

///////////////////////////////////
//  Turn
/////////////////////////////////
void Turn(int NewSetting)
{
	if (NewSetting > currentPosition)
		{
	   nMotorEncoderTarget[motorA] = NewSetting - currentPosition;       // incremental amount to move motor
		 motor[motorA] = 100;								//motor A is run at a 100 power level
		 while ((nMotorRunState[motorA] != runStateIdle) && AutoPilotON)
    	{}  																						// wait for action to complete
		}
	if (NewSetting < currentPosition)
		{
		nMotorEncoderTarget[motorA] = currentPosition - NewSetting;       // incremental amount to move motor
  	motor[motorA] = -100;								//motor A is run at a 100 power level
    while ((nMotorRunState[motorA] != runStateIdle) && AutoPilotON)
	    {}  																						// wait for action to complete
		}
	motor[motorA] = 0;
	currentPosition = nMotorEncoder[motorA];
}
////////////////////////////////
// Set the rudder straight
///////////////////////////////

void StraightenUp()
{
			if (DeflectLeft) // return the rudder to the neutral position
				{
				 Turn(currentPosition - 100);
				 DeflectLeft = false;
				}
			if (DeflectRight)
				{
					Turn(currentPosition + 100);
					DeflectRight = false;
				}
}
///////////////////////////////////////////////
// This is the section where we turn the rudder to head the plane to the new course, and stop turning when we're on
// that course.
//////////////////////////////////////////////

void SteeringLoop ()
{
	LoopStop = false;
	while (!LoopStop)
		{
		CompassHeading = SensorValue[kCompass];
		nxtDisplayTextLine(1, "Target: %d", (short) TargetHeading); //display target compass heading
		nxtDisplayTextLine(2, "Current: %d", (short) CompassHeading); //display current compass heading
		nxtDisplayTextLine(3, "Deflect Left: %d", (short) DeflectLeft); //display left rudder command
		nxtDisplayTextLine(4, "Deflect Right: %d",(short) DeflectRight); //display right rudder command
		if ((CompassHeading > TargetHeading + 10) || (CompassHeading < TargetHeading -10 )) // turn the rudder a set amount left or right depending on compass reading
			{
				 if (!DeflectLeft && !DeflectRight) // rudder not already turned
					{
					 if (CompassHeading > TargetHeading + 10)
						 {
							Turn(100);
						  DeflectLeft = true;
						 }
					 if (CompassHeading < TargetHeading - 10)
						 {
							Turn(-100);
	  				  DeflectRight = true;
						 }
					}
			}
		else StraightenUp();

		if (SensorValue(touchSensor) == 0)
			{
			LoopStop = true;
			StraightenUp();
		  }
		}
}

/////////////////////////////////////////
// Take inititial readings and start steering loop
/////////////////////////////////////////

void Autopilot()
{
	nxtDisplayTextLine(4, " Autopilot ON  ");  // Display the text on line number 4 of 8 on the LCD
	StartTask(CheckAutoPilot);
	for (ncounter = 0; ncounter < 10; ++ncounter)
		{
			workingtotal = workingtotal + SensorValue[kCompass];  //average the initial compass readings
			wait10Msec(1);
		}
	InitialCompassHeading = workingtotal/10;
	while ((InitialCompassHeading > 360) || (InitialCompassHeading < 0)) // just in case it's still screwed up
		{
			InitialCompassHeading = SensorValue[kCompass];  // it it's out of range, keep sampling until it's right
		}
	TargetHeading = InitialCompassHeading + turn;
	if (TargetHeading > 359) TargetHeading = TargetHeading - 360; //correct for values over 360
	nMotorEncoder = 0; //reset the motor encoder to 0
	SteeringLoop();                     // go to the steering loop and do all the work
	StopTask(CheckAutoPilot);
	nxtDisplayTextLine(4, " Autopilot OFF  ");  // Display the text on line number 4 of 8 on the LCD
}

// Main program

  task main()
{
	Initialize(); //Initialize sensors//
	while(true)   //Loop forever
   {
     alive(); //ensure that the NXT doesn't automatically power down while waiting for the autopilot to be turned on
     if (SensorValue(touchSensor) == 1) Autopilot();
     Autopilot();
    }
}