public void ObstacleListener(OBSTACLE_DIRECTION direction, bool isThereAnObstacle)
{
if (!Map.MapInformation.isObstacleDetecteurOn())
{
Informations.printInformations(Priority.MEDIUM, "Mouvement - ObstacleListener - !Map.MapInformation.isObstacleDetecteurOn(), no change on trajectory. ");
return;
}
if (direction == Robot.robot.BASE_ROULANTE.GetDirection())
{
if (isThereAnObstacle /*&& Robot.robot.BASE_ROULANTE.kangaroo.currentMode == "D"*/)
{
Informations.printInformations(Priority.MEDIUM, "Mouvement - ObstacleListener - obstacle detected : pausing movement. ");
this.Pause();
}
if (!isThereAnObstacle)
{
Informations.printInformations(Priority.MEDIUM, "Mouvement - ObstacleListener - obstacle removed : resuming movement. ");
//Robot.robot.IHM.AfficherInformation("UNPAUSED", false);
//new Thread(() => this.Start()).Start();
isPaused = false;
}
}
else
{
Informations.printInformations(Priority.MEDIUM, "Mouvement - ObstacleListener - obstacle in other direction : ignored. ");
}
}
public int GetDurationOfRotation(float angle)
{
// Récupère la vitesse (et le mode de fonctionnement ?) / dépend de la vitesse et du mode (à rajouter en param dans ce cas)
// Pour renvoyer le nombre de ms à attendre pour effectuer la rotation
Informations.printInformations(Priority.LOW, "la durée de la rotation a été de _ secondes");
return(0);
}
public bool move(int value)
{
if (m_mode != AX12Mode.joint)
{
Informations.printInformations(Priority.HIGH, "Servo ID " + m_ID + " pas en omde joint, erreur en appelant move!");
return(true);
}
Informations.printInformations(Priority.LOW, "Servo ID " + m_ID + " en mode joint, moving...");
byte[] buf = { 0x1E, (byte)(value), (byte)(value >> 8) };
if (sendCommand(m_ID, Instruction.AX_WRITE_DATA, buf) == null)
{
return(true); // ERROR !!!
}
Thread.Sleep(10);
bool currentMoving = isMoving();
if (currentMoving != true)
{
Informations.printInformations(Priority.HIGH, "WARNING !!! isMoving = false right after Servo.Move()...");
return(false);
}
do
{
Thread.Sleep(10);
} while (isMoving());
TEMPORAIRE_position = value;
Thread.Sleep(40); // a commenter ou decommenter
return(false);
}
//on met a jour
private void updatePosition(mode m)
{
switch (m)
{
case mode.turn:
int angleDeplacement = 0;
double newTheta = position.theta;
int errorCodeAngle = 0;
errorCodeAngle = getDataSinceLastReset(mode.turn, ref angleDeplacement);
if (errorCodeAngle == 0)
{
newTheta += angleDeplacement;
position = new PointOriente(position.x, position.y, newTheta);
}
break;
case mode.drive:
int deplacement = 0;
double theta = position.theta;
double X = position.x;
double Y = position.y;
int errorCode = getDataSinceLastReset(mode.drive, ref deplacement);
if (errorCode == 0)
{
double angle = System.Math.PI * theta / 180.0;
X += deplacement * System.Math.Cos(angle);
Y += deplacement * System.Math.Sin(angle);
position = new PointOriente(X, Y, theta);
}
break;
}
Informations.printInformations(Priority.LOW, "l'information sur la position a été mise à jour");
}
public AX12(int socket, int id)
{
if (serialPort == null)
{
string COMSerie = GT.Socket.GetSocket(socket, true, null, null).SerialPortName; //permet d'associer le nom de communication série au socket (ici 'COMSerie' au socket11)
int baudRate;
if ((Robot.robot.TypeRobot == TypeRobot.GRAND_ROBOT && Robot.robot.isGRSpiderI) ||
(Robot.robot.TypeRobot == TypeRobot.PETIT_ROBOT && Robot.robot.isPRSpiderI))
{
baudRate = 1000000;
direction_TX = new OutputPort((Cpu.Pin)GHI.Pins.FEZSpider.Socket11.Pin3, false);
}
else
{
baudRate = 937500;
direction_TX = new OutputPort((Cpu.Pin)GHI.Pins.FEZSpiderII.Socket11.Pin3, false);
}
SerialPort PortCOM = new SerialPort(COMSerie, baudRate, Parity.None, 8, StopBits.One);
PortCOM.ReadTimeout = 500; // temps de réception max limité à 500ms
PortCOM.WriteTimeout = 500; // temps d'émission max limité à 500ms
PortCOM.Open();
if (PortCOM.IsOpen)
{
PortCOM.Flush();
Informations.printInformations(Priority.HIGH, "SERVOS : Port COM ouvert : " + COMSerie);
}
else
{
Informations.printInformations(Priority.HIGH, "ERREUR : SERVOS : Port COM fermé !!!!!! Port : " + COMSerie);
}
//direction_TX = new OutputPort((Cpu.Pin)GT.Socket.GetSocket(socket, true, null, null).CpuPins[3], false); // ligne de direction de data au NLB (Spider en réception de l'interface AX12)
//direction_TX = new OutputPort((Cpu.Pin)EMX.IO26, false); // ligne de direction de data au NLB (Spider en réception de l'interface AX12)
serialPort = PortCOM;
//direction_TX = new OutputPort((Cpu.Pin)GT.Socket.GetSocket(socket, true, null, null)., false);
//OutputPort direction_TX = new OutputPort(GHI.Pins.G120E.Gpio.P2_30, false); //équivalente à la précédente
// OutputPort direction_TX = new OutputPort(GHI.Pins.FEZSpiderII.Socket11.Pin3,false); équivalente à la précédente
/*// configure le port de communication série, ligne half-duplex, pin IO3/TXD0 du socket 11
* string COMSerie = GT.Socket.GetSocket(11, true, null, null).SerialPortName; //permet d'associer le nom de communication série au socket (ici 'COMSerie' au socket11)
* // string COMSerie = GHI.Pins.G120E.SerialPort.Com1;
* Debug.Print(COMSerie);
* SerialPort PortCOM = new SerialPort(COMSerie, 937500, Parity.None, 8, StopBits.One);
* PortCOM.ReadTimeout = 500; // temps de réception max limité à 500ms
* PortCOM.WriteTimeout = 500; // temps d'émission max limité à 500ms
*
* // ouverture du port de communication série et vider son buffer
* PortCOM.Open();
* if (PortCOM.IsOpen) PortCOM.Flush();
*
* serialPort = PortCOM;*/
}
this.cax12 = new CAX_12((byte)id, serialPort, direction_TX);
//GHI.Pins.FEZSpiderII.Socket11
}
protected void OnObstacleChange(bool isThereAnobstacle)
{
Informations.printInformations(Priority.LOW, "CapteurObstacle - OnObstacleChange called , OBSTACLE=" + isThereAnobstacle + " in direction=" + this.direction);
if (CapteurObstacleEvent != null)
{
CapteurObstacleEvent(index, isThereAnobstacle);
}
}
/// <summary>
/// Rotation RELATIVE par rapport à la position actuelle du servo.
/// (Passe en mode wheel si nécessaire)
/// </summary>
/// <param name="angle">En degrès</param>
/// <returns>Durée en ms estimée de la rotation</returns>
public int RotateOf(float angle)
{
// TODO
string a = angle.ToString();
Informations.printInformations(Priority.HIGH, "rotation du servo de " + a + "degrés");
return(1);
}
/// <summary>
/// Rotation ABSOLUE : défini l'angle de manière directe.
/// </summary>
/// <param name="angle">En degrès</param>
/// <returns>Durée en ms estimée de la rotation</returns>
public int SetAngle(float angle)
{
// TODO
string a = angle.ToString();
Informations.printInformations(Priority.HIGH, "l'angle absolu du robot est désormais de " + a + "degrés");
return(1);
}
public RecepteurCodeCouleur(int socket)
: base(socket)
{
this.adapteur = new GTM.GHIElectronics.XBeeAdapter(socket);
adapteur.Configure(9600, GT.SocketInterfaces.SerialParity.None, GT.SocketInterfaces.SerialStopBits.One, 8, GT.SocketInterfaces.HardwareFlowControl.NotRequired);
this.m_port = adapteur.Port;
m_port.Open();
Informations.printInformations(Priority.HIGH, "Port COM Recepteur code couleur ouvert.");
}
public void PRCubeRecolteDeplierBras(int numeroPRCube)
{
switch (numeroPRCube)
{
// PLACER LE BON BRAS EN FONCTION DU PRCube
}
string ncube = numeroPRCube.ToString();
Informations.printInformations(Priority.HIGH, "le bras pour le cube " + ncube + " a été déplié");
}
private void JackChange(bool newStatus)
{
Informations.printInformations(Priority.HIGH, "JackAction : Jack changed, status = success & stop listening to Jack, new Jack status : " + newStatus);
//Robot.Robot.robot.JACK.JackChangeEvent -= this.JackChange;
//Robot.Robot.robot.JACK.StopTimer();
Robot.Robot.robot.JACK.doneListeningToJack = true;
Robot.Robot.robot.StartCountdown();
this.Status = ActionStatus.SUCCESS;
//DisplayOnIHM();
}
public override void Execute()
{
Informations.printInformations(Priority.HIGH, "ActionBaseRoulante - execute called. Desc : " + this.description);
Robot.Robot.robot.BASE_ROULANTE.InstructionCompletedEvent += () =>
{
Informations.printInformations(Priority.HIGH, "ActionBaseRoulante : completed, status = success");
this.Status = ActionStatus.SUCCESS;
};
this.mouvement.Start();
}
public Infrarouge(int socket, int port, OBSTACLE_DIRECTION direction)
: base(direction)
{
Informations.printInformations(Priority.MEDIUM, "New infrarouge on socket " + socket + " & port " + port + "; pin : " + Socket.GetSocket(socket, true, null, null).CpuPins[port] + "; type of pin : " + Socket.GetSocket(socket, true, null, null).CpuPins[port].GetType());
this.capteurIR = new InterruptPort(
Socket.GetSocket(socket, true, null, null).CpuPins[port],
true,
Port.ResistorMode.PullUp,
Port.InterruptMode.InterruptEdgeBoth);
this.capteurIR.OnInterrupt += (uint pin, uint state, DateTime time) => OnObstacleChange(state == 0);
}
public void launchSpeed(double speed)
{
if (speed >= -1.0f && speed <= 1.0f)
{
moteur.SetSpeed(MotorDriverL298.Motor.Motor1, speed);
Informations.printInformations(Priority.MEDIUM, "LanceurBalle : launched motor 1 with speed " + speed);
}
else
{
Informations.printInformations(Priority.HIGH, "LanceurBalle : VITESSE INVALIDE !!! Doit etre de module <= 1. here speed = " + speed);
}
}
public void Execute(int delay)
{
Informations.printInformations(Priority.MEDIUM, "Executing action servo absolue; angle : " + angle + "; description : " + description);
/*if (delay >= 0)
* delay = servomoteur.SetAngle(angle, duration);
* else
* delay = servomoteur.SetAngle(angle);*/
servomoteur.SetAngle(angle);
Thread.Sleep(100);
this.Status = ActionStatus.SUCCESS;
}
public bool isMoving()
{
byte[] buf = { (byte)Address.AX_MOVING, 0x01 };
//Debug.Print("IS MOVING ?");
int[] retour = sendCommand(m_ID, Instruction.AX_READ_DATA, buf);
if (retour == null || retour.Length <= 0)
{
Informations.printInformations(Priority.HIGH, "Servomoteur : isMoving got bad return : no params returned...");
return(false);
}
return(retour[0] == 1);
}
public bool SetAngle(int steps, int sleepDuration)
{
// http://folk.uio.no/matsh/inf4500/files/datasheets/Dynamixel%20-%20AX-12_files/dx_series_goal.png
// http://folk.uio.no/matsh/inf4500/files/datasheets/Dynamixel%20-%20AX-12.htm
Informations.printInformations(Priority.LOW, "l'angle absolu de l'AX-12 est désormais de " + steps + "steps");
this.cax12.setMode(AX12Mode.joint);
//this.cax12.setMovingSpeed(speed.forward);
return(this.cax12.move(steps));
//return sleepDuration; // TODO : change depending of the previous angle
}
public Jack(int socket, int port)
{
Informations.printInformations(Priority.MEDIUM, "New Jack on socket " + socket + " & port " + port + "; pin : " + Socket.GetSocket(socket, true, null, null).CpuPins[port] + "; type of pin : " + Socket.GetSocket(socket, true, null, null).CpuPins[port].GetType());
/*capteurIR = new InterruptPort(Socket.GetSocket(socket, true, null, null).CpuPins[port], false, Port.ResistorMode.Disabled, Port.InterruptMode.InterruptEdgeBoth);
* //capteurIR.EnableInterrupt();
* capteurIR.OnInterrupt += new NativeEventHandler((uint data1, uint data2, DateTime time) => this.OnObstacleChange());*/
this.cableJack = new InputPort(Socket.GetSocket(socket, true, null, null).CpuPins[port], false, Port.ResistorMode.Disabled);
this.timer = new Gadgeteer.Timer(REFRESH_RATE);
timer.Tick += this.Tick;
}
public void LaunchMovingInstructionCompletedEvent()
{
Informations.printInformations(Priority.MEDIUM, "BaseRoulante : LaunchMovingInstructionCompletedEvent called");
if (this.InstructionCompletedEvent != null)
{
Informations.printInformations(Priority.MEDIUM, "BaseRoulante - LaunchMovingInstructionCompletedEvent : listeners found !!!");
this.InstructionCompletedEvent();
}
else
{
Informations.printInformations(Priority.MEDIUM, "BaseRoulante - LaunchMovingInstructionCompletedEvent : no listeners found");
}
}
private void Tick(Timer t)
{
iterations++;
bool status = IsJackOn();
Robot_P16.Robot.Robot.robot.IHM.AfficherInformation("Iteration " + iterations + "\nJack satus :\n" + Robot_P16.Robot.Robot.robot.JACK.IsJackOn(), false);
if (status != lastStatus)
{
Informations.printInformations(Priority.HIGH, "Jack changed : new status : " + status);
lastStatus = status;;
OnJackChange(lastStatus);
}
}
private void UpdatePositionFromFeedback(string feedback)
{
Informations.printInformations(Priority.VERY_LOW, "Kangaroo - UpdatePositionFromFeedback - mode : " + this.currentMode + "; feedback : " + feedback);
//T,P100
if (/*this.currentMode == null || */ feedback == null || feedback.Length < 4)
{
return; // this.position;
}
char status = feedback[2];
Informations.printInformations(Priority.LOW, "FEED : " + feedback);
if (feedback[2] == 'E')
{
return;
}
//Debug.Print("Status : " + status);
string sub_str = feedback.Substring(3, feedback.Length - 3);
//Debug.Print("Deplacement read : " + sub_str);
int deplacementFromFeedback = Int32.Parse(sub_str);
if (feedback[0] == 'D')
{
double deplacement = deplacementFromFeedback - distanceIncrementale;
distanceIncrementale = deplacementFromFeedback;
deplacement *= RAPPORT_DISTANCE_CODEUR_VERS_MM;
deplacement /= RAPPORT_DISTANCE_MM_VERS_CODEUR;
deplacement = deplacement / ratioPointOrienteVersKangarooDIST;
if (Robot.robot.TypeRobot == TypeRobot.PETIT_ROBOT)
{
deplacement = -deplacement;
}
double angle = System.Math.PI * position.theta / 180.0;
position.x = position.x + deplacement * System.Math.Cos(angle);
position.y = position.y + deplacement * System.Math.Sin(angle);
//Debug.Print("MAJ Position = " + position.x + "," + position.y + "," + position.theta);
}
else
{
double deplacement = deplacementFromFeedback - angleIncremental;
angleIncremental = deplacementFromFeedback;
deplacement /= RAPPORT_ANGLE_DEGRE_VERS_CODEUR;
deplacement = deplacement / ratioPointOrienteVersKangarooANGLE;
if (Robot.robot.TypeRobot == TypeRobot.GRAND_ROBOT)
{
deplacement = -deplacement;
}
position.theta += deplacement;
//Debug.Print("MAJ Position = " + position.x + "," + position.y + "," + position.theta);
}
}
public void detectObstacle(Gadgeteer.Timer timer)
{
//if (Robot.robot.OBSTACLE_MANAGER.ObstacleChangeEvent == null) return; TODO : UNCOMMENT AFTER DEBUG
bool obstacle = IsThereAnObstacle();
Informations.printInformations(Priority.VERY_LOW, "Obstacle IR : " + obstacle + "; lastStatus : " + lastStatus);
if (obstacle != this.lastStatus)
{
this.lastStatus = obstacle;
this.OnObstacleChange(obstacle);
}
}
public override void Execute()
{
Informations.printInformations(Priority.MEDIUM, "ActionEnSerie - execute called. Desc : " + this.description);
//this.ResetStatus(); Not necessary for now
this.indexOfCurrentAction = 0;
if (listeActions.Length > this.indexOfCurrentAction)
{
this.listeActions[this.indexOfCurrentAction].StatusChangeEvent += this.Feedback;
this.listeActions[this.indexOfCurrentAction].Execute();
}
else // No actions, success
{
this.Status = ActionStatus.SUCCESS;
}
}
public void stop()
{
//Init();
//this.CheckMovingStatus();
Informations.printInformations(Priority.MEDIUM, "Kangaroo - called stop()");
//
//Init();
if (currentMode != null)
{
this.EnvoyerCommande("T,powerdown\r\n");
this.EnvoyerCommande("D,powerdown\r\n");
//this.EnvoyerCommande(currentMode+",p"+distanceIncrementale+"\r\n");
MoveCompleted.Set();
}
//MoveCompleted.Set();
}
public void Start()
{
// Launch command
Informations.printInformations(Priority.HIGH, "Mouvement - Start() called");
if (Robot.robot.OBSTACLE_MANAGER != null && !subscribedToEvent)
{
Robot.robot.OBSTACLE_MANAGER.ObstacleChangeEvent += this.ObstacleListener;
subscribedToEvent = true;
}
Informations.printInformations(Priority.HIGH, "Started at" + GetPosition().x.ToString() + "," + GetPosition().y.ToString() + "," + GetPosition().theta.ToString());
bool ok;
if (this.isDirectionForced)
{
ok = this.GoToOrientedPoint(this.destination, this.forcedDirection);
}
else
{
ok = this.GoToOrientedPoint(this.destination);
}
if (ok)
{
this.isPaused = false;
if (Robot.robot.OBSTACLE_MANAGER != null)
{
Robot.robot.OBSTACLE_MANAGER.ObstacleChangeEvent -= this.ObstacleListener;
subscribedToEvent = false;
}
Informations.printInformations(Priority.HIGH, "Launch completed event");
Robot.robot.BASE_ROULANTE.LaunchMovingInstructionCompletedEvent();
}
else
{
while (isPaused)
{
Thread.Sleep(500);
}
Start();
}
}
private string sendAndReceiveUpdate(string prefix)
{
lock (readLocker)
{
string commande = prefix + ",getp\r\n";
EnvoyerCommande(commande);
string feedback = "";
int suffixLeftToRead = 2;
while (suffixLeftToRead > 0)
{
char charRead = (char)m_portCOM.ReadByte();
if (charRead == '\r' || charRead == '\n')
{
suffixLeftToRead--;
}
else
{
feedback += charRead;
}
}
/*byte[] bytesRead = new byte[m_portCOM.BytesToRead];
* m_portCOM.Read(bytesRead, 0, m_portCOM.BytesToRead);
*
* blockRead = false;*
* //UnblockRead.Set();
*
* char[] feedback_chars = System.Text.Encoding.UTF8.GetChars(bytesRead);
*
* string feedback = new string(feedback_chars);*/
if (feedback != null && feedback.Length >= 2)
{
//if(feedback.Substring(feedback.Length - 4) == "\r\n") {
//feedback = feedback.Substring(0, feedback.Length - 2);
//}
}
Informations.printInformations(Priority.VERY_LOW, (feedback + ",len:" + feedback.Length));
return(feedback);
}
}
public bool drive(int distance, int vitesse)
{
lastInstructionSent = DateTime.Now;
Informations.printInformations(Priority.HIGH, "Drive, Distance demande : " + distance);
string commande;
vitesse *= RAPPORT_DISTANCE_MM_VERS_CODEUR;
distance = (int)((double)(distance) / (double)(RAPPORT_DISTANCE_CODEUR_VERS_MM)*(double)(RAPPORT_DISTANCE_MM_VERS_CODEUR)*ratioPointOrienteVersKangarooDIST);
Init();
currentMode = "D";
commande = "D,p" + distance + "s" + vitesse + "\r\n";
if (!EnvoyerCommande(commande))
{
return(false);
}
Informations.printInformations(Priority.MEDIUM, commande);
Informations.printInformations(Priority.MEDIUM, "Kangaroo - Drive : sent command, waiting for move completion");
MoveCompleted.WaitOne();
currentMode = null;
//Thread.Sleep(100);
return(true);
}
public bool rotate(double angle, int vitesse)
{
lastInstructionSent = DateTime.Now;
string commande;
Informations.printInformations(Priority.HIGH, "Rotate, Angle demande : " + angle);
vitesse *= RAPPORT_ANGLE_DEGRE_VERS_CODEUR;
angle = RAPPORT_ANGLE_DEGRE_VERS_CODEUR * angle * ratioPointOrienteVersKangarooANGLE;
Init();
currentMode = "T";
commande = "T,p" + (int)(angle) + "s" + vitesse + "\r\n";
Informations.printInformations(Priority.MEDIUM, "Command sent: " + commande);
if (!EnvoyerCommande(commande))
{
return(false);
}
Informations.printInformations(Priority.MEDIUM, "Kangaroo - Turn : sent command, waiting for move completion");
MoveCompleted.WaitOne();
currentMode = null;
//Thread.Sleep(100);
return(true);
}
public Boolean GoToOrientedPoint(PointOriente pt) // Automatically find direction
{
double angle;
double deltaX, deltaY, deltaTheta;
deltaX = pt.x - this.GetPosition().x;
deltaY = pt.y - this.GetPosition().y;
Informations.printInformations(Priority.HIGH, "Going to " + pt.x.ToString() + "," + pt.y.ToString() + "\r\n");
if (deltaX == 0)
{
if (deltaY > 0)
{
deltaTheta = 90;
}
else
{
deltaTheta = 270;
}
}
else
{
deltaTheta = convertTo360(180 / System.Math.PI * System.Math.Atan2(deltaY, deltaX));
}
angle = (convertTo360(this.GetPosition().theta) - convertTo360(deltaTheta) + 90);
Informations.printInformations(Priority.HIGH, "Angle before modulo : " + angle.ToString());
angle = convertTo360(angle);
Informations.printInformations(Priority.LOW, "J'ai deltaTheta = " + deltaTheta.ToString());
Informations.printInformations(Priority.LOW, "Donc angle = " + angle.ToString());
if (angle > 180)
{
return(this.GoToOrientedPoint(pt, OBSTACLE_DIRECTION.ARRIERE));
}
else
{
return(this.GoToOrientedPoint(pt, OBSTACLE_DIRECTION.AVANT));
}
}
public void launchJackEvent()
{
if (doneListeningToJack)
{
Informations.printInformations(Priority.MEDIUM, "LaunchJackEvent called, but done listening to Jack.");
return; // Already called by another interrupt event
}
if (isJackEventAboutToBeLaunched)
{
Informations.printInformations(Priority.HIGH, "LaunchJackEvent called, but was already called earlier.");
return; // Already called by another interrupt event
}
isJackEventAboutToBeLaunched = true; // Not very Thread safe, but who cares ?
new Thread(() => {
Thread.Sleep(500);
Informations.printInformations(Priority.HIGH, "Jack interrupt event detected : isJackOn : " + this.IsJackOn());
if (JackChangeEvent != null)
{
JackChangeEvent(this.IsJackOn());
}
isJackEventAboutToBeLaunched = false;
}).Start();
}
