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(); }