package com.grt192.benchtest.mechanism;
import com.grt192.actuator.GRTJaguar;
import com.grt192.core.Command;
import com.grt192.core.Mechanism;
import com.grt192.mechanism.GRTDriverStation;
/**
*
* @author anand
*/
public class BenchMechanism extends Mechanism {
public static final int WAIT_TIME = 5000;
private GRTJaguar bottomJag1;
private GRTJaguar bottomJag2;
private GRTJaguar bottomJag3;
private GRTJaguar bottomJag4;
private GRTJaguar topJag1;
private GRTJaguar topJag2;
private GRTJaguar topJag3;
private GRTJaguar topJag4;
private GRTJaguar topJag5;
public BenchMechanism() {
bottomJag1 = new GRTJaguar(2);
bottomJag1.start();
bottomJag2 = new GRTJaguar(3);
bottomJag2.start();
bottomJag3 = new GRTJaguar(4);
bottomJag3.start();
bottomJag4 = new GRTJaguar(5);
bottomJag4.start();
topJag1 = new GRTJaguar(6);
topJag1.start();
topJag2 = new GRTJaguar(7);
topJag2.start();
topJag3 = new GRTJaguar(8);
topJag3.start();
topJag4 = new GRTJaguar(9);
topJag4.start();
topJag5 = new GRTJaguar(10);
topJag5.start();
addActuator("top1", topJag1);
addActuator("top2", topJag2);
addActuator("top3", topJag3);
addActuator("top4", topJag4);
addActuator("top5", topJag5);
addActuator("bottom1", bottomJag1);
addActuator("bottom2", bottomJag2);
addActuator("bottom3", bottomJag3);
addActuator("bottom4", bottomJag4);
}
public void testTimedPorts() {
System.out.println("Running 1");
bottomJag1.enqueueCommand(new Command(1.0, WAIT_TIME, true));
try {
Thread.sleep(WAIT_TIME);
} catch (Exception e) {
}
;
System.out.println("Running 2");
bottomJag2.enqueueCommand(new Command(1.0, WAIT_TIME, true));
try {
Thread.sleep(WAIT_TIME);
} catch (Exception e) {
}
System.out.println("Running 3");
bottomJag3.enqueueCommand(new Command(1.0, WAIT_TIME, true));
try {
Thread.sleep(WAIT_TIME);
} catch (Exception e) {
}
System.out.println("Running 4");
bottomJag4.enqueueCommand(new Command(1.0, WAIT_TIME, true));
try {
Thread.sleep(WAIT_TIME);
} catch (Exception e) {
}
System.out.println("Running t1");
topJag1.enqueueCommand(new Command(1.0, WAIT_TIME, true));
try {
Thread.sleep(WAIT_TIME);
} catch (Exception e) {
}
System.out.println("Running t2");
topJag2.enqueueCommand(new Command(1.0, WAIT_TIME, true));
try {
Thread.sleep(WAIT_TIME);
} catch (Exception e) {
}
System.out.println("Running t3");
topJag3.enqueueCommand(new Command(1.0, WAIT_TIME, true));
try {
Thread.sleep(WAIT_TIME);
} catch (Exception e) {
}
System.out.println("Running t4");
topJag4.enqueueCommand(new Command(1.0, WAIT_TIME, true));
try {
Thread.sleep(WAIT_TIME);
} catch (Exception e) {
}
System.out.println("Running t5");
topJag5.enqueueCommand(new Command(1.0, WAIT_TIME, true));
try {
Thread.sleep(WAIT_TIME);
} catch (Exception e) {
}
}
}