//this will handle integration and such of/for the imu and heading;
//basically: integrating x-y accel, and using the heading to change angling.
//hopefully
public PositionTracker(IMU440 i, Microcontroller m, log loger)
{
lg = loger;
navigation = i;
mic = m;
xvel = 0;
yvel = 0;
}
public AutonomousBehaviour(IMU440 IMU1, Microcontroller uC1, PositionTracker pos1, ThrustManager lev1, log ERRORLOG)
{
// TODO: Complete member initialization
this.IMU = IMU1;
this.uC = uC1;
this.pos = pos1;
this.lev = lev1;
this.ERRORLOG = ERRORLOG;
}
public ThrustManager(IMU440 nav, Microcontroller u, PositionTracker p, log loger)
{
//Purpose: Constructor for thrustManager
//Inputs: the IMU object, and the microcontroller object, which each have the serial communication functions within.
lg = loger;
navigation = nav;
position = p;
uCon = u;
desiredPitch = 0;
desiredRoll = 0;
desiredDepth = -0.5;
desiredYaw = 0;
desiredSurge = 0;
desiredSway = 0;
Tp = 0; //pitch
Tr = 0; //roll
Ty = 0; //yaw
Td = 0; //depth
Tsu = 0; //surge
Tsw = 0; //sway
uCon.sensorRequest();
//gots to tune these pids
ptch = new PID(0, 0, 0, 60.0, -60.0, 40, -40, getActualPitch, getDesiredPitch, computeTp);
rll = new PID(.5, 0, .4, 60.0, -60.0, 40, -40, getActualRoll, getDesiredRoll, computeTr);
surge = new PID(0.6, 0, 0, 3, -3, 75, -75, getActualSurge, getDesiredSurge, computeTsu);
sway = new PID(0.6, 0, 0, 3, -3, 75, -75, getActualSway, getDesiredSway, computeTsw);
depth = new PID(0.8, 0.1, .8, 0, -9, 75, -75, getActualDepth, getDesiredDepth, computeTd);
yaw = new PID(0, 0, 0, 180, -180, 40, -40, getActualYaw, getDesiredYaw, computeTy);
ptch.Enable();
rll.Enable();
//commented cause we dont have everything worked out for yaw, and i haven't written something to calculate velocity
surge.Enable();
sway.Enable();
depth.Enable();
yaw.Enable();
backgroundThrustManager = new Thread(new ThreadStart(levelingTask));
backgroundThrustManager.IsBackground = true;
backgroundThrustManager.Start();
}
//public static Sonar sonar;
//public static AutonomousBehaviour auto;
static void Main(string[] args)
{
//we might consider switching the COM port references over to commandline input
bool is_init = false;
bool init_imu = false;
bool init_due = false;
int _try = 0;
ERRORLOG = new log();
Dictionary <string, string> serials = parseSerialLocations();
ERRORLOG.LOG(" Starting Program", 0);
while (!is_init && _try < 5)
{
try
{
//externals
ERRORLOG.LOG("F**k you jeff", 0);
ERRORLOG.LOG("Initialization: " + _try.ToString() + " of 6", 0);
if (!init_imu)
{
IMU = new IMU440(serials["IMU"], 57600, ERRORLOG, false);
}
init_imu = true;
ERRORLOG.LOG("Initialized IMU", 0);
if (!init_due)
{
uC = new Microcontroller(serials["DUE"], 115200, ERRORLOG);
}
init_due = true;
ERRORLOG.LOG("Initialized DUE", 0);
//sonar will make an appearance soon.
//sonar = new Sonar(serials["SONAR"], log);//port
//internals
pos = new PositionTracker(IMU, uC, ERRORLOG);
lev = new ThrustManager(IMU, uC, pos, ERRORLOG);
ERRORLOG.LOG("Initialized ThrustManager", 0);
//auto = new AutonomousBehaviour(IMU,uC,pos,lev,ERRORLOG);
is_init = true;
}
catch (Exception e)
{
ERRORLOG.LOG(e.Message, 0);
Thread.Sleep(5000);
_try++;
is_init = false;
}
}
//this is because communications issues:
for (int l = 0; l < 15; l++)
{
uC.sensorRequest();
}
while (is_init)
{
}
ERRORLOG.LOG("Exiting", 0);
//while (true)
//{//testing driver
// Console.WriteLine("pitch: " + Math.Round(IMU.pitchAngle*(180/Math.PI)));
// Console.WriteLine("roll: " + Math.Round(IMU.rollAngle * (180 /Math.PI)));
// Console.WriteLine("tp: " + Math.Round(lev.Tp,4));
// Console.WriteLine("tr: " + Math.Round(lev.Tr,4));
// Console.WriteLine(IMU.boardTemp);
// Thread.Sleep(200);
// Console.Clear();
//}
}
