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