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);
}
}