56 lines
2.1 KiB
C#
56 lines
2.1 KiB
C#
using LinearAlgebra;
|
|
|
|
namespace RoboidControl {
|
|
|
|
// The robot is based on a differential drive
|
|
public class BB2B_Encoder : DifferentialDrive {
|
|
readonly DifferentialDrive drive;
|
|
readonly TouchSensor touchLeft;
|
|
readonly TouchSensor touchRight;
|
|
const float speed = 180.0f; // wheel rotation speed in degrees
|
|
|
|
public BB2B_Encoder(Participant owner) : base(owner) {
|
|
this.name = "BB2B";
|
|
this.SetDriveDimensions(0.064f, 0.128f);
|
|
|
|
// Update the basic motors to motors with encoder
|
|
EncoderMotor leftMotor = new(this, new RelativeEncoder()) {
|
|
position = new Spherical(0.064f, Direction.left)
|
|
};
|
|
EncoderMotor rightMotor = new(this, new RelativeEncoder()) {
|
|
position = new Spherical(0.064f, Direction.right)
|
|
};
|
|
this.SetMotors(leftMotor, rightMotor);
|
|
|
|
// Is has a touch sensor at the front left of the roboid
|
|
touchLeft = new(this) {
|
|
position = Spherical.Degrees(0.12f, -30, 0),
|
|
orientation = new SwingTwist(-30, 0, 0)
|
|
};
|
|
touchRight = new(this) {
|
|
position = Spherical.Degrees(0.12f, 30, 0),
|
|
orientation = new SwingTwist(30, 0, 0)
|
|
};
|
|
}
|
|
|
|
public override void Update(ulong currentTimeMs, bool recurse = true) {
|
|
|
|
// The left wheel turns forward when nothing is touched on the right side
|
|
// and turn backward when the roboid hits something on the right
|
|
float leftWheelVelocity = touchRight.touchedSomething ? -speed : speed;
|
|
|
|
// The right wheel does the same, but instead is controlled by
|
|
// touches on the left side
|
|
float rightWheelVelocity = touchLeft.touchedSomething ? -speed : speed;
|
|
|
|
// When both sides are touching something, both wheels will turn backward
|
|
// and the roboid will move backwards
|
|
|
|
this.SetWheelAngularVelocity(leftWheelVelocity, rightWheelVelocity);
|
|
|
|
base.Update(currentTimeMs, recurse);
|
|
}
|
|
}
|
|
|
|
}
|