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