void Main() {
InitBlockHelper();
InitMem();
Tracker.Run();
WriteMem();
BlockHelper.RunAction("Timer Block", "TriggerNow");
}
class Tracker{
private const float SENSI = 60;
private const int DELAY_TIMES = 6;
private const string gyroName = "Gyroscope";
private IMyGyro Gyro = (IMyGyro)BlockHelper.GetBlock(gyroName);
private IMySensorBlock Sensor = (IMySensorBlock)BlockHelper.GetBlock("Sensor");
//These Directions is base on the angle of view when you face to ship, not base on ship
private string[][] sensorSets = {new string[]{"50", "50", "50", "50", "01", "50"}, //Top L R T B B F
new string[]{"01", "50", "50", "50", "50", "50"}, //Right
new string[]{"50", "50", "50", "50", "50", "01"}, //Bottom
new string[]{"50", "01", "50", "50", "50", "50"}};//Left
private int watchDirectionIndex; //0:T 1:R 2:B 3:L
private int topDetectedCount;
private int rightDetectedCount;
private int bottomDetectedCount;
private int leftDetectedCount;
private static Tracker tracker;
private Tracker() {}
private void initTracker(){
this.topDetectedCount = Mem.GetInt("topDetectedCount");
this.rightDetectedCount = Mem.GetInt("rightDetectedCount");
this.bottomDetectedCount = Mem.GetInt("bottomDetectedCount");
this.leftDetectedCount = Mem.GetInt("leftDetectedCount");
this.watchDirectionIndex = Mem.GetInt("watchDirectionIndex");
}
static public void Run(){
if (tracker == null)
tracker = new Tracker();
if (tracker.NeedWait())
return;
tracker.initTracker();
tracker.Track();
}
public void Track(){
UpdateDirectionState();
SetGyro();
SwitchWatchDirection();
}
private void UpdateDirectionState(){
if (IsSensorDetected()) {
switch(Convert.ToString(watchDirectionIndex)) {
case "0":
Mem.SetInt("topDetectedCount", ++topDetectedCount);
break;
case "1":
Mem.SetInt("rightDetectedCount", ++rightDetectedCount);
break;
case "2":
Mem.SetInt("bottomDetectedCount", ++bottomDetectedCount);
break;
case "3":
Mem.SetInt("leftDetectedCount", ++leftDetectedCount);
break;
}
}
else {
switch(Convert.ToString(watchDirectionIndex)) {
case "0":
topDetectedCount = 0;
Mem.SetInt("topDetectedCount", topDetectedCount);
break;
case "1":
rightDetectedCount = 0;
Mem.SetInt("rightDetectedCount", rightDetectedCount);
break;
case "2":
bottomDetectedCount = 0;
Mem.SetInt("bottomDetectedCount", bottomDetectedCount);
break;
case "3":
leftDetectedCount = 0;
Mem.SetInt("leftDetectedCount", leftDetectedCount);
break;
}
}
}
private void SetGyro(){
if (topDetectedCount > 0 && bottomDetectedCount > 0) {
ResetXAxis();
}
else if (topDetectedCount > 0) {
TurnToTop();
}
else if (bottomDetectedCount > 0) {
TurnToBottom();
}
if (rightDetectedCount > 0 && leftDetectedCount > 0) {
ResetYAxis();
}
else if (rightDetectedCount > 0) {
TurnToRight();
}
else if (leftDetectedCount > 0) {
TurnToLeft();
}
}
private void SwitchWatchDirection(){
int index = watchDirectionIndex;//Mem.GetInt("watchDirectionIndex");
index = ++index % 4;
Sensor.SetValue("Left", SensorSet(index, 0));
Sensor.SetValue("Right", SensorSet(index, 1));
Sensor.SetValue("Top", SensorSet(index, 2));
Sensor.SetValue("Bottom", SensorSet(index, 3));
Sensor.SetValue("Back", SensorSet(index, 4));
Sensor.SetValue("Front", SensorSet(index, 5));
Mem.SetInt("watchDirectionIndex", index);
}
private bool NeedWait(){
int currentTimes = Mem.GetInt("currentTimes");
if (currentTimes < DELAY_TIMES) {
++currentTimes;
}
else{
currentTimes = 0;
}
Mem.SetInt("currentTimes", currentTimes);
return currentTimes != 0;
}
private bool IsSensorDetected() {
return Sensor.IsActive;
}
private void ResetXAxis() {
Gyro.SetValue("Pitch", 0F);
}
private void ResetYAxis() {
Gyro.SetValue("Yaw", 0F);
}
private void TurnToTop() {
Gyro.SetValue("Pitch", SENSI);
}
private void TurnToBottom() {
Gyro.SetValue("Pitch", -SENSI);
}
private void TurnToLeft() {
Gyro.SetValue("Yaw", SENSI);
}
private void TurnToRight() {
Gyro.SetValue("Yaw", -SENSI);
}
private float SensorSet(int index, int i) {
return Convert.ToSingle(sensorSets[index][i]);
}
}