public GyroCal(AHRS sensor) { int i = 0; InitializeComponent(); this.sensor = sensor; // Add DataReceived event handler. sensor.DataReceivedEvent += new DataReceivedDelegate(DataReceivedEventHandler); sensor.confReceivedEvent += new confReceivedDelegate(confReceivedEventHandler); confReceivedEventHandler(i); data_collection_enabled = false; next_data_index = 0; loggedData = new double[SAMPLES, 3]; threshold = 1.5 * 3.14159 / 180; bias = new double[3]; calMat = new double[3, 3]; calMat[0, 0] = 1.0; calMat[1, 1] = 1.0; calMat[2, 2] = 1.0; D = new GeneralMatrix(SAMPLES, 10); for (i = 0; i < SAMPLES; i++) { loggedData[i, 0] = 0; loggedData[i, 1] = 0; loggedData[i, 2] = 0; } }
public AHRSInterface() { InitializeComponent(); initializeSerialPort(); sensor = new AHRS(); // Set up event handlers sensor.PacketTimeoutEvent += new StateDelegate(TimeoutEventHandler); sensor.PacketReceivedEvent += new PacketDelegate(PacketReceivedEventHandler); sensor.DataReceivedEvent += new DataReceivedDelegate(DataReceivedEventHandler); sensor.PacketSentEvent += new PacketDelegate(PacketSentEventHandler); sensor.COMFailedEvent += new COMFailedDelegate(COMFailedEventHandler); renderTimer = new Timer(); renderTimer.Interval = 10; renderTimer.Enabled = true; renderTimer.Tick += new System.EventHandler(OnRenderTimerTick); //Form_3Dcuboid form_3DcuboidA = new Form_3Dcuboid(new string[] { "RightInv.png", "LeftInv.png", "BackInv.png", "FrontInv.png", "TopInv.png", "BottomInv.png" }); form_3DcuboidA.Text += " A"; BackgroundWorker backgroundWorkerA = new BackgroundWorker(); backgroundWorkerA.DoWork += new DoWorkEventHandler(delegate { form_3DcuboidA.ShowDialog(); }); backgroundWorkerA.RunWorkerAsync(); }
public AHRSLog(AHRS sensor) { InitializeComponent(); loggingEnabled = false; this.sensor = sensor; sensor.DataReceivedEvent += new DataReceivedDelegate(DataReceivedEventHandler); }
public ConfSet(AHRS sensor) { int i = 0; InitializeComponent(); this.sensor = sensor; // Add DataReceived event handler. sensor.confReceivedEvent += new confReceivedDelegate(confReceivedEventHandler); confReceivedEventHandler(i); }
// turningpid turn; public Robot2() { button = new DigitalInput(0); NavX = new AHRS(SPI.Port.MXP); frontLeft = new Jaguar(4); frontRight = new Jaguar(2); backLeft = new Jaguar(5); backRight = new Jaguar(3); joystick = new Joystick(0); frontRight.Inverted = true; backRight.Inverted = true; shooter = new Jaguar(1); turrent = new Jaguar(0); collector = new Jaguar(6); // turn = new turningpid(); }
public calcVar(AHRS sensor) { int i = 0; InitializeComponent(); this.sensor = sensor; // Add DataReceived event handler. sensor.DataReceivedEvent += new DataReceivedDelegate(DataReceivedEventHandler); data_collection_enabled = false; next_data_index = 0; accxdata = new double[SAMPLES]; accydata = new double[SAMPLES]; acczdata = new double[SAMPLES]; magxdata = new double[SAMPLES]; magydata = new double[SAMPLES]; magzdata = new double[SAMPLES]; gyroxdata = new double[SAMPLES]; gyroydata = new double[SAMPLES]; gyrozdata = new double[SAMPLES]; calMat = new double[3, 3]; calMat[0, 0] = 1.0; calMat[1, 1] = 1.0; calMat[2, 2] = 1.0; for (i = 0; i < SAMPLES; i++) { accydata[i] = 0; acczdata[i] = 0; magxdata[i] = 0; magydata[i] = 0; magzdata[i] = 0; gyroxdata[i] = 0; gyroydata[i] = 0; gyrozdata[i] = 0; } }
public InfoDump(AHRS sensor) { InitializeComponent(); this.sensor = sensor; // Add new event handlers sensor.PacketReceivedEvent += new PacketDelegate(PacketReceivedEventHandler); sensor.PacketTimeoutEvent += new StateDelegate(TimeoutEventHandler); sensor.DataReceivedEvent += new DataReceivedDelegate(DataReceivedEventHandler); displayAHRSStates(); initializeGraphs(); timer1.Interval = 50; timer1.Start(); time = 0; }
void initParts() { scalingB = new CANTalon(0); rightMotorA = new CANTalon(1) { NeutralMode = NeutralMode.Brake, Inverted = true }; rightMotorB = new CANTalon(2) { NeutralMode = NeutralMode.Brake, Inverted = true }; rightMotorC = new CANTalon(3) { NeutralMode = NeutralMode.Brake, Inverted = true }; scalingArm = new CANTalon(4) { NeutralMode = NeutralMode.Brake }; indexer = new CANTalon(5) { NeutralMode = NeutralMode.Brake }; collector = new CANTalon(6); shooterA = new CANTalon(9); shooterB = new CANTalon(10); shooterAngle = new CANTalon(11) { NeutralMode = NeutralMode.Brake }; leftMotorA = new CANTalon(12) { NeutralMode = NeutralMode.Brake }; leftMotorB = new CANTalon(13) { NeutralMode = NeutralMode.Brake }; leftMotorC = new CANTalon(14) { NeutralMode = NeutralMode.Brake }; scalingA = new CANTalon(15); leftDriveEncoder = new Encoder(0, 1, false, EncodingType.K1X); rightDriveEncoder = new Encoder(2, 3, true, EncodingType.K1X); shooterSpeedB = new Counter(4) { SamplesToAverage = 5 }; shooterSpeedA = new Counter(5) { SamplesToAverage = 5 }; armLowerLimit = new DigitalInput(6); ballSensor = new DigitalInput(7); tapeSensor = new DigitalInput(8); armUpperLimit = new DigitalInput(9); flag = new Servo(9); armPot = new AnalogInput(0); shooterPot = new AnalogInput(1); sonar = new AnalogInput(2); navx = new AHRS(SPI.Port.MXP, (byte)50); autoTimer = new Timer(); leftStick = new Joystick(0); rightStick = new Joystick(1); angleControl = new PIDController(.03, .0005, .5, navx, null) { Continuous = true }; angleControl.SetInputRange(-180, 180); angleControl.SetOutputRange(-.6, .6); angleControl.SetAbsoluteTolerance(1.0); driveControl = new PIDController(.04, .00005, .03, navx, null) { Continuous = true }; driveControl.SetInputRange(-180, 180); driveControl.SetOutputRange(-1, 1); }
/// <summary> /// Initialize the controllers. /// </summary> public static void Init() { // Instantiates all the hardware devices with the respective ports. #region InstantiateDevices can_01 = new CANTalon(1); can_02 = new CANTalon(2); //can_02.MotorControlMode = ControlMode.Follower; //can_02.Set(can_01.DeviceId); //can_02.ReverseOutput(true); can_03 = new CANTalon(3); can_04 = new CANTalon(4); //can_04.MotorControlMode = ControlMode.Follower; //can_04.Set(can_03.DeviceId); can_05 = new CANTalon(5); can_06 = new CANTalon(6); can_07 = new CANTalon(7); can_08 = new CANTalon(8); can_09 = new CANTalon(9); can_10 = new CANTalon(10); can_11 = new Compressor(11); can_12 = new PowerDistributionPanel(12); pcm_11_0 = new Solenoid(11, 0); pcm_11_1 = new Solenoid(11, 1); pcm_11_2 = new Solenoid(11, 2); usb_0 = new Joystick(0); usb_1 = new Joystick(1); usb_2 = new Joystick(2); sw_0 = new DriveTrainObject(Left1, Left2, Right1, Right2); #endregion // Clears the sticky faults on all CAN devices. #region ClearCANStickyFaults can_01.ClearStickyFaults(); can_02.ClearStickyFaults(); can_03.ClearStickyFaults(); can_04.ClearStickyFaults(); can_05.ClearStickyFaults(); can_06.ClearStickyFaults(); can_07.ClearStickyFaults(); can_08.ClearStickyFaults(); can_09.ClearStickyFaults(); can_10.ClearStickyFaults(); can_11.ClearAllPCMStickyFaults(); can_12.ClearStickyFaults(); #endregion // Disable motor safety so that it doesn't stop motors from being output to. #region DisableSafety can_01.SafetyEnabled = false; can_02.SafetyEnabled = false; can_03.SafetyEnabled = false; can_04.SafetyEnabled = false; can_05.SafetyEnabled = false; can_06.SafetyEnabled = false; can_07.SafetyEnabled = false; can_08.SafetyEnabled = false; can_09.SafetyEnabled = false; can_10.SafetyEnabled = false; sw_0.SafetyEnabled = false; #endregion Intake2.Inverted = true; Shooter_Pivot.Inverted = true; Intake1.Inverted = true; Agitator.Inverted = true; // Initializes the camera server with the default options. CameraServer.Instance.StartAutomaticCapture(); //UsbCamera cam = new UsbCamera("cam0", 0); //cam.SetResolution(320, 240); //CameraServer.Instance.StartAutomaticCapture(cam); // Instantiates the NavX. NavX = new AHRS(SPI.Port.MXP); // Creates a new copy of the turntable. TurntableEncoder = new Encoder(8, 9); #region PID Controllers // Handles moving forwards and backwards for the shooter using the Pixy as the sensor. CamForward = new CamForwardPID(3.00, 0.00, 0.00, 0.00); // Handles the setpoint needed for the shooter using Pixy data. ShooterPos = new ShooterPosPID(3.00, 0.00, 0.00, 0.00); // Creates a new instance of the turn controller. TurnController = new TurningPID(new PIDF { kP = 0.0465, // 0.054 kI = 0.00, kD = 0.00, kF = 0.00 }); // Sets the tolerance of the PID controller to 0.02. // Cancels the turning if the error is within 0.02. TurnController.Controller.SetAbsoluteTolerance(0.2); // Adds the PID controller to the Live Window for easier testing. LiveWindow.AddActuator("PID Controllers", "Turn Control", TurnController.Controller); #endregion }
public VTOLUAV() // Constructor -> Is Called when the Object is created { // When the Object is loaded from the sd card this method will not be called uavData.Add(totalpidgain); joystick.values.Add(phi_rolerate); joystick.values.Add(theta_rolerate); joystick.values.Add(psi_rolerate); joystick.values.Add(throttle); uavData.Add(joystick); lagesensor = new AHRS("lagesensor", UsbHelper.GetDevicPathebyClass("AHRS")); lagesensor["cursettings"].Value = "371,376,379,17,-127,-87"; uavData.Add(lagesensor); gpsempfänger = new GPS("gpsempfänger", UsbHelper.GetDevicPathebyClass("GPS"), 38400); uavData.Add(gpsempfänger); // empfängerusb = new UAVJoystick ("empfängerusb", "", new IntPtr ()); //uavData.Add (empfängerusb); watch.Start(); // Servo Initialisierung servo1 = new PWM("servo1", 0, null, 1); //("Name", startwert, "null"=ausgabegerät_maestro, Kanal am Maestro) uavData.Add(servo1); servo1 ["LowLimit"].DoubleValue = -100; servo1 ["Neutral"].DoubleValue = 0; servo1 ["HighLimit"].DoubleValue = 100; servo1 ["Invert"].IntValue = 0; servo1.SetHomePosition(0); servo2 = new PWM("servo2", 0, null, 2); uavData.Add(servo2); servo2 ["LowLimit"].DoubleValue = -100; servo2 ["Neutral"].DoubleValue = 0; servo2 ["HighLimit"].DoubleValue = 100; servo2 ["Invert"].IntValue = 0; servo2.SetHomePosition(0); servo3 = new PWM("servo3", 100, null, 3); uavData.Add(servo3); servo3 ["LowLimit"].DoubleValue = -80; servo3 ["Neutral"].DoubleValue = 0; servo3 ["HighLimit"].DoubleValue = 80; servo3 ["Invert"].IntValue = 1; servo3.SetHomePosition(100); servo4 = new PWM("servo4", 100, null, 4); uavData.Add(servo4); servo4 ["LowLimit"].DoubleValue = -80; servo4 ["Neutral"].DoubleValue = 0; servo4 ["HighLimit"].DoubleValue = 80; servo4 ["Invert"].IntValue = 1; servo4.SetHomePosition(100); servo5 = new PWM("servo5", -80, null, 5); uavData.Add(servo5); servo5 ["LowLimit"].DoubleValue = -100; servo5 ["Neutral"].DoubleValue = 0; servo5 ["HighLimit"].DoubleValue = 100; servo5 ["Invert"].IntValue = 0; servo5.SetHomePosition(-80); servo6 = new PWM("servo6", -80, null, 6); uavData.Add(servo6); servo6 ["LowLimit"].DoubleValue = -100; servo6 ["Neutral"].DoubleValue = 0; servo6 ["HighLimit"].DoubleValue = 100; servo6 ["Invert"].IntValue = 0; servo6.SetHomePosition(-80); // PID CONFIG -------------------------------------- kp_Höhe = new UAVParameter("kp_Höhe", 7, 0, 10, 0); //Sinnvoll=1Höhe = new UAVParameter ("kp_Höhe", 1, max, min, updaterate) kd_Höhe = new UAVParameter("kd_Höhe", 10, -10, 10, 0); //Sinnvoll=-3 Achtung aus irgend einem Grund negativ !!! ki_Höhe = new UAVParameter("ki_Höhe", 0, 0, 10, 0); //Sinnvoll=1 ks_Höhe = new UAVParameter("ks_Höhe", 0.0, 0, 10, 0); //Sinnvoll=0.2 lp_Höhe = new UAVParameter("lp_Höhe", 1, 0, 10, 0); ld_Höhe = new UAVParameter("ld_Höhe", 1, 0, 10, 0); li_Höhe = new UAVParameter("li_Höhe", 1, 0, 10, 0); ls_Höhe = new UAVParameter("ls_Höhe", 0.2, 0, 10, 0); uavData.Add(kp_Höhe); uavData.Add(kd_Höhe); uavData.Add(ki_Höhe); uavData.Add(ks_Höhe); uavData.Add(lp_Höhe); uavData.Add(ld_Höhe); uavData.Add(li_Höhe); uavData.Add(ls_Höhe); //---------------------------------------------------- kp_Quer = new UAVParameter("kp_Quer", 5, 0, 10, 0); //Sinnvoll=1 kd_Quer = new UAVParameter("kd_Quer", 5, 0, 10, 0); //Sinnvoll=3 ki_Quer = new UAVParameter("ki_Quer", 0, 0, 10, 0); //Sinnvoll=1 ks_Quer = new UAVParameter("ks_Quer", 0.0, 0, 10, 0); //Sinnvoll=0.2 lp_Quer = new UAVParameter("lp_Quer", 1, 0, 10, 0); ld_Quer = new UAVParameter("ld_Quer", 1, 0, 10, 0); li_Quer = new UAVParameter("li_Quer", 1, 0, 10, 0); ls_Quer = new UAVParameter("ls_Quer", 0.2, 0, 10, 0); uavData.Add(kp_Quer); uavData.Add(kd_Quer); uavData.Add(ki_Quer); uavData.Add(ks_Quer); uavData.Add(lp_Quer); uavData.Add(ld_Quer); uavData.Add(li_Quer); uavData.Add(ls_Quer); //---------------------------------------------------- kp_Seite = new UAVParameter("kp_Seite", -3, -10, 10, 0); kd_Seite = new UAVParameter("kd_Seite", 6, -10, 10, 0); ki_Seite = new UAVParameter("ki_Seite", 0, -10, 10, 0); ks_Seite = new UAVParameter("ks_Seite", 0, -10, 10, 0); lp_Seite = new UAVParameter("lp_Seite", 0.5, 0, 10, 0); ld_Seite = new UAVParameter("ld_Seite", 0.5, 0, 10, 0); li_Seite = new UAVParameter("li_Seite", 1, 0, 10, 0); ls_Seite = new UAVParameter("ls_Seite", 0, 0, 10, 0); uavData.Add(kp_Seite); uavData.Add(kd_Seite); uavData.Add(ki_Seite); uavData.Add(ks_Seite); uavData.Add(lp_Seite); uavData.Add(ld_Seite); uavData.Add(li_Seite); uavData.Add(ls_Seite); //---------------------------------------------------- sp_Höhe = new UAVParameter("Höhe_SP", 0, -90, 90, 10); sp_Quer = new UAVParameter("Quer_SP", 0, -180, 180, 10); sp_Seite = new UAVParameter("Seite_SP", 0, -180, 180, 10); uavData.Add(sp_Höhe); uavData.Add(sp_Quer); uavData.Add(sp_Seite); output_Höhe = new UAVParameter("PID_Out_Höhe", 0, -100, 100, 0); output_Quer = new UAVParameter("PID_Out_Quer", 0, -100, 100, 0); output_Seite = new UAVParameter("PID_Out_Seite", 0, -100, 100, 0); uavData.Add(output_Höhe); uavData.Add(output_Quer); uavData.Add(output_Seite); SteuerMotorLeistung = new UAVParameter("SteuerMotorLeistung", 0, 0, 100, 0); HauptMotorLeistung = new UAVParameter("HauptMotorLeistung", -80, -100, 100, 0); HauptMotorDiff = new UAVParameter("HauptMotorDiff", 0, -20, 20, 0); uavData.Add(SteuerMotorLeistung); uavData.Add(HauptMotorLeistung); uavData.Add(HauptMotorDiff); NewSeitePV = new UAVParameter("NewSeitePV", 0, -180, 180, 0); uavData.Add(NewSeitePV); uavData.Add(output_Höhe); }
public VTOLUAV() // Constructor -> Is Called when the Object is created { // When the Object is loaded from the sd card this method will not be called joystick.values.Add(phi_rolerate); joystick.values.Add(theta_rolerate); joystick.values.Add(psi_rolerate); joystick.values.Add(throttle); uavData.Add(joystick); //Factor = new UAVParameter ("Factory", 10, 0, 1000, 0); // Factor for Mixers //uavData.Add (Factor); lagesensor = new AHRS("lagesensor", UsbHelper.GetDevicPathebyClass("AHRS")); uavData.Add(lagesensor); ////gpsempfänger = new GPS ("gpsempfänger", UsbHelper.GetDevicPathebyClass ("GPS"), 38400); ////uavData.Add (gpsempfänger); ////empfängerusb = new UAVJoystick ("empfängerusb", "", new IntPtr ()); ////uavData.Add (empfängerusb); // Servo Initialisierung servo1 = new PWM("servo1", 0, null, 1); //("Name", startwert, "null"=ausgabegerät_maestro, Kanal am Maestro) uavData.Add(servo1); servo1["LowLimit"].DoubleValue = -80; servo1["Neutral"].DoubleValue = 0; servo1["HighLimit"].DoubleValue = 80; servo1["Invert"].IntValue = 0; servo2 = new PWM("servo2", 0, null, 2); uavData.Add(servo2); servo2 ["LowLimit"].DoubleValue = -80; servo2 ["Neutral"].DoubleValue = 0; servo2 ["HighLimit"].DoubleValue = 80; servo2 ["Invert"].IntValue = 0; servo3 = new PWM("servo3", 90, null, 3); uavData.Add(servo3); servo3 ["LowLimit"].DoubleValue = -100; servo3 ["Neutral"].DoubleValue = 0; servo3 ["HighLimit"].DoubleValue = 100; servo3 ["Invert"].IntValue = 1; servo4 = new PWM("servo4", 90, null, 4); uavData.Add(servo4); servo4 ["LowLimit"].DoubleValue = -100; servo4 ["Neutral"].DoubleValue = 0; servo4 ["HighLimit"].DoubleValue = 100; servo4 ["Invert"].IntValue = 1; servo5 = new PWM("servo5", 0, null, 5); uavData.Add(servo5); servo5 ["LowLimit"].DoubleValue = -100; servo5 ["Neutral"].DoubleValue = 0; servo5 ["HighLimit"].DoubleValue = 100; servo5 ["Invert"].IntValue = 0; //// PID CONFIG kp_Höhe = new UAVParameter("kp_Höhe", 5, 0, 10, 0); kd_Höhe = new UAVParameter("kd_Höhe", 0, 0, 10, 0); ki_Höhe = new UAVParameter("ki_Höhe", 0, 0, 10, 0); uavData.Add(kp_Höhe); uavData.Add(kd_Höhe); uavData.Add(ki_Höhe); kp_Quer = new UAVParameter("kp_Quer", 5, 1, 10, 0); kd_Quer = new UAVParameter("kd_Quer", 0, 1, 10, 0); ki_Quer = new UAVParameter("ki_Quer", 0, 1, 10, 0); uavData.Add(kp_Quer); uavData.Add(kd_Quer); uavData.Add(ki_Quer); kp_Seite = new UAVParameter("kp_Seite", 5, 0, 10, 0); kd_Seite = new UAVParameter("kd_Seite", 0, 0, 10, 0); ki_Seite = new UAVParameter("ki_Seite", 0, 0, 10, 0); uavData.Add(kp_Seite); uavData.Add(kd_Seite); uavData.Add(ki_Seite); sp_Höhe = new UAVParameter("Höhe_SP", 0, -90, 90, 10); sp_Quer = new UAVParameter("Quer_SP", 0, -180, 180, 10); sp_Seite = new UAVParameter("Seite_SP", 0, -180, 180, 10); uavData.Add(sp_Höhe); uavData.Add(sp_Quer); uavData.Add(sp_Seite); PID_Out_Höhe = new UAVParameter("PID_Out_Höhe", 0, -100, 100, 0); PID_Out_Quer = new UAVParameter("PID_Out_Quer", 0, -100, 100, 0); PID_Out_Seite = new UAVParameter("PID_Out_Seite", 0, -100, 100, 0); uavData.Add(PID_Out_Höhe); uavData.Add(PID_Out_Quer); uavData.Add(PID_Out_Seite); }
public saveconfiguration(AHRS sensor) { InitializeComponent(); this.sensor = sensor; }
public AHRSupload(AHRS sensor) { InitializeComponent(); this.sensor = sensor; }