/// <summary>
/// this method initializes all needed motors and sensors
/// to control the robot accordingly
/// </summary>
public static void InitRobot()
{
AddEmergencyStopOption();
LeftTrack = new Motor(Constants.LEFT_TRACK_PORT);
RightTrack = new Motor(Constants.RIGHT_TRACK_PORT);
GrapplerRiserMotor = new Motor(Constants.GRAPPLER_RISER_PORT);
GrapplerWheelMotor = new Motor(Constants.GRAPPLER_WHEEL_PORT);
ColorSensor = new EV3ColorSensor(Constants.EV3_COLOR_SENSOR_PORT);
IRSensor = new EV3IRSensor(Constants.IR_SENSOR_PORT);
UltraSonicSensor = new EV3UltrasonicSensor(Constants.ULTRASONIC_SENSOR_PORT);
ColorSensor.Mode = ColorMode.RGB;
UltraSonicSensor.Mode = UltraSonicMode.Centimeter;
GrapplerPosition = GrapplerPosition.Down;
FoodState = FoodState.Searching;
IsInitialized = true;
WaitToFullyBootProgram();
CalibrizeGrappler();
PrintEmptyLine();
WaitForStartButtonPress();
TeamMode = TeamMode.WinnieTeam;
//InitColour();
}
public static void Main(string[] args)
{
EventWaitHandle stopped = new ManualResetEvent(false);
ColorMode[] modes = { ColorMode.Color, ColorMode.Reflection, ColorMode.Ambient, ColorMode.Blue };
int modeIdx = 0;
var sensor = new EV3ColorSensor(SensorPort.In1);
ButtonEvents buts = new ButtonEvents();
LcdConsole.WriteLine("Use color on port1");
LcdConsole.WriteLine("Up read value");
LcdConsole.WriteLine("Down raw value");
LcdConsole.WriteLine("Enter change mode");
LcdConsole.WriteLine("Esc. terminate");
buts.EscapePressed += () => {
stopped.Set();
};
buts.UpPressed += () => {
LcdConsole.WriteLine("Sensor value: " + sensor.ReadAsString());
};
buts.DownPressed += () => {
LcdConsole.WriteLine("Raw sensor value: " + sensor.ReadRaw());
};
buts.EnterPressed += () => {
modeIdx = (modeIdx + 1) % modes.Length;
sensor.Mode = modes[modeIdx];
LcdConsole.WriteLine("Sensor mode is set to: " + modes[modeIdx]);
};
stopped.WaitOne();
}
public override void Init()
{
this.proc = new Thread(new ThreadStart(Loop));
this.dist = new IRSensor(SensorPort.In1, IRMode.Proximity);
this.color = new EV3ColorSensor(SensorPort.In2, ColorMode.Color);
}
public async Task Setup()
{
// Set brick
_brick = new Brick();
// Set Smacker ;)
_smackMotor = new Motor(_brick, Iot.Device.BrickPi3.Models.BrickPortMotor.PortB);
//_smackMotor.PropertyChanged += HandleTachoPropertyChangedEvent;
// Set ultrasonic
_sonic = new EV3UltraSonicSensor(_brick, SensorPort.Port1, UltraSonicMode.Centimeter);
_sonic.PropertyChanged += HandleSonicSmackPropertyChangedEvent;
// Set movement motors
_leftMotor = new Motor(_brick, Iot.Device.BrickPi3.Models.BrickPortMotor.PortC);
_rightMotor = new Motor(_brick, Iot.Device.BrickPi3.Models.BrickPortMotor.PortD);
// Gyro
_gyro = new EV3GyroSensor(_brick, SensorPort.Port3);
_gyro.Mode = GyroMode.Angle;
//_gyro.PropertyChanged += HandleGyroMovementPropertyChangedEvent;
// Color
_color = new EV3ColorSensor(_brick, SensorPort.Port2, ColorSensorMode.Color, 20);
_color.PropertyChanged += HandleMovementColorPropertyChangedEvent;
// Touch
_touch = new EV3TouchSensor(_brick, SensorPort.Port4, 20);
_touch.PropertyChanged += HandleHit;
}
public static void Main(string[] args)
{
var sensor = new EV3ColorSensor(SensorPort.In1);
sensor.Mode = ColorMode.RGB;
Motor motorA = new Motor(MotorPort.OutA);
Motor motorB = new Motor(MotorPort.OutB);
Motor motorD = new Motor(MotorPort.OutD);
string solution;
Cube cube = new Cube(motorA, motorB, motorD); // instantiate a real cube
// Cube cube = new Cube (); // virtual cube used for tests
// Solver.Randomizer (cube, 5000); // scramble the virtual cube
DateTime starttime = DateTime.Now;
TimeSpan elapsedtime = new TimeSpan();
MoveCube.BuildCube2P(cube, MoveCube.BuildCube(cube, motorA, motorB, motorD, sensor));
elapsedtime = DateTime.Now - starttime;
Console.WriteLine(cube.GetCubeMap());
Console.WriteLine(cube.ToString());
Console.WriteLine("Elapsed time scanning cube: {0}", elapsedtime);
starttime = DateTime.Now;
// uncomment line below if you want to use the internal algorithm to solve the cube.
// It's the easiest way, but also the slowest.
// solution = Solver.Solve (cube);
// comment the following block if you intend to use the internal algorithm to solve
// the cube. By default the main program tries to read the solution from a solution
// file. Just run the Kociemba algorithm on a pc and use scp, putty or any other ssh
// tool to send the file to the intelligent Lego brick.
Communication.ClearCube(Communication.CubePath);
Communication.ClearCube(Communication.SolutionPath);
Communication.SaveCube(Communication.CubePath, cube.ToString());
solution = Communication.ReceiveSolution();
elapsedtime = DateTime.Now - starttime;
Console.WriteLine("Solution: {0}", solution);
Console.WriteLine("Elapsed time receiving solution: {0}", elapsedtime);
starttime = DateTime.Now;
Solver.TranslateMove(solution, cube);
Console.WriteLine(cube.GetCubeMap());
elapsedtime = DateTime.Now - starttime;
Console.WriteLine("Time elapsed moving cube: {0}", elapsedtime);
Communication.ClearCube(Communication.CubePath);
Communication.ClearCube(Communication.SolutionPath);
}
public ScannerRobot() :
base(new MotorPort[] { RobotSetup.XPort, RobotSetup.YPort, RobotSetup.PenPort })
{
_resetSensor = new EV3TouchSensor(RobotSetup.XResetPort);
_colorSensor = new EV3ColorSensor(RobotSetup.ColorPort, ColorMode.RGB);
_scanThread = new Thread(ScanPollThread);
_scanThread.IsBackground = true;
_scanThread.Start();
}
public override void Init()
{
this.proc = new Thread(new ThreadStart(Loop));
this.move = new Movement();
this.touch = new TouchSensor(SensorPort.In4);
this.color = new EV3ColorSensor(SensorPort.In2);
this.dist = new IRSensor(SensorPort.In1, IRMode.Proximity);
this.speak = new Speaker(100);
this.rand = new Random();
}
public static RGBColor[] CountFacelets(Motor a, EV3ColorSensor sensor)
{
RGBColor[] colors = new RGBColor[8];
a.ResetTacho();
for (int count = 0; count < 8;)
{
if (count == a.GetTachoCount() / 45 % 8)
{
colors [count++] = sensor.ReadRGB();
}
}
return(colors);
}
private async Task TestEV3Color()
{
//brick.Stop();
//brick.SetTimeout(250);
EV3ColorSensor nxtlight = new EV3ColorSensor(BrickPortSensor.PORT_S4, ColorSensorMode.Reflection);
EV3TouchSensor touch = new EV3TouchSensor(BrickPortSensor.PORT_S1);
//brick.Stop();
//brick.SetupSensors();
RGBColor rgb;
await Task.Delay(5000);
for (int i = 0; i < nxtlight.NumberOfModes(); i++)
{
int count = 0;
while ((count < 100) && !touch.IsPressed())
{
//Debug.WriteLine(string.Format("NXT Touch, Raw: {0}, ReadASString: {1}, IsPressed: {2}, NumberNodes: {3}, SensorName: {4}", touch.ReadRaw(), touch.ReadAsString(), touch.IsPressed(), touch.NumberOfModes(), touch.GetSensorName()));
//Debug.WriteLine(string.Format("EV3 Touch, Raw: {0}, ReadASString: {1}, IsPressed: {2}, NumberNodes: {3}, SensorName: {4}", ev3Touch.ReadRaw(), ev3Touch.ReadAsString(), ev3Touch.IsPressed(), ev3Touch.NumberOfModes(), ev3Touch.GetSensorName()));
//Debug.WriteLine(string.Format("NXT Sound, Raw: {0}, ReadASString: {1}, NumberNodes: {2}, SensorName: {3}", sound.ReadRaw(), sound.ReadAsString(), sound.NumberOfModes(), sound.GetSensorName()));
//brick.UpdateValues();
Debug.WriteLine(string.Format("EV3 Color Sensor, Raw: {0}, ReadASString: {1}",
nxtlight.ReadRaw(), nxtlight.ReadAsString()));
rgb = nxtlight.ReadRGBColor();
Debug.WriteLine(string.Format("Color: {0}, Red: {1}, Green: {2}, Blue: {3}",
nxtlight.ReadColor(), rgb.Red, rgb.Green, rgb.Blue));
//brick.Stop();
//brick.Start();
//nxtlight.ColorMode = ColorSensorMode.Ambient;
await Task.Delay(1000);
//if ((touch.IsPressed()) && ev3Touch.IsPressed())
count++;
//nxtlight.ColorMode = ColorSensorMode.Color;
}
if (nxtlight.ColorMode == ColorSensorMode.Reflection)
{
nxtlight.ColorMode = ColorSensorMode.Color;
}
else
{
nxtlight.ColorMode = ColorSensorMode.Reflection;
}
//brick.SetupSensors();
await Task.Delay(5000);
}
}
public static void Main(string[] args)
{
const string to = "*****@*****.**";
const string from = "*****@*****.**";
const string password = "******";
ManualResetEvent terminateProgram = new ManualResetEvent(false);
var colorSensor = new EV3ColorSensor(SensorPort.In1);
ButtonEvents buts = new ButtonEvents();
SmtpClient smptpClient = new SmtpClient("smtp.gmail.com", 587);
smptpClient.EnableSsl = true;
smptpClient.UseDefaultCredentials = false;
smptpClient.Credentials = new NetworkCredential(from, password);
smptpClient.DeliveryMethod = SmtpDeliveryMethod.Network;
ServicePointManager.ServerCertificateValidationCallback =
delegate(object s, X509Certificate certificate, X509Chain chain, SslPolicyErrors sslPolicyErrors)
{ return(true); };
MailMessage message = new MailMessage();
message.To.Add(to);
message.From = new MailAddress(from);
message.Subject = "Color mail from my EV3";
LcdConsole.Clear();
LcdConsole.WriteLine("Use color on port1");
LcdConsole.WriteLine("Enter send mail");
LcdConsole.WriteLine("Esc. terminate");
buts.EscapePressed += () => {
terminateProgram.Set();
};
buts.EnterPressed += () => {
LcdConsole.WriteLine("Sending email");
try{
message.Body = "EV3 read color: " + colorSensor.ReadColor();
smptpClient.Send(message);
LcdConsole.WriteLine("Done sending email");
}
catch (Exception e)
{
LcdConsole.WriteLine("Failed to send email");
Console.WriteLine(e.StackTrace);
}
};
terminateProgram.WaitOne();
message = null;
}
public async Task TestColorSensor()
{
EV3ColorSensor color = new EV3ColorSensor(_brick, SensorPort.Port2, ColorSensorMode.Blue);
color.PropertyChanged += HandleColorPropertyChangedEvent;
while (false)
{
try
{
if (color.ColorMode == ColorSensorMode.Color)
{
Console.WriteLine($"{color.ReadColor()}");
}
else
{
Console.WriteLine(color.CalculateRawAverage());
/*
* var rgb = color.ReadRGBValues();
*
* Console.WriteLine($"Red: {rgb.Red}");
* Console.WriteLine($"Blue: {rgb.Green}");
* Console.WriteLine($"Green: {rgb.Blue}");
* Console.WriteLine($"Ambient: {rgb.Blue}");
*
* var rgb2 = color.ReadRGBColor();
*
* Console.WriteLine($"Red2: {rgb.Red}");
* Console.WriteLine($"Blue2: {rgb.Green}");
* Console.WriteLine($"Green2: {rgb.Blue}");
*/
}
}
catch (System.Exception ex)
{
Console.WriteLine($"{ex.Message}");
}
await Task.Delay(1000);
}
}
public static void Main(string[] args)
{
InfoDialog dialog = new InfoDialog("Misie 2 + 3"); //print iets op het lcd scherm van de brick
dialog.Show(); //wacht todat op ok is gedrukt
Motor motorR = new Motor(MotorPort.OutB); //definieeren van de rechtermotor
Motor motorL = new Motor(MotorPort.OutA); //definieeren van de linkermotor
Motor motorU = new Motor(MotorPort.OutC); //definieeren van de robot arm motor
EV3ColorSensor sensor = new EV3ColorSensor(SensorPort.In4); //definieeren van de kleurensensor
EV3GyroSensor gyroSensor = new EV3GyroSensor(SensorPort.In2, GyroMode.Angle); //definieeren van de gyrosensor
//ga er vanuit dat deze functies namen logisch genoeg zijn en geen comments nodig hebben
DriveStraight(4000, 30, motorR, motorL);
DriveToColor(25, "White", motorR, motorL, sensor);
Turning(gyroSensor, -86, motorL, motorR);
DriveStraight(2500, 30, motorR, motorL);
Turning(gyroSensor, -86, motorL, motorR);
DriveStraight(2000, 25, motorR, motorL);
ArmUp(1500, -20, motorU);
DriveStraight(100, -25, motorR, motorL);
ArmUp(600, -20, motorU);
DriveStraight(2000, -25, motorR, motorL);
Thread.Sleep(2000);
ArmUp(200, 20, motorU);
DriveStraight(700, 25, motorR, motorL);
DriveStraight(700, -25, motorR, motorL);
Turning(gyroSensor, 86, motorL, motorR);
DriveStraight(2170, 30, motorL, motorR);
Turning(gyroSensor, -90, motorL, motorR);
DriveStraight(5500, 45, motorL, motorR);
Turning(gyroSensor, -82, motorL, motorR);
DriveStraight(6000, 30, motorL, motorR);
LcdConsole.WriteLine("shutdown");
Shutdown(motorL, motorR); //afsluiten van het programma
}
/// <summary>
/// // Field initialization
/// </summary>
private void init()
{
// Connected sensors
colorSensor = new EV3ColorSensor(SensorPort.In1, ColorMode.Reflection);
// Motors
steerWheel = new Motor(MotorPort.OutA);
rightEngine = new Motor(MotorPort.OutB);
leftEngine = new Motor(MotorPort.OutC);
rightEngine.Off();
leftEngine.Off();
steerWheel.Off();
steerWheel.ResetTacho();
// SteeringWheel motor parameters
gearRatio = 3;
steerPower = 30;
currentSteeringAngle = 0;
// PositionPID
steerPID = new MediumMotorPositionPID();
steerPID.Motor = steerWheel;
steerPID.MaxPower = steerPower;
steerPID.MinPower = (sbyte)-steerPower;
// Sensors update thread
stopSensorUpdate = new ManualResetEvent(false);
sensorUpdateSamplingTime = 50;
sensorUpdateThread = new Thread(SensorUpdateThread);
// Drive thread
stopDrive = new ManualResetEvent(false);
driveSamplingTime = 100;
driveThread = new Thread(DriveThread);
stopLCDThread = new ManualResetEvent(false);
lCDThreadSampleTime = 250;
lCDThread = new Thread(LCDThread);
}
static private void TestEV3Color()
{
Console.WriteLine("EV3 sensor color test mode");
EV3ColorSensor nxtlight = new EV3ColorSensor(_brick, SensorPort.Port2, ColorSensorMode.Green);
EV3TouchSensor touch = new EV3TouchSensor(_brick, SensorPort.Port1);
RGBColor rgb;
Thread.Sleep(5000);
for (int i = 0; i < nxtlight.NumberOfModes(); i++)
{
int count = 0;
while ((count < 100) && !touch.IsPressed())
{
Console.WriteLine($"EV3 Color Sensor, Raw: {nxtlight.ReadRaw()}, ReadASString: {nxtlight.ReadAsString()}");
rgb = nxtlight.ReadRGBColor();
Console.WriteLine($"Color: {nxtlight.ReadColor()}, Red: {rgb.Red}, Green: {rgb.Green}, Blue: {rgb.Blue}");
Thread.Sleep(1000);
count++;
}
nxtlight.SelectNextMode();
Thread.Sleep(5000);
}
}
public static void Main(string[] args)
{
ManualResetEvent terminateProgram = new ManualResetEvent(false);
string fbToken = "CAACpSl1Qm3cBAHAMyZAY...";//This is not valied
var fb = new FacebookClient(fbToken);
Font f = Font.MediumFont;
Point offset = new Point(0, 25);
Point p = new Point(10, Lcd.Height - 75);
Point boxSize = new Point(100, 24);
Rectangle box = new Rectangle(p, p + boxSize);
var colorSensor = new EV3ColorSensor(SensorPort.In1);
ButtonEvents buts = new ButtonEvents();
LcdConsole.WriteLine("Use color on port1");
LcdConsole.WriteLine("Enter post value");
LcdConsole.WriteLine("Esc. terminate");
buts.EscapePressed += () => {
terminateProgram.Set();
};
buts.EnterPressed += () => {
Color color = colorSensor.ReadColor();
Lcd.Clear();
Lcd.WriteTextBox(f, box + offset * 0, "Color: " + color, true);
Lcd.WriteTextBox(f, box + offset * 1, "Send to Facebook" + color, true);
Lcd.Update();
colorSensor.ReadColor();
var me = fb.Get("monobrick.dk") as JsonObject;
var uid = me["id"];
string url = string.Format("{0}/{1}", uid, "feed");
var argList = new Dictionary <string, object>();
argList["message"] = "A program running MonoBrick Firmware was aked to read the color sensor. Color read: " + colorSensor.ReadColor().ToString();
fb.Post(url, argList);
};
terminateProgram.WaitOne();
}
static void Main(string[] args)
{
ButtonEvents buttons = new ButtonEvents();
TcpListener server = null;
TcpClient client = null;
bool run = true;
buttons.EscapePressed += () =>
{
if (server != null)
{
server.Stop();
}
if (client != null)
{
client.Close();
}
run = false;
};
Log("KinematicServer 1.5");
float mainRatio = 46.667f;
float secondaryRatio = 40.0f;
float handRatio = 8f;
#if DUMPLOGS
StringBuilder logs = new StringBuilder();
#endif
using (RobotMotors motors = new RobotMotors(MotorPort.OutA, MotorPort.OutB, MotorPort.OutC)
{
MainMotorRatio = mainRatio, SecondaryMotorRatio = secondaryRatio, HandMotorRatio = handRatio
})
{
EV3TouchSensor sensor1 = new EV3TouchSensor(SensorPort.In1);
EV3ColorSensor sensor2 = new EV3ColorSensor(SensorPort.In2);
sensor2.Mode = ColorMode.Reflection;
int sensor2max = -1;
int pause = 0;
buttons.EnterReleased += () =>
{
pause++;
};
int currentPause = 0;
motors.Calibrate((RobotMotors.CalibrationSteps step) =>
{
if (!run)
{
return(true);
}
switch (step)
{
case RobotMotors.CalibrationSteps.Main:
if (sensor1.IsPressed())
{
return(true);
}
break;
case RobotMotors.CalibrationSteps.Pause:
if (currentPause != pause)
{
currentPause = pause;
return(true);
}
break;
case RobotMotors.CalibrationSteps.Secondary:
int sensor2reading = sensor2.ReadRaw();
if (sensor2reading > 19 &&
sensor2reading < sensor2max) // moving away from sweet spot (but remove noise)
{
#if DUMPLOGS
logs.Append(string.Format("{0}:{1}:{2}\n\n", motors.GetRawTacho(MotorPort.OutB), sensor2reading, sensor2max));
logs.Append("-----------------\n\n");
#endif
return(true);
}
else if (sensor2reading > sensor2max)
{
sensor2max = sensor2reading; // keep going when it increases
}
#if DUMPLOGS
logs.Append(string.Format("{0}:{1}:{2}\n\n", motors.GetRawTacho(MotorPort.OutB), sensor2reading, sensor2max));
#endif
break;
case RobotMotors.CalibrationSteps.SecondaryReset:
sensor2max = -1;
#if DUMPLOGS
logs.Append("***********************\n\n");
#endif
break;
}
/*
* Lcd.Clear();
* int line = 0;
* Lcd.WriteText(Font.MediumFont, new Point(0, line), "Calibrating...", true);
* line += (int)(Font.MediumFont.maxHeight);
* Lcd.WriteText(Font.MediumFont, new Point(0, line), string.Format("Refl.: {0} / {1}", sensor2.ReadRaw(), sensor2max), true);
* line += (int)(Font.MediumFont.maxHeight);
* Lcd.WriteText(Font.MediumFont, new Point(0, line), string.Format("A: {0}", motors.GetRawTacho(MotorPort.OutA)), true);
* line += (int)(Font.MediumFont.maxHeight);
* Lcd.WriteText(Font.MediumFont, new Point(0, line), string.Format("B: {0}", motors.GetRawTacho(MotorPort.OutB)), true);
* line += (int)(Font.MediumFont.maxHeight);
* Lcd.Update();
*/
return(false);
});
}
// stop here
if (!run)
{
return;
}
// main loop
Lcd.Clear();
Lcd.Update();
Log("Starting...");
try
{
using (RobotMotors motors = new RobotMotors(MotorPort.OutA, MotorPort.OutB, MotorPort.OutC)
{
MainMotorRatio = mainRatio, SecondaryMotorRatio = secondaryRatio, HandMotorRatio = handRatio
})
{
// Set the TcpListener on port 13000.
Int32 port = 13000;
// TcpListener server = new TcpListener(port);
server = new TcpListener(IPAddress.Any, port);
// Start listening for client requests.
server.Start();
// Buffer for reading data
Byte[] bytes = new Byte[256];
String data = null;
// Enter the listening loop.
while (run)
{
Log("Waiting for a connection... ");
// Perform a blocking call to accept requests.
// You could also user server.AcceptSocket() here.
client = server.AcceptTcpClient();
Log("Connected!");
#if DUMPLOGS
// DEBUG
byte[] logBuffer = Encoding.ASCII.GetBytes(logs.ToString());
client.GetStream().Write(logBuffer, 0, logBuffer.Length);
#endif
motors.MainMotorSpeed = 64;
motors.SecondaryMotorSpeed = 64;
motors.HandMotorSpeed = 127;
data = null;
// Get a stream object for reading and writing
NetworkStream stream = client.GetStream();
int read;
string message = "";
// Loop to receive all the data sent by the client.
int commandCount = 0;
while (run && (read = stream.Read(bytes, 0, bytes.Length)) != 0)
{
// Translate data bytes to a ASCII string.
data = System.Text.Encoding.ASCII.GetString(bytes, 0, read);
for (int i = 0; i < read; i++)
{
char c = data[i];
if (c != '\0')
{
message += c;
}
else
{
String[] rawCommand = message.Split(';');
// get message type
switch (rawCommand[0])
{
case "UP":
motors.Queue(new HandCommand(true));
commandCount = 0;
break;
case "DWN":
motors.Queue(new HandCommand(false));
commandCount = 0;
break;
case "MOV":
commandCount++;
float mainRotation = float.Parse(rawCommand[1]);
float secondaryRotation = float.Parse(rawCommand[2]);
motors.Queue(
new MoveCommand {
MainRotation = (int)Math.Round(mainRotation, MidpointRounding.AwayFromZero),
SecondaryRotation = (int)Math.Round(secondaryRotation, MidpointRounding.AwayFromZero)
});
break;
}
//
Lcd.Clear();
int line = 0;
Lcd.WriteText(Font.MediumFont, new Point(0, line), string.Format("Count: {0}", commandCount), true);
line += (int)(Font.MediumFont.maxHeight);
Lcd.WriteText(Font.MediumFont, new Point(0, line), string.Format("Last: {0}", message), true);
line += (int)(Font.MediumFont.maxHeight);
Lcd.Update();
//
message = "";
}
}
}
}
}
}
catch (Exception ex)
{
Log(ex.Message);
if (client.Connected)
{
byte[] buffer = Encoding.ASCII.GetBytes(ex.Message);
client.GetStream().Write(buffer, 0, buffer.Length);
}
throw;
}
finally
{
// Stop listening for new clients.
server.Stop();
}
}
/*
* This method controls the movements of the robot to position
* each face of the cube to be scanned by the color sensor.
*/
public static char[] BuildCube(Cube cube, Motor motorA, Motor motorB, Motor motorD, EV3ColorSensor sensor)
{
char[] faces = new char[6];
// Position face in cube
for (int j = 0; j < 6; j++)
{
switch (j)
{
case 0:
BuildFace(cube.face[3], motorA, motorB, sensor);
cube.face [3].TurnCWCore();
cube.face [3].TurnCWCore();
MoveCube.Move(motorD, MoveCube.GrabArm);
MoveCube.Move(motorD, MoveCube.PullArm);
MoveCube.Move(motorD, MoveCube.RestArm);
break;
case 1:
BuildFace(cube.face[2], motorA, motorB, sensor);
cube.face [2].TurnCWCore();
MoveCube.Move(motorD, MoveCube.GrabArm);
MoveCube.Move(motorD, MoveCube.PullArm);
MoveCube.Move(motorD, MoveCube.RestArm);
break;
case 2:
BuildFace(cube.face[1], motorA, motorB, sensor);
MoveCube.MoveRel(motorA, MoveCube.CCW90);
MoveCube.Move(motorD, MoveCube.GrabArm);
MoveCube.Move(motorD, MoveCube.PullArm);
MoveCube.Move(motorD, MoveCube.RestArm);
break;
case 3:
BuildFace(cube.face[0], motorA, motorB, sensor);
cube.face [0].TurnCCWCore();
MoveCube.MoveRel(motorA, MoveCube.CW90);
MoveCube.Move(motorD, MoveCube.GrabArm);
MoveCube.Move(motorD, MoveCube.PullArm);
MoveCube.Move(motorD, MoveCube.RestArm);
break;
case 4:
BuildFace(cube.face [4], motorA, motorB, sensor);
MoveCube.Move(motorD, MoveCube.GrabArm);
MoveCube.Move(motorD, MoveCube.PullArm);
MoveCube.Move(motorD, MoveCube.RestArm);
break;
case 5:
BuildFace(cube.face [5], motorA, motorB, sensor);
break;
}
}
for (int i = 0; i < 6; i++)
{
faces [i] = cube.face [i].square [1, 1];
}
return(faces);
}
/*
* This method controls the movements of the robot to scan the facelets
* of any single face of the cube.
*/
public static void BuildFace(Face face, Motor motorA, Motor motorB, EV3ColorSensor sensor)
{
RGBColor[] colors = null;
RGBColor color = null;
Thread count = new Thread(() => colors = CountFacelets(motorA, sensor));
MoveCube.Move(motorB, MoveCube.SensorMid, 15);
color = sensor.ReadRGB();
MoveCube.Move(motorB, MoveCube.SensorSide, 15);
count.Start();
while (!count.IsAlive)
{
;
}
WaitHandle handle = motorA.SpeedProfile((sbyte)100, 0, (uint)360, 0, true);
handle.WaitOne();
count.Join();
MoveCube.Move(motorB, MoveCube.SensorRest, 15);
char colorvalue = ' ';
for (int i = 0; i < 9; i++)
{
switch (i)
{
case 0:
colorvalue = getColorValue(color);
face.square[1, 1] = colorvalue;
break;
case 1:
color = colors[0];
colorvalue = getColorValue(color);
face.square[2, 1] = colorvalue;
break;
case 3:
color = colors[2];
colorvalue = getColorValue(color);
face.square[1, 2] = colorvalue;
break;
case 5:
color = colors[4];
colorvalue = getColorValue(color);
face.square[0, 1] = colorvalue;
break;
case 7:
color = colors[6];
colorvalue = getColorValue(color);
face.square[1, 0] = colorvalue;
break;
case 2:
color = colors[1];
colorvalue = getColorValue(color);
face.square[2, 2] = colorvalue;
break;
case 4:
color = colors[3];
colorvalue = getColorValue(color);
face.square[0, 2] = colorvalue;
break;
case 6:
color = colors[5];
colorvalue = getColorValue(color);
face.square[0, 0] = colorvalue;
break;
case 8:
color = colors[7];
colorvalue = getColorValue(color);
face.square[2, 0] = colorvalue;
break;
}
Console.WriteLine("Line: {0} Color: {1} RGB code: {2}", i, colorvalue, color);
}
}
public static void Main(string[] args)
{
//AutoFin->false
ManualResetEvent terminateProgram = new ManualResetEvent(false);
//Motor_Rady
LcdConsole.WriteLine("***Motor_Rady***");
Motor motorA = new Motor(MotorPort.OutA);
Motor motorD = new Motor(MotorPort.OutD);
motorA.Off();
motorD.Off();
motorA.ResetTacho();
motorD.ResetTacho();
//Vehicle_Rady
Vehicle vehicle = new Vehicle(MotorPort.OutA, MotorPort.OutD);
WaitHandle waitHandle;
vehicle.ReverseLeft = false;
vehicle.ReverseRight = false;
int b = 0;
//Sensor_Rady
LcdConsole.WriteLine("***Sensor_Rady***");
//Touch_Rady
var touchSensor1 = new EV3TouchSensor(SensorPort.In1);
var touchSensor2 = new EV3TouchSensor(SensorPort.In4);
//UltraSonic_Rady
var UltraSonicSensor = new EV3UltrasonicSensor(SensorPort.In3, UltraSonicMode.Centimeter);
//Color_Rady
EventWaitHandle stopped = new ManualResetEvent(false);
ColorMode[] modes = { ColorMode.Color, ColorMode.Reflection, ColorMode.Ambient, ColorMode.Blue };
int modeIdx = 0;
var sensor = new EV3ColorSensor(SensorPort.In2, ColorMode.Reflection);
//Conection
LcdConsole.WriteLine("***Conect_Rady***");
//Http
Encoding enc = Encoding.UTF8;
string input = "Detect";
string url = "http://nursinghomeexplorer.azurewebsites.net/index.php";
string param = "";
//
Hashtable ht = new Hashtable();
ht["langpair"] = "#ja";
ht["hl"] = "en";
ht["text"] = HttpUtility.UrlEncode(input, enc);
foreach (string k in ht.Keys)
{
param += String.Format("{0}={1}&", k, ht[k]);
}
byte[] data = Encoding.ASCII.GetBytes(param);
//
HttpWebRequest req = (HttpWebRequest)WebRequest.Create(url);
req.Method = "POST";
req.ContentType = "application/x-www-form-urlencoded";
req.ContentLength = data.Length;
LcdConsole.WriteLine("***Conected!***");
//timer set
System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();
long a = 0;
//buts
ButtonEvents buts = new ButtonEvents();
LcdConsole.WriteLine("Up read BrightLine");
LcdConsole.WriteLine("Down read Floor");
//LcdConsole.WriteLine ("Left Exit program");
//LcdConsole.WriteLine ("Right Exit program");
LcdConsole.WriteLine("Enter change color mode");
LcdConsole.WriteLine("Esc. StartRun");
buts.UpPressed += () =>
{
LcdConsole.WriteLine("Sensor value: " + sensor.ReadAsString());
};
buts.DownPressed += () =>
{
LcdConsole.WriteLine("Raw sensor value: " + sensor.ReadRaw());
};
buts.EnterPressed += () =>
{
modeIdx = (modeIdx + 1) % modes.Length;
sensor.Mode = modes[modeIdx];
LcdConsole.WriteLine("Sensor mode is set to: " + modes[modeIdx]);
};
//Left_Finish
buts.LeftPressed += () => {
terminateProgram.Set();
};
//Escape_StartRun
buts.EscapePressed += () =>
{
LcdConsole.WriteLine("***StartRun***");
stopped.Set();
//loop_run on the line
while (true)
{
//touchsensor2 Pressed
if (touchSensor2.IsPressed() == true)
{
//motors stop
motorA.Brake();
motorD.Brake();
LcdConsole.WriteLine("***Stop***");
//timer start
sw.Start();
//after 5sec
Thread.Sleep(5000);
a = sw.ElapsedMilliseconds;
LcdConsole.WriteLine("***Stop_5sec***");
if (a >= 5000)
{
//timer stop
a = 0;
sw.Stop();
sw.Reset();
if (touchSensor2.IsPressed() == true)
{
LcdConsole.WriteLine("***Stop_ISPressed2***");
//
Stream reqStream = req.GetRequestStream();
LcdConsole.WriteLine("***Stop_GetReqStream***");
reqStream.Write(data, 0, data.Length);
LcdConsole.WriteLine("***Stop_write***");
reqStream.Close();
LcdConsole.WriteLine("***Stop_reqStream.close***");
break;
}
if (touchSensor1.IsPressed() == true)
{
//
Stream reqStream = req.GetRequestStream();
reqStream.Write(data, 0, data.Length);
reqStream.Close();
break;
}
}
else
{
continue;
}
}
//touchsensor1 pressed
if (touchSensor1.IsPressed() == true)
{
motorA.Brake();
motorD.Brake();
LcdConsole.WriteLine("***Stop***");
//timer start
sw.Start();
//after 5sec
Thread.Sleep(5000);
a = sw.ElapsedMilliseconds;
if (a >= 5000)
{
//timer stop
a = 0;
sw.Stop();
sw.Reset();
if (touchSensor1.IsPressed() == true)
{
//
Stream reqStream = req.GetRequestStream();
reqStream.Write(data, 0, data.Length);
reqStream.Close();
break;
}
if (touchSensor2.IsPressed() == true)
{
//
Stream reqStream = req.GetRequestStream();
reqStream.Write(data, 0, data.Length);
reqStream.Close();
break;
}
}
else
{
continue;
}
}
//Ultrasonic on
if (UltraSonicSensor.Read() <= 30)
{
motorA.Brake();
motorD.Brake();
LcdConsole.WriteLine("***Stop***");
//timer start
sw.Start();
//after 5sec
Thread.Sleep(5000);
a = sw.ElapsedMilliseconds;
if (a >= 5000)
{
//timer stop
a = 0;
sw.Stop();
sw.Reset();
if (UltraSonicSensor.Read() <= 30)
{
//
Stream reqStream = req.GetRequestStream();
reqStream.Write(data, 0, data.Length);
reqStream.Close();
break;
}
}
else
{
continue;
}
}
b = sensor.Read();
if (b < 15)
{
vehicle.TurnRightForward(10, 100);
}
if (15 <= b && b < 60)
{
vehicle.TurnLeftForward(10, 60);
}
if (b >= 60)
{
waitHandle = vehicle.Forward(10, 0, true);
}
}
};
terminateProgram.WaitOne();
buts.LeftPressed += () => {
terminateProgram.Set();
};
}
private static void Main()
{
BrickConsole.WriteLine("Start!");
var speaker = new Speaker(10);
speaker.Beep();
var tank = new Tank(MotorPort.OutD, MotorPort.OutA);
var irSensorWrapper = new IRSensorLockWrapper(new EV3IRSensor(SensorPort.In2));
var colorSensor = new EV3ColorSensor(SensorPort.In1, ColorMode.Color);
var touchSensor = new EV3TouchSensor(SensorPort.In3);
var buttons = new ButtonEvents();
buttons.EscapePressed += () =>
{
tank.Stop();
BrickConsole.WriteLine("End!");
speaker.Buzz();
Environment.Exit(0);
};
var remote = new RemoteControl(irSensorWrapper, IRChannel.One);
remote.ButtonsReleased += btn =>
{
tank.Stop();
BrickConsole.WriteLine("TankState: {0}", tank);
};
remote.ButtonsPressed += btn =>
{
switch (btn)
{
case RemoteButton.LeftUp:
tank.StartForward(speed);
break;
case RemoteButton.LeftDown:
tank.StartBackward(speed);
break;
case RemoteButton.RightUp:
tank.StartSpinLeft(speed);
break;
case RemoteButton.RightDown:
tank.StartSpinRight(speed);
break;
case RemoteButton.LeftUp | RemoteButton.LeftDown:
BrickConsole.WriteLine("Color: {0}", colorSensor.ReadColor());
break;
case RemoteButton.RightUp | RemoteButton.RightDown:
BrickConsole.WriteLine("TouchSensor.IsPressed: {0}", touchSensor.IsPressed());
break;
case RemoteButton.LeftUp | RemoteButton.RightUp:
BrickConsole.WriteLine("Distance: {0} cm", irSensorWrapper.Do(x => x.ReadDistance()));
break;
case RemoteButton.Beacon:
BrickConsole.WriteLine("Color: {0}", colorSensor.ReadColor());
BrickConsole.WriteLine("Distance: {0} cm", irSensorWrapper.Do(x => x.ReadDistance()));
BrickConsole.WriteLine("TouchSensor.IsPressed: {0}", touchSensor.IsPressed());
break;
}
};
Thread.Sleep(Timeout.Infinite);
}
static void DriveToColor(sbyte speed, string Color, Motor motorR, Motor motorL, EV3ColorSensor sensor)
{
int i = 0;
LcdConsole.WriteLine("zoeken naar" + sensor.ReadAsString() + "en " + Color);
Thread.Sleep(1000);
motorL.SetSpeed(speed);
motorR.SetSpeed(speed);
while (Color != sensor.ReadAsString())
{
LcdConsole.WriteLine("Sensor ẁaarde:" + sensor.ReadAsString());
if (Color == (sensor.ReadAsString()))
{
LcdConsole.WriteLine("Sensor ẁaarde:" + sensor.ReadAsString());
LcdConsole.WriteLine("GEVONDEN " + sensor.ReadAsString());
motorR.Off();
motorL.Off();
break;
}
Thread.Sleep(50); // wacht voor zoveel miliseconde voor de volgende loop
i++;
if (i > 60) // als 5000 miliseconden voorbij zijn stop
{
LcdConsole.WriteLine("heeft de kleur niet kunnen vinden");
motorR.Off();
motorL.Off();
break;
}
}
}
