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