public Robot(RobotPHX robotPHX, string name)
{
this.robotPHX = robotPHX;
servoMotors[(int)ServoType.FINGER] = robotPHX.QueryDeviceServoMotor(name + "_doigt/a1/servo");
servoMotors[(int)ServoType.ARM] = robotPHX.QueryDeviceServoMotor(name + "_bras/a1/servo");
servoMotors[(int)ServoType.SHOULDER] = robotPHX.QueryDeviceServoMotor(name + "_epaule/a1/servo");
Debug.Assert(servoMotors[0] != null &&
servoMotors[1] != null &&
servoMotors[2] != null);
core = robotPHX.QueryGeom("basPlatine");
Debug.Assert(core != null);
gyro = robotPHX.QueryDeviceAccelGyro("basPlatine/gyro");
Debug.Assert(gyro != null);
}
/*
* Initializes robots devices - motors and sensors.
* Initialization results are posted with corresponding events.
*/
public void InitDevices()
{
LeftMotor = Robot.QueryDeviceMotor(Constants.LEFT_MOTOR);
RightMotor = Robot.QueryDeviceMotor(Constants.RIGHT_MOTOR);
if (MotorsCheckedEvent != null)
{
MotorsCheckedEvent(LeftMotor != null && RightMotor != null);
}
LeftSensor = Robot.QueryDeviceDistance(Constants.RIGHT_SENSOR);
RightSensor = Robot.QueryDeviceDistance(Constants.LEFT_SENSOR);
if (SensorsCheckedEvent != null)
{
SensorsCheckedEvent(LeftSensor != null && RightSensor != null);
}
RobotGeometry = Robot.QueryGeom(Constants.BASE);
}