internal void ArduinoBrickOdometryChanged(RobotAbstraction.IHardwareComponent sender)
{
IOdometry odom = (IOdometry)sender;
//odom.XMeters;
//odom.YMeters;
//odom.ThetaRadians;
lock (currentSensorsDataLock)
{
SensorsDataPlucky sensorsData = new SensorsDataPlucky(this.currentSensorsData);
sensorsData.WheelEncoderLeftTicks = odom.LDistanceTicks;
sensorsData.WheelEncoderRightTicks = odom.RDistanceTicks;
//Debug.WriteLine(sensorsData.ToString());
this.currentSensorsData = sensorsData;
}
}
private void InitDrive()
{
IDifferentialMotorController dmc = hardwareBrick.produceDifferentialMotorController();
driveController = new DifferentialDrive(hardwareBrick)
{
wheelRadiusMeters = WHEEL_RADIUS_METERS,
wheelBaseMeters = WHEEL_BASE_METERS,
encoderTicksPerRevolution = ENCODER_TICKS_PER_REVOLUTION,
speedToVelocityFactor = SPEED_TO_VELOCITY_FACTOR,
turnToOmegaFactor = TURN_TO_OMEGA_FACTOR,
differentialMotorController = dmc
};
this.odometry = hardwareBrick.produceOdometry(ODOMETRY_SAMPLING_INTERVAL_MS);
this.odometry.OdometryChanged += ((SensorsControllerPlucky)sensorsController).ArduinoBrickOdometryChanged;
driveController.hardwareBrickOdometry = odometry;
driveGeometry = (IDifferentialDrive)driveController;
driveController.Init();
driveController.Enabled = true;
}
private void InitDrive()
{
IDifferentialMotorController dmc = hardwareBrick.produceDifferentialMotorController();
driveController = new DifferentialDrive(hardwareBrick)
{
wheelRadiusMeters = WHEEL_RADIUS_METERS,
wheelBaseMeters = WHEEL_BASE_METERS,
encoderTicksPerRevolution = ENCODER_TICKS_PER_REVOLUTION,
speedToVelocityFactor = SPEED_TO_VELOCITY_FACTOR,
turnToOmegaFactor = TURN_TO_OMEGA_FACTOR,
differentialMotorController = dmc
};
this.odometry = hardwareBrick.produceOdometry(ODOMETRY_SAMPLING_INTERVAL_MS);
this.odometry.OdometryChanged += ((SensorsControllerPlucky)sensorsController).ArduinoBrickOdometryChanged;
this.gps = hardwareBrick.produceGps(GPS_SAMPLING_INTERVAL_MS);
this.gps.GpsPositionChanged += ((SensorsControllerPlucky)sensorsController).ArduinoBrickGpsChanged;
this.gps.Enabled = true;
driveController.hardwareBrickOdometry = odometry;
driveGeometry = (IDifferentialDrive)driveController;
driveController.Init();
driveController.Enabled = true;
}
