public void baseRand()
{
while (run)
{
greate.setvolume(100);
greate.setVoice(Greating.Zira);
greate.speak("hello");
greate.great();
Random rand = new Random();
int num = rand.Next(10, 20);
int time = rand.Next(2, 7);
Thread.Sleep(2000);
IRobotCreate.SetDrive(-num, num);
Thread.Sleep(2000);
IRobotCreate.SetDrive(0, 0);
Thread.Sleep(2000);
IRobotCreate.SetDrive(num, -num);
Thread.Sleep(2000);
IRobotCreate.SetDrive(0, 0);
Thread.Sleep(2000);
IRobotCreate.SetDrive(num, -num);
Thread.Sleep(2000);
IRobotCreate.SetDrive(0, 0);
Thread.Sleep(2000);
IRobotCreate.SetDrive(-num, num);
Thread.Sleep(2000);
IRobotCreate.SetDrive(0, 0);
Thread.Sleep(time * 1000);
}
IRobotCreate.SetDrive(0, 0);
}
private void Window_Closing(object sender, System.ComponentModel.CancelEventArgs e)
{
Behaviour.run = false;
update.Abort();
personData.Dispose();
personModule.Dispose();
faceData.Dispose();
sm.Dispose();
IRobotCreate.SetDrive(0, 0); //To Stop iRobot
}
public void init()
{
double speed, inputSpeed;
servos = new Servo("COM17");
servos.enableTorque(neck, Servo.Torque_1);
servos.enableTorque(head, Servo.Torque_1);
servos.setSpeed(neck, 10); //set speed to 10ms
servos.setSpeed(head, 10); //set speed to 10ms
greate = new Greating(); //create greating object
//iRobot = new IRobotCreate();
IRobotCreate.init(115200, "COM7");
IRobotCreate.sensorDataHandler += new IRobotCreate.SensorDataHandler(onSensorData);
}
public static void stopAll()
{
IRobotCreate.SetDrive(0, 0);
}
private void Update()
{
// Start AcquireFrame-ReleaseFrame loop
while (sm.AcquireFrame(true) >= pxcmStatus.PXCM_STATUS_NO_ERROR)
{
// Acquire color image data
PXCMCapture.Sample sample = sm.QuerySample();
Bitmap colorBitmap;
PXCMImage.ImageData colorData;
sample.color.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_RGB32, out colorData);
colorBitmap = colorData.ToBitmap(0, sample.color.info.width, sample.color.info.height);
// Create an instance of MyTrackedPerson
MyTrackedPerson myTrackedPerson = new MyTrackedPerson();
// Acquire person tracking data
personData = personModule.QueryOutput();
myTrackedPerson.PersonsDetected = personData.QueryNumberOfPeople();
if (myTrackedPerson.PersonsDetected == 1)
{
// person track data
PXCMPersonTrackingData.Person trackedPerson = personData.QueryPersonData(PXCMPersonTrackingData.AccessOrderType.ACCESS_ORDER_BY_ID, 0);
PXCMPersonTrackingData.PersonTracking trackedPersonData = trackedPerson.QueryTracking();
PXCMPersonTrackingData.BoundingBox2D personBox = trackedPersonData.Query2DBoundingBox();
myTrackedPerson.X = personBox.rect.x;
myTrackedPerson.Y = personBox.rect.y;
myTrackedPerson.H = personBox.rect.h;
myTrackedPerson.W = personBox.rect.w;
// Acquire face tracking data
faceData.Update();
myTrackedPerson.FacesDetected = faceData.QueryNumberOfDetectedFaces();
if (myTrackedPerson.FacesDetected == 1)
{
PXCMFaceData.Face face = faceData.QueryFaceByIndex(0);
PXCMFaceData.DetectionData faceDetectionData = face.QueryDetection();
PXCMRectI32 faceRectangle;
faceDetectionData.QueryBoundingRect(out faceRectangle);
myTrackedPerson.FaceH = faceRectangle.h;
myTrackedPerson.FaceW = faceRectangle.w;
myTrackedPerson.FaceX = faceRectangle.x;
myTrackedPerson.FaceY = faceRectangle.y;
float faceDepth;
faceDetectionData.QueryFaceAverageDepth(out faceDepth);
myTrackedPerson.FaceDepth = faceDepth;
//save image
if (doit == true)
{
colorBitmap.Save("myBitmap" + imgID + ".bmp");
doit = false;
stopwatch.Start();
}
else if (stopwatch.Elapsed.Seconds > 10)
{
imgID++;
doit = true;
stopwatch.Reset();
}
}
//IRobotCreate.SetDrive(40, 40);
//my.servoNeck.setTargetPosition(1500);
my.servos.move(Behaviour.neck, 100);//change because new servo class
if ((0 < (myTrackedPerson.X)) && ((myTrackedPerson.X) <= 120))
{
//my.servoShoulder.setTargetPosition(1300);
my.servos.move(Behaviour.neck, 90);
IRobotCreate.SetDrive(20, -20);
}
else if ((120 < (myTrackedPerson.X)) && ((myTrackedPerson.X) < 310))
{
// follow up based on face depth
//my.servoShoulder.setTargetPosition(1500);
my.servos.move(Behaviour.head, 100);//change because new servo class
float depth = myTrackedPerson.FaceDepth - (int)myTrackedPerson.FaceDepth % 100;
if (myTrackedPerson.FacesDetected == 1 && depth < 1750 && depth > 1400)
{
IRobotCreate.SetDrive(0, 0);
}
else if (myTrackedPerson.FacesDetected == 1 && depth < 1400)
{
IRobotCreate.SetDrive(-100, -100);
}
else if (myTrackedPerson.FacesDetected == 1 && depth > 1750)
{
IRobotCreate.SetDrive(100, 100);
}
}
else if (310 <= (myTrackedPerson.X))
{
//my.servoShoulder.setTargetPosition(1700);
my.servos.move(Behaviour.head, 120);//change because new servo class
IRobotCreate.SetDrive(-20, 20);
}
}
//my.servoNeck.SetSpeed(40);
// my.servoShoulder.SetSpeed(40);
my.servos.setSpeed(Behaviour.neck, 100); //change because new servo class
my.servos.setSpeed(Behaviour.head, 100); //change because new servo class
// Update UI
Render(colorBitmap, myTrackedPerson);
// Release resources
colorBitmap.Dispose();
sample.color.ReleaseAccess(colorData);
sm.ReleaseFrame();
}
}
public void SetIRobotDrive(int left, int right)
{
IRobotCreate.SetDrive(left, right);
}