public Program()
{
Robot robot = new Robot("1", "COM4");
smc = new SimpleMotorController(robot, "COM1", 14);
sdt = new SharpDistanceTracker(robot, 1);
Thread.Sleep(1000);
while (true)
{
if (true)//sdt.ReadSensor() > 200)
{
// read the values controlling the left joystick, map to values from -100 to 100
// set the motor speed magnitude as the 100 * square root of |spd|/100, for one nonlinear joystick to motor speed mapping
smc.motorBrake = 0;
double spd = ((double)(robot.UIAnalogVals[1] - 128) * 100 / (double)128);
if(spd > 0)
smc.motorSpeed = System.Math.Pow((spd / (double)100), .5) * 100;
else
smc.motorSpeed = -1 * System.Math.Pow(((-1*spd) / (double)100), .5) * 100;
}
else
{
smc.motorBrake = 0;
double spd = -50;
smc.motorSpeed = System.Math.Pow((spd / (double)100), .5) * 100;
}
Thread.Sleep(500);
}
}
///program.Run(window);
// Constructor
public StudentCode(Robot robot)
{
this.robot = robot;
smcR = new SimpleMotorController(robot, 13); //let's say 13 is right motor on drivetrain
smcL = new SimpleMotorController(robot, 14); //let's say 14 is left motor on drivetrain
//mm = new MicroMaestro(robot, 12);
timer = 0;
}
// Constructor
public StudentCode(Robot robot)
{
this.robot = robot;
// pot = new AnalogIn(AnalogIn.Pin.Ain0);
// IR = new AnalogIn(AnalogIn.Pin.Ain1);
smcR = new SimpleMotorController(robot, 13); //let's say 13 is right motor on drivetrain
smcL = new SimpleMotorController(robot, 14); //let's say 14 is left motor on drivetrain
// armMotor = new SimpleMotorController(robot, 15); // TODO: Port 15 is the arm motor
distanceS = new SharpDistanceTracker(0, 2); //We are using a long distance sensor
timer = 0;
}
public Robot(string teamID)
{
this.teamID = teamID;
radio = new Radio(this, portName);
// Initialize single motor COM port
motorPort = new SerialPort("COM1", 9600);
motorPort.ReadTimeout = 2;
motorPort.Open();
// Send baudrate autodetect byte (0xAA)
motorPort.Write(new byte[] { (byte)0xAA }, 0, 1);
leftMotor = new SimpleMotorController(this, 13);
rightMotor = new SimpleMotorController(this, 14);
}
/* Constructor
* Takes a robot as argument so that the code
* knows which robot to control (the main method could
* theoretically control multiple robots), and so that
* the robot is accesable
*/
public StudentCode(Robot robot)
{
Button = new InputPort((Cpu.Pin)FEZ_Pin.Digital.IO4, true, Port.ResistorMode.PullUp);
lLightSens = new AnalogIn((AnalogIn.Pin) FEZ_Pin.AnalogIn.An1);
rLightSens = new AnalogIn((AnalogIn.Pin) FEZ_Pin.AnalogIn.An0);
// lLightSens.SetLinearScale(0, 10000);
// rLightSens.SetLinearScale(0, 10000);
this.robot = robot;
leftMotor = new SimpleMotorController(robot, "COM1", 13);
rightMotor = new SimpleMotorController(robot, "COM1", 14); //Check
rightMotor.motorBrake = 0;
leftMotor.motorBrake = 0;
// sdt = new SharpDistanceTracker(robot, 0);
}
// Constructor
public StudentCode(Robot robot)
{
this.robot = robot;
smcR = new SimpleMotorController(robot, 13); //let's say 13 is right motor on drivetrain
smcL = new SimpleMotorController(robot, 14); //let's say 14 is left motor on drivetrain
mm = new MicroMaestro(robot, 12);
encL = new Encoder(robot, 0);
encR = new Encoder(robot, 1);
pidL = new PIDController(KP, KI, KD);
pidR = new PIDController(KP, KI, KD);
sDT1 = new SharpDistanceTracker(0);
sDT2 = new SharpDistanceTracker(1);
sDT3 = new SharpDistanceTracker(2);
sDT4 = new SharpDistanceTracker(3);
sDT5 = new SharpDistanceTracker(4);
}
// Constructor
public StudentCode(Robot robot)
{
this.robot = robot;
smcR = new SimpleMotorController(robot, 13); //let's say 13 is right motor on drivetrain
smcL = new SimpleMotorController(robot, 14); //let's say 14 is left motor on drivetrain
//mm = new MicroMaestro(robot, 12);
switchL = new DigitalSwitch(1);
switchR = new DigitalSwitch(2);
rfL = new SharpDistanceTracker(0);
rfR = new SharpDistanceTracker(1);
rfR1 = 0;
rfR2 = 0;
rfL1 = 0;
rfL2 = 0;
timer = 0;
}