47 lines
1.6 KiB
C#
47 lines
1.6 KiB
C#
using LinearAlgebra;
|
|
|
|
namespace RoboidControl {
|
|
|
|
// The robot is based on a differential drive
|
|
public class BB2B : DifferentialDrive {
|
|
readonly DifferentialDrive drive;
|
|
readonly TouchSensor touchLeft;
|
|
readonly TouchSensor touchRight;
|
|
const float speed = 0.5f;
|
|
|
|
public BB2B(Participant owner) : base(owner) {
|
|
this.name = "BB2B";
|
|
this.SetDriveDimensions(0.064f, 0.128f);
|
|
|
|
// 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 leftWheelSpeed = touchRight.touchedSomething ? -speed : speed;
|
|
|
|
// The right wheel does the same, but instead is controlled by
|
|
// touches on the left side
|
|
float rightWheelSpeed = touchLeft.touchedSomething ? -speed : speed;
|
|
|
|
// When both sides are touching something, both wheels will turn backward
|
|
// and the roboid will move backwards
|
|
|
|
this.SetWheelVelocity(leftWheelSpeed, rightWheelSpeed);
|
|
|
|
base.Update(currentTimeMs, recurse);
|
|
}
|
|
}
|
|
|
|
}
|