public Bipedal5LinkHardware(double m, double m1, double m2, double l1, double l2, PointD hip, double[] theta, double servoSpeed, ServoController hardware)
{
M = m;
M1 = m1;
M2 = m2;
L1 = l1;
L2 = l2;
Joints = new double[5];
ServoSpeed = servoSpeed;
Theta = new double[theta.Length];
Array.Copy(theta, Theta, theta.Length);
Feedback = new double[9];
P = new PointD[5];
P[0] = hip;
P[1] = PointD.FromPoint(P[0], Theta[0] - Theta[1], L1);
P[2] = PointD.FromPoint(P[0], Theta[0] - Theta[2], L1);
P[3] = PointD.FromPoint(P[1], Theta[0] - Theta[1] - Theta[3], L2);
P[4] = PointD.FromPoint(P[2], Theta[0] - Theta[2] - Theta[4], L2);
M1PlusM2 = M1 + M2;
M1PlusM2TimesTwo = M1PlusM2 * 2;
MTotal = M + 2 * M1 + 2 * M2;
PiOverTwo = Math.PI / 2;
PiOverFour = Math.PI / 4;
ThreePiOverTwo = 3 * Math.PI / 2;
UpdateCenterOfMass();
PenLeft = new Pen(Brushes.DarkOrange, 3);
BrushLeft = new SolidBrush(Color.DarkOrange);
PenRight = new Pen(Brushes.Yellow, 3);
BrushRight = new SolidBrush(Color.Yellow);
PenGround = new Pen(Brushes.SaddleBrown, 2);
LSteps = new ArrayList(100);
RSteps = new ArrayList(100);
System.Reflection.Assembly a = System.Reflection.Assembly.GetExecutingAssembly();
S = new SoundPlayer(a.GetManifestResourceStream("Bipedal5Link.step.wav"));
Hardware = hardware;
}
public Bipedal5LinkHardware(double m, double m1, double m2, double l1, double l2, PointD hip, double[] theta, double servoSpeed, ServoController hardware)
{
M = m;
M1 = m1;
M2 = m2;
L1 = l1;
L2 = l2;
Joints = new double[5];
ServoSpeed = servoSpeed;
Theta = new double[theta.Length];
Array.Copy(theta, Theta, theta.Length);
Feedback = new double[9];
P = new PointD[5];
P[0] = hip;
P[1] = PointD.FromPoint(P[0], Theta[0] - Theta[1], L1);
P[2] = PointD.FromPoint(P[0], Theta[0] - Theta[2], L1);
P[3] = PointD.FromPoint(P[1], Theta[0] - Theta[1] - Theta[3], L2);
P[4] = PointD.FromPoint(P[2], Theta[0] - Theta[2] - Theta[4], L2);
M1PlusM2 = M1 + M2;
M1PlusM2TimesTwo = M1PlusM2 * 2;
MTotal = M + 2 * M1 + 2 * M2;
PiOverTwo = Math.PI / 2;
PiOverFour = Math.PI / 4;
ThreePiOverTwo = 3 * Math.PI / 2;
UpdateCenterOfMass();
PenLeft = new Pen(Brushes.DarkOrange, 3);
BrushLeft = new SolidBrush(Color.DarkOrange);
PenRight = new Pen(Brushes.Yellow, 3);
BrushRight = new SolidBrush(Color.Yellow);
PenGround = new Pen(Brushes.SaddleBrown, 2);
LSteps = new ArrayList(100);
RSteps = new ArrayList(100);
System.Reflection.Assembly a = System.Reflection.Assembly.GetExecutingAssembly();
S = new SoundPlayer(a.GetManifestResourceStream("Bipedal5Link.step.wav"));
Hardware = hardware;
}
