Example #1
0
File: Leveler.cs Project: NGCP/UUV
        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);
            }
        }