public void Right(int degrees = 90)
{
sbyte negativeSpeed = convertSpeedToNegative(TURN_SPEED);
_rightMotor.SetSpeed(negativeSpeed);
_leftMotor.SetSpeed(TURN_SPEED);
_rightMotor.ResetTacho();
_leftMotor.ResetTacho();
bool turning = true;
int turnCount = Convert.ToInt32(ONE_DEGREE_TURN * degrees);
while (turning)
{
bool right = _rightMotor.GetTachoCount() <= (turnCount * -1);
bool left = _leftMotor.GetTachoCount() >= turnCount;
if (right || left)
{
_leftMotor.Brake();
_rightMotor.Brake();
turning = false;
}
}
Thread.Sleep(100);
}
public void Reset()
{
_hefMotor.SetSpeed(SPEED_UP);
while (_limitSensor.ReadAsString() == "Off")
{
Thread.Sleep(100);
}
_hefMotor.Brake();
}
public static void FinalizePRogram()
{
BIOS_FINALE_BEEP();
Writeln("Finalization called!");
if (IsRobot)
{
for (MotorPort i = MotorPort.OutA; i <= MotorPort.OutD; i++)
{
// Turn off all motors
Motor FinaleMotor = new Motor(i);
FinaleMotor.Brake();
FinaleMotor.ResetTacho();
FinaleMotor.Off();
}
for (SensorPort i = SensorPort.In1; i <= SensorPort.In4; i++)
{
// Cannot finalize all sensors
}
}
else
{
// Windows
}
Writeln("Finalization finished.");
LogFileWriter.Close();
LogFileWriter = null;
}
static void DriveStraight(int time, sbyte speed, Motor motorR, Motor motorL) //Om de robot in een rechte lijn te laten rijden voor een bepaalde tijd
{
motorL.SetSpeed(speed);
motorR.SetSpeed(speed);
Thread.Sleep(time);
motorR.Brake();
motorL.Brake();
motorR.Off();
motorL.Off();
}
public void Forward(sbyte speed, int turns)
{
_leftMotor.SetSpeed(speed);
_rightMotor.SetSpeed(speed);
_rightMotor.ResetTacho();
while (_running)
{
if (_rightMotor.GetTachoCount() < turns)
{
_leftMotor.Brake();
_rightMotor.Brake();
}
CurrentLogger.Logger.debug(_rightMotor.GetTachoCount().ToString());
}
Off();
}
static void Turning(EV3GyroSensor gyroSensor, int draaiGraden, Motor motorL, Motor motorR)
{
gyroSensor.Reset();
sbyte draaiSnelheid = 20;
sbyte minDraaiSnelheid = -20;
int graden = 0;
int counter = 0;
int updateTime = 8;
if (draaiGraden > 0) //als hij naar rechts moet draaien dus een positief getal is
{
LcdConsole.WriteLine("GROTER ALS 0 ", draaiGraden);
motorL.SetSpeed(draaiSnelheid);
motorR.SetSpeed(minDraaiSnelheid);
while (graden <= draaiGraden) //terwijl het aantal graden dat hij gedraaid is kleiner is dan het aantal graden dat hij moet draaien
{
LcdConsole.WriteLine("Gyro sensor: " + gyroSensor.ReadAsString());
graden = gyroSensor.Read();
Thread.Sleep(updateTime);
counter++;
if (counter > 500)
{
break;
}
}
}
if (draaiGraden < 0) //als hij naar links moet draaien dus het aantal graden negatief is
{
LcdConsole.WriteLine("KLEINER ALS 0 ", draaiGraden);
motorL.SetSpeed(minDraaiSnelheid);
motorR.SetSpeed(draaiSnelheid);
while (graden >= draaiGraden) //terwijl het aantal graden dat hij gedraaid is groter is dan het aantal graden dat hij moet draaien
{
LcdConsole.WriteLine("Gyro sensor: " + gyroSensor.ReadAsString());
graden = gyroSensor.Read();
Thread.Sleep(updateTime);
counter++;
if (counter > 500)
{
break;
}
}
}
else
{
LcdConsole.WriteLine("IK KAN GEEN 0 GRADEN DRAAIEN " + gyroSensor.ReadAsString());
}
motorR.Brake();
motorL.Brake();
motorR.Off();
motorL.Off();
Thread.Sleep(1000);
}
public static void Main(string[] args)
{
Buttons btns = new Buttons();
Motor motor = new Motor(MotorPort.OutA);
MotorSync motorSync = new MotorSync(MotorPort.OutA, MotorPort.OutD);
LcdConsole.WriteLine("Use Motor on A");
LcdConsole.WriteLine("Use Motor on D");
LcdConsole.WriteLine("Press Ent. to start");
btns.GetKeypress();
motor.ResetTacho();
LcdConsole.WriteLine("Running forward with 20");
motor.On(20);
System.Threading.Thread.Sleep(2500);
LcdConsole.WriteLine("Printing motor speed: " + motor.GetSpeed());
System.Threading.Thread.Sleep(2500);
LcdConsole.WriteLine("Running backwards with 70");
motor.On(-70);
System.Threading.Thread.Sleep(3000);
LcdConsole.WriteLine("Reverse direction");
motor.Reverse = true;
System.Threading.Thread.Sleep(3000);
LcdConsole.WriteLine("Brake");
motor.Reverse = false;
motor.Brake();
System.Threading.Thread.Sleep(3000);
LcdConsole.WriteLine("Off");
motor.Off();
System.Threading.Thread.Sleep(3000);
LcdConsole.WriteLine("Move to zero");
motor.MoveTo(40, 0, true);
LcdConsole.WriteLine("Motor at position: " + motor.GetTachoCount());
System.Threading.Thread.Sleep(2000);
LcdConsole.WriteLine("Creating a step profile");
motor.SpeedProfileStep(40, 100, 1500, 100, true);
motor.Off();
LcdConsole.WriteLine("Motor at position: " + motor.GetTachoCount());
System.Threading.Thread.Sleep(2000);
LcdConsole.WriteLine("Motor " + motorSync.BitField + " synchronised forward for 2500 steps");
motorSync.On(50, 0, 2500, true);
motorSync.Off();
LcdConsole.WriteLine("Motor " + motorSync.BitField + " synchronised with second motor moving at half speed");
motorSync.On(-20, 50, 2500, false); //coast when done
LcdConsole.WriteLine("Done executing motor test");
}
private void ParkSensorArm()
{
if (touchSensor.IsPressed())
{
return;
}
motorSensor.SetSpeed(20);
while (!touchSensor.IsPressed())
{
Thread.Sleep(50);
}
motorSensor.Brake();
motorSensor.ResetTacho();
}
/// <summary>
/// Stop the PID
/// </summary>
/// <exception cref="InvalidOperationException">Motor must be assigned before operating the PID</exception>
public override void Stop()
{
// Stop the PID
base.Stop();
// Check for motor validity
if (Motor == null)
{
InvalidOperationException ex = new InvalidOperationException("Motor must be assigned before operating the PID");
throw (ex);
}
// Stop motor
Motor.SetSpeed(0);
Motor.Brake();
}
public virtual void Pee()
{
//Lift leg
Motor motor = new Motor(MotorPort.OutA);
motor.ResetTacho();
motor.SetSpeed(20);
System.Threading.Thread.Sleep(2500);
motor.Brake();
//System.Threading.Thread.Sleep(2500);
//Make peeing sound
MakeSound(DogSound.Pee);
LcdConsole.WriteLine("Peeing");
//Reset bladder
BladderLevel = 0;
}
public void Calibrate(Func <CalibrationSteps, bool> calibrated, bool skip = false)
{
Motor motor = null;
ResetTachos();
// calibrate hand
motor = _motors[2];
int handTacho = int.MaxValue;
motor.SpeedProfile(-32, 0, 180, 0, false);
// rotate until blocked
while ((handTacho != motor.GetTachoCount()))
{
handTacho = motor.GetTachoCount();
Thread.Sleep(75);
}
motor.Brake();
Off();
// pen up
motor.SpeedProfile(127, 0, 180, 0, true).WaitOne();
// found limit
ResetTachos();
if (skip)
{
return;
}
motor = _motors[0];
motor.SpeedProfile(-64, 0, (uint)Math.Abs(180 * MainMotorRatio), 0, false);
while (!calibrated(CalibrationSteps.Main))
{
;
}
Off();
// found limit
ResetTachos();
// move to neutral position
motor.SpeedProfile(64, 0, (uint)Math.Abs(90 * MainMotorRatio), 0, true).WaitOne();
// seconday
for (int i = 0; i < 3; i++)
{
motor = _motors[1];
motor.SpeedProfile(-64, 0, (uint)Math.Abs(360 * SecondaryMotorRatio), 0, false);
while (!calibrated(CalibrationSteps.Secondary))
{
;
}
motor.Brake();
motor.Off();
int right = motor.GetTachoCount();
// while (!calibrated(CalibrationSteps.Pause)) ;
// reset max
calibrated(CalibrationSteps.SecondaryReset);
// secondary (2nd step)
motor.SpeedProfile(64, 0, (uint)Math.Abs(360 * SecondaryMotorRatio), 0, false);
while (!calibrated(CalibrationSteps.Secondary))
{
;
}
motor.Brake();
motor.Off();
int left = motor.GetTachoCount();
//while (!calibrated(CalibrationSteps.Pause)) ;
// find middle point
int err = (right - left) / 2;
if (err != 0)
{
motor.SpeedProfile((err > 0) ? (sbyte)64 : (sbyte)-64, 0, (uint)Math.Abs(err), 0, true).WaitOne();
}
else
{
break; // nothing to do - we are good!
}
//while (!calibrated(CalibrationSteps.Pause)) ;
}
// found limit for second motor
ResetTacho(1);
}
public void motorBrake(int indexBrick, char motor)
{
Motor brickMotor = getMotorByIndexBreakAndLitera(indexBrick, motor);
brickMotor.Brake();
}
public static void MainThread()
{
// All in one thread
Init();
SetColor(ConsoleColor.Red);
LogFileWriter.WriteLine("");
Log("New log file started.");
try
{
TRobot.Writeln(PathForExe);
if (IsRobot)
{
Emergency.Start();
FinalizePRogram();
LogFileWriter = File.AppendText(LogFile);
BIOS_Beep();
// MonoBrick.EV3.Mailbox mail = new MonoBrick.EV3.Mailbox();
// Thread.Sleep(1000000);
/*string[] arr = MonoBrick.Bluetooth<BrickCommand, BrickReply>.GetPortNames();
* for (int i = 0; i < arr.Length; i++)
* {
* Writeln(arr[i]);
* }
* Thread.Sleep(100000);*/
Motor TestMotor = new Motor(MotorPort.OutD);
Writeln(TestMotor.GetTachoCount().ToString());
TestMotor.SetSpeed(100);
Thread.Sleep(1000);
TestMotor.Brake();
Writeln(TestMotor.GetTachoCount().ToString());
// Thread.Sleep(100000);
// System.IO.Ports.SerialPort serialPort = new SerialPort();
string[] ARR = SerialPort.GetPortNames();
for (int i = 0; i < ARR.Length; i++)
{
Writeln($"Serial {ARR[i]}");
}
Writeln("Hello from Robot");
long A = 0;
TDirectionizer directionizer = new TDirectionizer(SensorPort.In2, SensorPort.In3);
Motor Left = new Motor(MotorPort.OutC);
Motor Right = new Motor(MotorPort.OutB);
/*while (true)
* {
* int[] res = directionizer.Directionize();
* LcdConsole.Clear();
* TRobot.Writeln($"Motorl: {res[0]}; Motorr: {res[1]};");
* Left.SetPower((sbyte)res[0]);
* Right.SetPower((sbyte)res[1]);
* Thread.Sleep(50);
* }*/
TMotivator vec = new TMotivator(MotorPort.OutC, MotorPort.OutB);
EV3UltrasonicSensor ultrasonicSensor = new EV3UltrasonicSensor(SensorPort.In1, UltraSonicMode.Centimeter);
Motor Test = new Motor(MotorPort.OutB);
Writeln($"Prev tacho: {Test.GetTachoCount()}");
vec.MoveForward(50, 360);
Writeln($"Tacho: {Test.GetTachoCount()}");
Thread.Sleep(100000);
/*while (true)
* {
* A++;
* vec.MoveForward_mm(50, 300);
* vec.TurnLeft(20, 180);
* vec.MoveForward_mm(50, 300);
* vec.TurnRight(20, 180);
* if (A >= 4)
* break;
* }*/
}
TField Fld = Field_Generated_Class.Generate();
TDijkstra djk = new TDijkstra(new TPoint(1, 1), new TPoint(2, 0), Fld);
TRobot.Writeln(djk.Search_Way().ToString() + $" steps {djk.CountSteps}");
djk.Way.Reverse();
for (int i = 0; i < djk.Way.Count; i++)
{
TRobot.Writeln($"[{djk.Way[i].Position.X}, {djk.Way[i].Position.Y}]");
}
}
catch (Exception E)
{
string Str = ($"[EXCEPTION][{E.GetType().Name}]: " + E.Message);
File.WriteAllLines(PathForExe + "last_exception.txt", new string[] { Str });
TRobot.Writeln(Str);
}
TRobot.Writeln("Press any key to continue...");
TRobot.Read();
if (IsRobot)
{
for (MotorPort i = MotorPort.OutA; i <= MotorPort.OutD; i++)
{
Motor mot = new Motor(i);
mot.Brake();
mot.SetPower(0);
}
}
FinalizePRogram();
}
public void Break()
{
motor.Brake();
}
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();
};
}
public static void Main(string[] args)
{
Motor motorA = new Motor(MotorPort.OutA);
Motor motorD = new Motor(MotorPort.OutD);
WaitHandle motorWaitHandle;
motorA.Off();
motorD.Off();
//Power control
LcdConsole.WriteLine("Set power to 50");
motorA.SetPower(50);
Thread.Sleep(3000);
LcdConsole.WriteLine("Break");
motorA.Brake();
//Speed control
LcdConsole.WriteLine("Set speed to 50");
motorA.SetSpeed(50);
Thread.Sleep(1000);
LcdConsole.WriteLine("Speed: " + motorA.GetSpeed());
Thread.Sleep(2000);
LcdConsole.WriteLine("Break");
motorA.Brake();
//Position control of single motor
Thread.Sleep(3000);
motorA.ResetTacho();
LcdConsole.WriteLine("Moving motor A to 2000 ");
motorWaitHandle = motorA.SpeedProfile(50, 200, 1600, 200, true);
//you could do something else here
LcdConsole.WriteLine("Waiting for motor A to stop");
motorWaitHandle.WaitOne();
LcdConsole.WriteLine("Done moving motor");
LcdConsole.WriteLine("Position A: " + motorA.GetTachoCount());
//Individual position control of both motors
Thread.Sleep(3000);
motorA.ResetTacho();
motorD.ResetTacho();
LcdConsole.WriteLine("Moving motors A to 2000");
LcdConsole.WriteLine("Moving motor B to 1000");
WaitHandle[] handles = new WaitHandle[2];
handles[0] = motorA.SpeedProfile(50, 200, 1600, 200, true);
handles[1] = motorD.SpeedProfile(50, 200, 600, 200, true);
//you could do something else here
LcdConsole.WriteLine("Waiting for both motors to stop");
WaitHandle.WaitAll(handles);
LcdConsole.WriteLine("Done moving both motors");
LcdConsole.WriteLine("Position A: " + motorA.GetTachoCount());
LcdConsole.WriteLine("Position D: " + motorD.GetTachoCount());
motorA.Off();
motorD.Off();
//Motor synchronisation position control
Thread.Sleep(3000);
motorA.ResetTacho();
motorD.ResetTacho();
MotorSync sync = new MotorSync(MotorPort.OutA, MotorPort.OutD);
LcdConsole.WriteLine("Sync motors to move 3000 steps forward");
motorWaitHandle = sync.StepSync(40, 0, 3000, true);
//you could do something else here
LcdConsole.WriteLine("Waiting for sync to stop");
motorWaitHandle.WaitOne();
LcdConsole.WriteLine("Done sync moving both motors");
LcdConsole.WriteLine("Position A: " + motorA.GetTachoCount());
LcdConsole.WriteLine("Position D: " + motorD.GetTachoCount());
sync.Off();
}
