<object width="420" height="315">
<param name="movie" value="http://www.youtube.com/v/jrFZDG9R-E0?version=3&hl=en_US&rel=0"></param>
<param name="allowFullScreen" value="true"></param>
<param name="allowscriptaccess" value="always"></param>
<embed src="http://www.youtube.com/v/jrFZDG9R-E0?version=3&hl=en_US&rel=0" type="application/x-shockwave-flash" width="420" height="315" allowscriptaccess="always" allowfullscreen="true"></embed>
</object>
- Code:
void UpdateRobobuilderArms(Byte bServo11, Byte bservo14)
{
//left arm
Byte bServo10=127, bServo12=127;
double dServo11 = Convert.ToDouble(bServo11);
// 5E-05x3 - 0.0213x2 + 2.9618x - 46.011
try
{
bServo10 = Convert.ToByte(0.00005 * Math.Pow(dServo11, 3) - 0.0213 * Math.Pow(dServo11, 2) + 2.9618 * dServo11 - 46.011);
// 8E-05x3 - 0.0339x2 + 4.7877x - 108.15
bServo12 = Convert.ToByte(0.00008 * Math.Pow(dServo11, 3) - 0.0339 * Math.Pow(dServo11, 2) + 4.7877 * dServo11 - 108.15);
}
catch (Exception)
{
}
// right arm
Byte bServo13=127 , bServo15=127;
try
{
double dServo14 = Convert.ToDouble(bservo14);
// 5E-05x3 - 0.0176x2 + 2.0147x + 87.278
bServo13 = Convert.ToByte(0.00005 * Math.Pow(dServo14,3) - 0.0176 * Math.Pow(dServo14,2) + 2.0147 * dServo14 + 87.27);
// 8E-05x3 - 0.0272x2 + 3.0598x + 24.756
bServo15 = Convert.ToByte(0.00008 * Math.Pow(dServo14,3) - 0.0272 * Math.Pow(dServo14,2) + 3.0598 * dServo14 + 24.756);
}
catch (Exception)
{
}
SendArmPosition(bServo10, bServo11, bServo12, bServo13, bservo14, bServo15);
}
void SendArmPosition(Byte bServo10, Byte bServo11, Byte bServo12, Byte bServo13, Byte bServo14, Byte bServo15 )
{
try {
if (!bRBConnected)
return;
rbWCK.wckMovePos(10, bServo10, 1); // 3rd parameter is Torque 0~4: 0=maximum
rbWCK.wckMovePos(11, bServo11, 1);
rbWCK.wckMovePos(12, bServo12, 1);
rbWCK.wckMovePos(13, bServo13, 1);
rbWCK.wckMovePos(14, bServo14, 1);
rbWCK.wckMovePos(15, bServo15, 1);
}
catch(Exception e)
{
MessageBox.Show("Unable to Update Servo Position " + e.Message);
}
}
int ScaleTo(double val, int imin, int imax, double dmin, double dmax)
{
int r;
if (val <dmin> dmax) val = dmax;
val = (val - dmin) / (dmax - dmin);
r= (int)((double)(imax - imin) * val) + imin;
return r;
}
void updaterobot(Microsoft.Kinect.Skeleton playerSkeleton)
{
//Figure out values for the shoulder based on Hand position.
// Kinect returns positive values if the hand is above the head and negative values if it's bellow the head
int ServoRange = bMaxShoulder - bMinShoulder;
//Left Arm
//Dim scaledLeftArm = playerSkeleton.Joints(JointType.HandRight).ScaleTo(1, ServoRange, 0.5F, 0.3F);
int sla = ScaleTo(playerSkeleton.Joints[JointType.HandRight].Position.Y, 0, ServoRange, 0.0, 0.3);
Byte bServo11 = Convert.ToByte(255 - (ServoRange - sla + bMinShoulder));
// Right Arm
//Dim scaledRightArm = playerSkeleton.Joints(JointType.HandLeft).ScaleTo(1, ServoRange, 0.5F, 0.3F);
int sra = ScaleTo(playerSkeleton.Joints[JointType.HandLeft].Position.Y, 0, ServoRange, 0.0, 0.3);
Byte bServo14 = Convert.ToByte(255-(255 - (ServoRange - sra + bMinShoulder)));
UpdateRobobuilderArms(bServo11, bServo14);
}
void ConnectToRB(string sCOMMNum)
{
try {
SerialPort s = new SerialPort(sCOMMNum,115200);
s.Open();
rbPCR = new RobobuilderLib.PCremote(s);
rbWCK = new RobobuilderLib.wckMotion(rbPCR);
if (rbWCK.wckReadPos(30))
{
//DCMP is true
rbWCK.DCMP = true;
rbWCK.PlayPose(1000, 10, wckMotion.basic16, true);
}
else
{
//DCMP false
rbPCR.runMotion(7); // basic posture
Thread.Sleep(3000);
rbWCK.DCMP = false;
rbPCR.setDCmode(true);
}
bRBConnected = true;
}
catch(Exception e)
{
MessageBox.Show("Unable to Connect to Robobuilder\n\n" + e.Message);
rbPCR = null;
bRBConnected = false;
}
}
void DisconnectFromRB()
{
if (rbWCK != null)
rbWCK.close();
if (rbPCR != null)
rbPCR.Close();
rbWCK = null;
rbPCR = null;
bRBConnected = false;
}
The COM port problem is caused by PCRemote trying to close the serial port with a null descriptor - it is bug - but the work round is to use the old style initialization, passing in the Serial port - and hence it won't be null.
I've also modified connect so it will work with either Standard firmware or DCMP firmware - it test servo 30.
Ive writen my own ScaleTo function so you don't need any extra library - just robobuilderlib.Statistics: Posted by l3v3rz — Sun Apr 01, 2012 3:48 pm
]]>
) !