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();
}
private void levelingTask()
{
//Purpose: background task to continuously send updated controller outputs thru serial to motorcontroller, and update PID vals
//Inputs:
while (true)
{
uCon.sensorRequest();
if (Microcontroller.PIDupdates.Count > 0)
{
foreach (pidConf d in Microcontroller.PIDupdates)
{
switch (d.index)
{
case 0:
desiredPitch = d.desval;
ptch.updateTerms(d);
//Ppitch = d;
break;
case 1:
desiredRoll = d.desval;
rll.updateTerms(d);
//Proll = d;
break;
case 2:
desiredSurge = d.desval;
surge.updateTerms(d);
Psurge = d;
break;
case 3:
desiredSway = d.desval;
sway.updateTerms(d);
Psway = d;
break;
case 4:
desiredDepth = d.desval;
depth.updateTerms(d);
Pdepth = d;
break;
case 5:
//desiredYaw = d.desval;
//yaw.updateTerms(d);
Pyaw = d;
break;
}
}//after we've gone through all of the pid updates, clear them
Microcontroller.PIDupdates.Clear();
}
//there will be some thread.pause in here, so we're not sending tooooo many updates.
//send the message. check byte implementation in the works.
//65 is byte val for A, identifying general thrust.
//66 is the byte val for B, which identifies corner thruster offsets.
//the thrust is from -81 to 81.
//first message has surge, sway, depth and yaw. Yaw is applied to surge(as offset) in the microcontroller
//uCon.Send(new byte[] { 65, (byte)(Tsu + 81), (byte)(Tsw + 81), (byte)(Td + 81), (byte)(Ty + 81), 88 });
uCon.Send(new byte[] { 65, (byte)(Tsu), (byte)(Tsw), (byte)(Td), (byte)(Ty), 88 });
Thread.Sleep(25);
//second message has corner motor leveling, applied as offset on microcontroller.
uCon.Send(new byte[] { 66, (byte)(((Tp + Tr) / 2) + 81), (byte)(((Tp - Tr) / 2) + 81), (byte)(((-Tr - Tp) / 2) + 81), (byte)((((Tr - Tp) / 2)) + 81), 88 });
Thread.Sleep(25);
}
}
//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();
//}
}
