Menu
▾
▴
[Krobot-commits] [SCM] krobot branch, master, updated. 15f2f81f08e1a3684c09b9be858889a925977fb4
|
From: Jérémie D. <Ba...@us...> - 2010-04-21 16:50:29
|
This is an automated email from the git hooks/post-receive script. It was
generated because a ref change was pushed to the repository containing
the project "krobot".
The branch, master has been updated
via 15f2f81f08e1a3684c09b9be858889a925977fb4 (commit)
from 0fc3c12e38eba6fab3a467ad96573098b8b88633 (commit)
Those revisions listed above that are new to this repository have
not appeared on any other notification email; so we list those
revisions in full, below.
- Log -----------------------------------------------------------------
commit 15f2f81f08e1a3684c09b9be858889a925977fb4
Author: Jeremie Dimino <di...@vo...>
Date: Wed Apr 21 18:10:53 2010 +0200
refactoring
-----------------------------------------------------------------------
Changes:
diff --git a/info/control/Makefile b/info/control/Makefile
index fd92830..ab1e117 100644
--- a/info/control/Makefile
+++ b/info/control/Makefile
@@ -7,7 +7,7 @@
PREFIX := $(HOME)
-OC := ocamlbuild -Is common,protocol,lib-krobot
+OC := ocamlbuild
OF := ocamlfind
ifeq ($(TERM),dumb)
diff --git a/info/control/_tags b/info/control/_tags
index 937c55b..1aaed9b 100644
--- a/info/control/_tags
+++ b/info/control/_tags
@@ -11,7 +11,7 @@
# | Protocol |
# +------------------------------------------------------------------+
-<protocol/krobot_interfaces.*>: pkg_obus
+<protocol/*>: pkg_obus
# +------------------------------------------------------------------+
# | Card tools |
@@ -42,9 +42,7 @@
# | Common |
# +------------------------------------------------------------------+
-<common/krobot_{types,util}.{ml,mli}>: syntax_camlp4o, pkg_lwt.syntax
-<common/krobot_{types,util}.*>: pkg_obus
-<common/var.*>: pkg_react
+<common/*>: pkg_react, pkg_obus, syntax_camlp4o, pkg_lwt.syntax
# +------------------------------------------------------------------+
# | Driver |
diff --git a/info/control/common/PcInterface.h b/info/control/common/PcInterface.h
deleted file mode 100644
index d681bb5..0000000
--- a/info/control/common/PcInterface.h
+++ /dev/null
@@ -1,249 +0,0 @@
-/**
- * @file PcInterface.h
- * Fichier commun avec le programme hsur la carte m (USB PC).
- * Ce fichier dnit de nombreuses constantes utilis pour le protocole
- * de communication carte <-> PC.
-*/
-
-#ifndef PC_INTERFACE_H
-#define PC_INTERFACE_H
-
-#define USB_VID 0x04D8 ///< Vendor ID commun aux diffntes cartes USB
-#define USB_PID_USB_DEV_BOARD 0x0001 ///< Product ID de la Carte d'essais
-#define USB_PID_PROXIMITY_SENSOR 0x0002 ///< Product ID de la Carte capteurs
-#define USB_PID_MOTOR_CONTROLLER 0x0003 ///< Product ID de la Carte d'asservissement
-#define USB_PID_ROBOT_INTERFACE 0x0004 ///< Product ID de la Carte d'actionneur
-#define USB_PID_BATTERY_MONITORING 0x0005 ///< Product ID de la carte Battery Monitoring
-#define USB_PID_BOOTLOADER 0x000b ///< Product ID d'une carte en mode bootloader
-
-// Protocole USB
-typedef struct _UP {
- BYTE HSEQ;
- BYTE DSEQ;
- BYTE CMD;
- BYTE ERR;
- BYTE RES0;
- BYTE RES1;
- BYTE RES2;
- BYTE RES3;
- BYTE DATA[52];
- BYTE RES4;
- BYTE RES5;
- BYTE RES6;
- BYTE RES7;
-} UP;
-
-#define UP_HSEQ 0 ///< Host sequence number
-#define UP_DSEQ 1 ///< Device sequence number
-#define UP_CMD 2
-#define UP_ERR 3
-#define UP_RES0 4
-#define UP_RES1 5
-#define UP_RES2 6
-#define UP_RES3 7
-#define UP_RES4 60
-#define UP_RES5 61
-#define UP_RES6 62
-#define UP_RES7 63
-#define UP_DATA 8
-
-// Commande (premier bit)
-#define CMD_RESET 0 ///< Reset du PIC
-#define CMD_BOOTLOADER 1 ///< Reset du PIC en mode bootloader
-#define CMD_GET 2 ///< Obtenir une information
-#define CMD_RESPOND 3 ///< Rnse ne commande
-#define CMD_ERR 4 ///< Envoyer une erreur
-#define CMD_SEND 5 ///< Envoyer du texte
-#define CMD_SET 6 ///< Dnir l't d'un parame
-#define CMD_TEST 7 ///< Commande grique pour dencher une action de test
-#define CMD_CALIBRATE 8 ///< Lance la calibration d'un tme [Carte capteurs]
-#define CMD_AX12 9
-#define CMD_TRAJ 10 ///< Transmet une trajetoire au Krobot
-#define CMD_MOTOR 11 ///< Gestion des moteurs
-#define CMD_MOTOR_TOR 12
-#define CMD_LCD 13 ///< Commande de l'afficheur LCD
-
-// CMD_GET arguments
-#define GET_RESET_SOURCE 0 ///< Demande au PIC la source du Reset
-#define GET_BOARD_INFO 1 ///< Demande au PIC le nom de la carte et l'auteur
-#define GET_FIRMWARE_BUILD 2 ///< Demande au PIC la date et l'heure de compilation du firmware
-#define GET_PORTS_CONFIG 3 ///< Demande la config TRIS des ports du PIC
-#define GET_PORTS_STATE 4 ///< Demande l't des ports du PIC
-#define GET_RANGEFINDER_STATE 5 ///< Demande au PIC les mesures des tmes [Carte capteurs]
-#define GET_ISENS 6 ///< Demande au PIC les valeurs des courants moteurs mesur [Carte d'asservissement]
-#define GET_CURRENT_POS 7 ///< Demande au PIC les positions actuelles des moteurs [Carte d'asservissement]
-#define GET_RANGEFINDER_CALIBRATION 8 ///< Demande au PIC les valeurs de calibration des tmes [Carte capteurs]
-#define GET_TOR_STATE 9 ///< Demande au PIC l't des capteurs de contact [Carte capteurs]
-#define GET_CMP03_DATA 10 ///< Rp les infos du compas ctronique CMP03
-#define GET_CELL_VOLTAGE 11 ///< Tensions des cellules [Battery Monitoring]
-#define GET_CURRENT 12 ///< Valeur instantandu courant dtar la batterie [Battery Monitoring]
-#define GET_POWER_STATE 13 ///< Etat de l'alimentation de puissance (On/Off) [Battery Monitoring]
-#define GET_BATTERY_STATE 14 ///< Etat des batteries (Pleine charge/Charge moyenne/Charge faible) [Battery Monitoring]
-#define GET_CURRENT_SPEED 15 ///< Demande au PIC la valeur actuelle d'un moteur [Carte d'asservissement]
-#define GET_INTEGRATION_SUM 16 ///< Demande au PIC la valeur actuelle du terme integration de l'asservissement [Carte d'asservissement]
-
-// CMD_ERR arguments
-#define ERR_UNKNOWN_CMD 1 ///< Commande inconnue
-#define ERR_UNKNOWN_GET 2 ///< Demande inconnue
-#define ERR_UNKNOWN_SET 3 ///< Demande inconnue
-#define ERR_INVALID_RESPONSE 4 ///< Rnse invalide
-#define ERR_AX12_WRONG_PACKET 5
-#define ERR_AX12_ERROR 6
-#define ERR_AX12_CHKSUM 7
-#define ERR_CMP03_NOT_RESPONDING 8
-#define ERR_ADJD_S371_NOT_RESPONDING 9
-#define ERR_LM_COMMAND_ERROR 10
-#define ERR_LM_POSITION_ERROR 11
-#define ERR_INVALID_AXIS 12
-
-// CMD_SET arguments
-#define SET_PORTS_CONFIG_INPUTS 0x00 ///< Dnir les entr du PIC
-#define SET_PORTS_CONFIG_OUTPUTS 0x01 ///< Dnir les sorties du PIC
-#define SET_PORTS_STATE_LOW 0x02 ///< Dnir les sorties 't bas du PIC
-#define SET_PORTS_STATE_HIGH 0x03 ///< Dnir les sorties 't haut du PIC
-#define SET_SERVO_CONFIG 0x04 ///< Dnir la config des servomoteurs
-#define SET_SERVO_STATE 0x05 ///< Dnir l't des servomoteurs
-
-// CMD_CALIBRATE arguments
-#define CAL_START 0x00
-#define CAL_CONTINUE 0x01
-#define CAL_STOP 0x02
-#define CAL_ERROR 0x03
-#define CAL_DONE 0x04
-#define CAL_PLACE_INF 0x05
-#define CAL_PLACE_30 0x06
-#define CAL_PLACE_100 0x07
-
-// GET_RESET_SOURCE reponse
-#define RESET_SOURCE_POR 0x01 ///< Power-on Reset
-#define RESET_SOURCE_RI 0x02 ///< RESET Instruction
-#define RESET_SOURCE_BOR 0x03 ///< Brown-out Reset
-#define RESET_SOURCE_WDT 0x04 ///< Watchdog Time-out Reset
-#define RESET_SOURCE_STKFUL 0x05 ///< Stack Full Reset
-#define RESET_SOURCE_STKUNF 0x06 ///< Stack Underflow Reset
-#define RESET_SOURCE_MCLR 0x07 ///< Master Clear Reset
-
-// CMD_AX12
-#define AX12_PING 1
-#define AX12_READ8 2
-#define AX12_READ16 3
-#define AX12_WRITE8 4
-#define AX12_WRITE16 5
-#define AX12_GOTO 6
-#define AX12_GET_POS 7
-#define AX12_GET_SPEED 8
-#define AX12_GET_LOAD 9
-#define AX12_GET_STATS 10
-#define AX12_WRITE_REG8 11
-#define AX12_WRITE_REG16 12
-#define AX12_ACTION 13
-#define AX12_RESET 14
-#define AX12_CONFIG 15
-
-#define AX12_EXEC_NOW 0x00
-#define AX12_EXEC_ACTION 0x01
-
-// CMD_LCD
-#define LCD_CLEAR 0x00
-#define LCD_CURSOR_ON 0x01
-#define LCD_CURSOR_OFF 0x02
-#define LCD_BACKLIGHT_ON 0x03
-#define LCD_BACKLIGHT_OFF 0x04
-#define LCD_GOTO_POS 0x05
-#define LCD_WRITE 0x06
-#define LCD_WRITE_LINE 0x07
-
-// CMD_TRAJ
-#define TRAJ_INIT 0x00
-#define TRAJ_FORWARD 0x01
-#define TRAJ_BACKWARD 0x02
-#define TRAJ_TR 0x03
-#define TRAJ_TL 0x04
-#define TRAJ_GOTO 0x05
-#define TRAJ_FINISHED 0x06
-#define TRAJ_STOP 0x07
-#define TRAJ_NEW_POSITION 0x08
-#define TRAJ_NEW_VELOCITY 0x09
-#define TRAJ_CHANGE_VELOCITY 0x0A
-#define TRAJ_CONFIG 0x0B
-#define TRAJ_CONFIG_DEFAULT 0x0C
-#define TRAJ_START 0x0D
-#define TRAJ_GET_REL_POS 0x0E
-#define TRAJ_READ_CONFIG 0x0F
-#define TRAJ_TURN 0x10
-
-#define TRAJ_NOT_COMPLETED 0x00
-#define TRAJ_COMPLETED 0x01
-
-#define TRAJ_STOP_MOTOR_OFF 256
-#define TRAJ_STOP_ABRUPT 512
-#define TRAJ_STOP_SMOOTH 1024
-
-// CMD_MOTOR
-#define MOTOR_ENABLE 1
-#define MOTOR_DISABLE 2
-#define MOTOR_MOVE 3
-
-#define MOTOR_RIGHT 1 ///< Sction du moteur de droite (moteur 1)
-#define MOTOR_LEFT 2 ///< Sction du moteur de gauche (moteur 2)
-#define MOTOR_BOTH 3 ///< Sction des 2 moteurs simultannt
-
-// XXX omplr
-#define READ_VERSION 0x00
-#define READ_FLASH 0x01
-#define WRITE_FLASH 0x02
-#define ERASE_FLASH 0x03
-#define READ_EEDATA 0x04
-#define WRITE_EEDATA 0x05
-#define READ_CONFIG 0x06
-#define WRITE_CONFIG 0x07
-#define UPDATE_LED 0x32
-#define RESET 0xFF
-
-#define FLASH_BOOT_START 0x00
-#define FLASH_BOOT_END 0x7FF
-#define FLASH_VECTOR_START 0x800
-#define FLASH_VECTOR_END 0x829
-#define FLASH_PAGE_START 0x82A
-#define FLASH_PAGE_END 0x7FFF
-
-#define PORTA_RA0 1
-#define PORTA_RA1 2
-#define PORTA_RA2 4
-#define PORTA_RA3 8
-#define PORTA_RA4 16
-#define PORTA_RA5 32
-#define PORTA_RA6 64
-
-#define PORTB_RB0 1
-#define PORTB_RB1 2
-#define PORTB_RB2 4
-#define PORTB_RB3 8
-#define PORTB_RB4 16
-#define PORTB_RB5 32
-#define PORTB_RB6 64
-#define PORTB_RB7 128
-
-#define PORTC_RC0 1
-#define PORTC_RC1 2
-#define PORTC_RC2 4
-#define PORTC_RC4 16
-#define PORTC_RC5 32
-#define PORTC_RC6 64
-#define PORTC_RC7 128
-
-#define PORTD_RD0 1
-#define PORTD_RD1 2
-#define PORTD_RD2 4
-#define PORTD_RD3 8
-#define PORTD_RD4 16
-#define PORTD_RD5 32
-#define PORTD_RD6 64
-#define PORTD_RD7 128
-
-#define PORTE_RE0 1
-#define PORTE_RE1 2
-#define PORTE_RE2 4
-#define PORTE_RE3 8
-
-#endif // PC_INTERFACE_H
diff --git a/info/control/common/krobot_config.ml b/info/control/common/krobot_config.ml
deleted file mode 100644
index f51a703..0000000
--- a/info/control/common/krobot_config.ml
+++ /dev/null
@@ -1,15 +0,0 @@
-(*
- * krobot_config.ml
- * ----------------
- * Copyright : (c) 2010, Jeremie Dimino <je...@di...>
- * Licence : BSD3
- *
- * This file is a part of [kro]bot.
- *)
-
-let back_sensors = [3; 6; 7; 10]
-let front_sensors = [0; 1; 2; 4; 5; 8; 9; 11; 12; 13; 14; 15]
-let initial_position = 200
-let bus_address = "unix:abstract=krobot"
-let update_delay = 0.05
-let reopen_delay = 1.0
diff --git a/info/control/common/krobot_config.mli b/info/control/common/krobot_config.mli
deleted file mode 100644
index a582c32..0000000
--- a/info/control/common/krobot_config.mli
+++ /dev/null
@@ -1,28 +0,0 @@
-(*
- * krobot_config.mli
- * -----------------
- * Copyright : (c) 2010, Jeremie Dimino <je...@di...>
- * Licence : BSD3
- *
- * This file is a part of [kro]bot.
- *)
-
-(** Krobot parameters *)
-
-val front_sensors : int list
- (** List of front sensors *)
-
-val back_sensors : int list
- (** List of back sensors *)
-
-val initial_position : int
- (** Position to borders at the beginning of the match *)
-
-val bus_address : string
- (** Default address of the krobot dbus daemon *)
-
-val update_delay : float
- (** Time to wait between updates *)
-
-val reopen_delay : float
- (** Time to wait before retrying to open a card *)
diff --git a/info/control/common/krobot_types.ml b/info/control/common/krobot_types.ml
deleted file mode 100644
index 4d3215c..0000000
--- a/info/control/common/krobot_types.ml
+++ /dev/null
@@ -1,108 +0,0 @@
-(*
- * krobot_types.ml
- * ---------------
- * Copyright : (c) 2010, Jeremie Dimino <je...@di...>
- * Licence : BSD3
- *
- * This file is a part of [kro]bot.
- *)
-
-(* Note: integers value must be taken from PcInterface.h *)
-
-let make_map l =
- ((fun x ->
- let rec loop = function
- | [] -> failwith "Krobot_types: invalid value"
- | (x', y) :: _ when x = x' -> y
- | _ :: l -> loop l
- in
- loop l),
- (fun y ->
- let rec loop = function
- | [] -> failwith "Krobot_types: invalid value"
- | (x, y') :: _ when y = y' -> x
- | _ :: l -> loop l
- in
- loop l))
-
-type move_result = [ `OK | `Stopped ]
-
-let int32_of_move_result, move_result_of_int32 =
- make_map [
- `OK, 0l;
- `Stopped, 1l;
- ]
-
-type motor = [ `Left | `Right | `Both ]
-
-let int32_of_motor, motor_of_int32 =
- make_map [
- `Left, Int32.of_int PcInterface.motor_left;
- `Both, Int32.of_int PcInterface.motor_both;
- `Right, Int32.of_int PcInterface.motor_right
- ]
-
-type stop_mode = [ `Off | `Abrupt | `Smooth ]
-
-let int32_of_stop_mode, stop_mode_of_int32 =
- make_map [
- `Off, Int32.of_int PcInterface.traj_stop_motor_off;
- `Abrupt, Int32.of_int PcInterface.traj_stop_abrupt;
- `Smooth, Int32.of_int PcInterface.traj_stop_smooth;
- ]
-
-type card_state = [ `Present | `Absent ]
-
-let int32_of_card_state, card_state_of_int32 =
- make_map [
- `Present, 0l;
- `Absent, 1l;
- ]
-
-type goto_mode = [ `Straight | `Curve_right | `Curve_left ]
-
-let int32_of_goto_mode, goto_mode_of_int32 =
- make_map [
- `Straight, 0l;
- `Curve_right, 1l;
- `Curve_left, 2l;
- ]
-
-type ax12_stats = {
- ax12_position : int;
- ax12_velocity : int;
- ax12_torque : int;
- ax12_voltage : int;
- ax12_temperature : int;
- ax12_cw_angle_limit : int;
- ax12_ccw_angle_limit : int;
-}
-
-type exec_mode = [ `Now | `Action ]
-
-let int32_of_exec_mode, exec_mode_of_int32 =
- make_map [
- `Now, Int32.of_int PcInterface.ax12_exec_now;
- `Action, Int32.of_int PcInterface.ax12_exec_action;
- ]
-
-type direction = [ `Forward | `Backward ]
-
-let int32_of_direction, direction_of_int32 =
- make_map [
- `Forward, 1l;
- `Backward, -1l;
- ]
-
-type ax12_action = {
- aa_id : int;
- aa_position : int;
- aa_velocity : int;
-}
-
-type motor_config = {
- motor_kp : int;
- motor_ki : int;
- motor_kd : int;
- motor_li : int;
-}
diff --git a/info/control/common/krobot_types.mli b/info/control/common/krobot_types.mli
deleted file mode 100644
index 3bc674f..0000000
--- a/info/control/common/krobot_types.mli
+++ /dev/null
@@ -1,80 +0,0 @@
-(*
- * krobot_types.mli
- * ----------------
- * Copyright : (c) 2010, Jeremie Dimino <je...@di...>
- * Licence : BSD3
- *
- * This file is a part of [kro]bot.
- *)
-
-(** Common types *)
-
-(** This types are used by the driver and the client-side krobot
- library. *)
-
-type move_result = [ `OK | `Stopped ]
- (** Result of a movement started by [Krobot.move] or
- [Krobot.turn] *)
-
-val int32_of_move_result : move_result -> int32
-val move_result_of_int32 : int32 -> move_result
-
-type motor = [ `Left | `Right | `Both ]
- (** Motors *)
-
-val int32_of_motor : motor -> int32
-val motor_of_int32 : int32 -> motor
-
-type stop_mode = [ `Off | `Abrupt | `Smooth ]
- (** Mode for stopping motors *)
-
-val int32_of_stop_mode : stop_mode -> int32
-val stop_mode_of_int32 : int32 -> stop_mode
-
-type direction = [ `Forward | `Backward ]
-
-val int32_of_direction : direction -> int32
-val direction_of_int32 : int32 -> direction
-
-type card_state = [ `Present | `Absent ]
- (** State of a card *)
-
-val int32_of_card_state : card_state -> int32
-val card_state_of_int32 : int32 -> card_state
-
-type goto_mode = [ `Straight | `Curve_right | `Curve_left ]
- (** Form of the trajectory for the goto command *)
-
-val int32_of_goto_mode : goto_mode -> int32
-val goto_mode_of_int32 : int32 -> goto_mode
-
-type exec_mode = [ `Now | `Action ]
- (** Mode of execution of the goto command for ax12 *)
-
-val int32_of_exec_mode : exec_mode -> int32
-val exec_mode_of_int32 : int32 -> exec_mode
-
-type ax12_stats = {
- ax12_position : int;
- ax12_velocity : int;
- ax12_torque : int;
- ax12_voltage : int;
- ax12_temperature : int;
- ax12_cw_angle_limit : int;
- ax12_ccw_angle_limit : int;
-}
-
-(** Action on an AX12: *)
-type ax12_action = {
- aa_id : int;
- aa_position : int;
- aa_velocity : int;
-}
-
-(** Motor configuration *)
-type motor_config = {
- motor_kp : int;
- motor_ki : int;
- motor_kd : int;
- motor_li : int;
-}
diff --git a/info/control/common/krobot_util.ml b/info/control/common/krobot_util.ml
deleted file mode 100644
index 382a8c4..0000000
--- a/info/control/common/krobot_util.ml
+++ /dev/null
@@ -1,46 +0,0 @@
-(*
- * krobot_util.ml
- * --------------
- * Copyright : (c) 2010, Jeremie Dimino <je...@di...>
- * Licence : BSD3
- *
- * This file is a part of [kro]bot.
- *)
-
-open Lwt
-
-let front_collide sensors =
- if Array.length sensors <> 16 then invalid_arg "Until.front_collide";
- let rec loop = function
- | 16 -> false
- | n -> (sensors.(n) && List.mem n Krobot_config.front_sensors) || loop (n + 1)
- in
- loop 0
-
-let back_collide sensors =
- if Array.length sensors <> 16 then invalid_arg "Until.back_collide";
- let rec loop = function
- | 16 -> false
- | n -> (sensors.(n) && List.mem n Krobot_config.back_sensors) || loop (n + 1)
- in
- loop 0
-
-let unexpected_reply name reply =
- lwt () = Lwt_log.log_f ~level:Lwt_log.Fatal "unexpected reply for request_name(%S): %s" name reply in
- exit 1
-
-let single_instance bus name =
- Lwt_event.always_notify_p
- (fun name ->
- lwt () = Lwt_log.log ~level:Lwt_log.Notice "service restarted, exiting" in
- exit 0)
- (React.E.filter ((=) name) (OBus_signal.event (OBus_bus.name_lost bus)));
- OBus_bus.request_name bus ~allow_replacement:true ~replace_existing:true name >>= function
- | `Primary_owner ->
- return ()
- | `In_queue ->
- unexpected_reply name "in-queue"
- | `Exists ->
- unexpected_reply name "exists"
- | `Already_owner ->
- unexpected_reply name "already-owner"
diff --git a/info/control/common/krobot_util.mli b/info/control/common/krobot_util.mli
deleted file mode 100644
index 3bcdbc9..0000000
--- a/info/control/common/krobot_util.mli
+++ /dev/null
@@ -1,24 +0,0 @@
-(*
- * krobot_util.mli
- * ---------------
- * Copyright : (c) 2010, Jeremie Dimino <je...@di...>
- * Licence : BSD3
- *
- * This file is a part of [kro]bot.
- *)
-
-(** Utilities *)
-
-val front_collide : bool array -> bool
- (** [front_collide sensors] returns whether on of the front sensors
- is activated *)
-
-val back_collide : bool array -> bool
- (** [front_collide sensors] returns whether on of the back sensors is
- activated *)
-
-val single_instance : OBus_bus.t -> OBus_name.bus -> unit Lwt.t
- (** [single_instance bus name] requests the bus name [name], and
- exit the program when this name is lost. This permit to easily
- restart the service and ensures that there is only one running
- instance. *)
diff --git a/info/control/common/types.ml b/info/control/common/types.ml
new file mode 100644
index 0000000..66845ed
--- /dev/null
+++ b/info/control/common/types.ml
@@ -0,0 +1,108 @@
+(*
+ * types.ml
+ * --------
+ * Copyright : (c) 2010, Jeremie Dimino <je...@di...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+(* Note: integers value must be taken from PcInterface.h *)
+
+let make_map l =
+ ((fun x ->
+ let rec loop = function
+ | [] -> failwith "Krobot_types: invalid value"
+ | (x', y) :: _ when x = x' -> y
+ | _ :: l -> loop l
+ in
+ loop l),
+ (fun y ->
+ let rec loop = function
+ | [] -> failwith "Krobot_types: invalid value"
+ | (x, y') :: _ when y = y' -> x
+ | _ :: l -> loop l
+ in
+ loop l))
+
+type move_result = [ `OK | `Stopped ]
+
+let int32_of_move_result, move_result_of_int32 =
+ make_map [
+ `OK, 0l;
+ `Stopped, 1l;
+ ]
+
+type motor = [ `Left | `Right | `Both ]
+
+let int32_of_motor, motor_of_int32 =
+ make_map [
+ `Left, Int32.of_int PcInterface.motor_left;
+ `Both, Int32.of_int PcInterface.motor_both;
+ `Right, Int32.of_int PcInterface.motor_right
+ ]
+
+type stop_mode = [ `Off | `Abrupt | `Smooth ]
+
+let int32_of_stop_mode, stop_mode_of_int32 =
+ make_map [
+ `Off, Int32.of_int PcInterface.traj_stop_motor_off;
+ `Abrupt, Int32.of_int PcInterface.traj_stop_abrupt;
+ `Smooth, Int32.of_int PcInterface.traj_stop_smooth;
+ ]
+
+type card_state = [ `Present | `Absent ]
+
+let int32_of_card_state, card_state_of_int32 =
+ make_map [
+ `Present, 0l;
+ `Absent, 1l;
+ ]
+
+type goto_mode = [ `Straight | `Curve_right | `Curve_left ]
+
+let int32_of_goto_mode, goto_mode_of_int32 =
+ make_map [
+ `Straight, 0l;
+ `Curve_right, 1l;
+ `Curve_left, 2l;
+ ]
+
+type ax12_stats = {
+ ax12_position : int;
+ ax12_velocity : int;
+ ax12_torque : int;
+ ax12_voltage : int;
+ ax12_temperature : int;
+ ax12_cw_angle_limit : int;
+ ax12_ccw_angle_limit : int;
+}
+
+type exec_mode = [ `Now | `Action ]
+
+let int32_of_exec_mode, exec_mode_of_int32 =
+ make_map [
+ `Now, Int32.of_int PcInterface.ax12_exec_now;
+ `Action, Int32.of_int PcInterface.ax12_exec_action;
+ ]
+
+type direction = [ `Forward | `Backward ]
+
+let int32_of_direction, direction_of_int32 =
+ make_map [
+ `Forward, 1l;
+ `Backward, -1l;
+ ]
+
+type ax12_action = {
+ aa_id : int;
+ aa_position : int;
+ aa_velocity : int;
+}
+
+type motor_config = {
+ motor_kp : int;
+ motor_ki : int;
+ motor_kd : int;
+ motor_li : int;
+}
diff --git a/info/control/common/types.mli b/info/control/common/types.mli
new file mode 100644
index 0000000..eba0912
--- /dev/null
+++ b/info/control/common/types.mli
@@ -0,0 +1,80 @@
+(*
+ * types.mli
+ * ---------
+ * Copyright : (c) 2010, Jeremie Dimino <je...@di...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+(** Common types *)
+
+(** This types are used by the driver and the client-side krobot
+ library. *)
+
+type move_result = [ `OK | `Stopped ]
+ (** Result of a movement started by [Krobot.move] or
+ [Krobot.turn] *)
+
+val int32_of_move_result : move_result -> int32
+val move_result_of_int32 : int32 -> move_result
+
+type motor = [ `Left | `Right | `Both ]
+ (** Motors *)
+
+val int32_of_motor : motor -> int32
+val motor_of_int32 : int32 -> motor
+
+type stop_mode = [ `Off | `Abrupt | `Smooth ]
+ (** Mode for stopping motors *)
+
+val int32_of_stop_mode : stop_mode -> int32
+val stop_mode_of_int32 : int32 -> stop_mode
+
+type direction = [ `Forward | `Backward ]
+
+val int32_of_direction : direction -> int32
+val direction_of_int32 : int32 -> direction
+
+type card_state = [ `Present | `Absent ]
+ (** State of a card *)
+
+val int32_of_card_state : card_state -> int32
+val card_state_of_int32 : int32 -> card_state
+
+type goto_mode = [ `Straight | `Curve_right | `Curve_left ]
+ (** Form of the trajectory for the goto command *)
+
+val int32_of_goto_mode : goto_mode -> int32
+val goto_mode_of_int32 : int32 -> goto_mode
+
+type exec_mode = [ `Now | `Action ]
+ (** Mode of execution of the goto command for ax12 *)
+
+val int32_of_exec_mode : exec_mode -> int32
+val exec_mode_of_int32 : int32 -> exec_mode
+
+type ax12_stats = {
+ ax12_position : int;
+ ax12_velocity : int;
+ ax12_torque : int;
+ ax12_voltage : int;
+ ax12_temperature : int;
+ ax12_cw_angle_limit : int;
+ ax12_ccw_angle_limit : int;
+}
+
+(** Action on an AX12: *)
+type ax12_action = {
+ aa_id : int;
+ aa_position : int;
+ aa_velocity : int;
+}
+
+(** Motor configuration *)
+type motor_config = {
+ motor_kp : int;
+ motor_ki : int;
+ motor_kd : int;
+ motor_li : int;
+}
diff --git a/info/control/common/util.ml b/info/control/common/util.ml
new file mode 100644
index 0000000..d6e90ec
--- /dev/null
+++ b/info/control/common/util.ml
@@ -0,0 +1,46 @@
+(*
+ * util.ml
+ * -------
+ * Copyright : (c) 2010, Jeremie Dimino <je...@di...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+open Lwt
+
+let front_collide sensors =
+ if Array.length sensors <> 16 then invalid_arg "Until.front_collide";
+ let rec loop = function
+ | 16 -> false
+ | n -> (sensors.(n) && List.mem n Config.front_sensors) || loop (n + 1)
+ in
+ loop 0
+
+let back_collide sensors =
+ if Array.length sensors <> 16 then invalid_arg "Until.back_collide";
+ let rec loop = function
+ | 16 -> false
+ | n -> (sensors.(n) && List.mem n Config.back_sensors) || loop (n + 1)
+ in
+ loop 0
+
+let unexpected_reply name reply =
+ lwt () = Lwt_log.log_f ~level:Lwt_log.Fatal "unexpected reply for request_name(%S): %s" name reply in
+ exit 1
+
+let single_instance bus name =
+ Lwt_event.always_notify_p
+ (fun name ->
+ lwt () = Lwt_log.log ~level:Lwt_log.Notice "service restarted, exiting" in
+ exit 0)
+ (React.E.filter ((=) name) (OBus_signal.event (OBus_bus.name_lost bus)));
+ OBus_bus.request_name bus ~allow_replacement:true ~replace_existing:true name >>= function
+ | `Primary_owner ->
+ return ()
+ | `In_queue ->
+ unexpected_reply name "in-queue"
+ | `Exists ->
+ unexpected_reply name "exists"
+ | `Already_owner ->
+ unexpected_reply name "already-owner"
diff --git a/info/control/common/util.mli b/info/control/common/util.mli
new file mode 100644
index 0000000..36ad923
--- /dev/null
+++ b/info/control/common/util.mli
@@ -0,0 +1,24 @@
+(*
+ * util.mli
+ * --------
+ * Copyright : (c) 2010, Jeremie Dimino <je...@di...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+(** Utilities *)
+
+val front_collide : bool array -> bool
+ (** [front_collide sensors] returns whether on of the front sensors
+ is activated *)
+
+val back_collide : bool array -> bool
+ (** [front_collide sensors] returns whether on of the back sensors is
+ activated *)
+
+val single_instance : OBus_bus.t -> OBus_name.bus -> unit Lwt.t
+ (** [single_instance bus name] requests the bus name [name], and
+ exit the program when this name is lost. This permit to easily
+ restart the service and ensures that there is only one running
+ instance. *)
diff --git a/info/control/config.ml b/info/control/config.ml
new file mode 100644
index 0000000..c248d6e
--- /dev/null
+++ b/info/control/config.ml
@@ -0,0 +1,15 @@
+(*
+ * config.ml
+ * ---------
+ * Copyright : (c) 2010, Jeremie Dimino <je...@di...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+let back_sensors = [3; 6; 7; 10]
+let front_sensors = [0; 1; 2; 4; 5; 8; 9; 11; 12; 13; 14; 15]
+let initial_position = 200
+let bus_address = "unix:abstract=krobot"
+let update_delay = 0.05
+let reopen_delay = 1.0
diff --git a/info/control/config.mli b/info/control/config.mli
new file mode 100644
index 0000000..645ac10
--- /dev/null
+++ b/info/control/config.mli
@@ -0,0 +1,28 @@
+(*
+ * config.mli
+ * ----------
+ * Copyright : (c) 2010, Jeremie Dimino <je...@di...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+(** Krobot parameters *)
+
+val front_sensors : int list
+ (** List of front sensors *)
+
+val back_sensors : int list
+ (** List of back sensors *)
+
+val initial_position : int
+ (** Position to borders at the beginning of the match *)
+
+val bus_address : string
+ (** Default address of the krobot dbus daemon *)
+
+val update_delay : float
+ (** Time to wait between updates *)
+
+val reopen_delay : float
+ (** Time to wait before retrying to open a card *)
diff --git a/info/control/driver/USBCard.ml b/info/control/driver/USBCard.ml
new file mode 100644
index 0000000..06098d8
--- /dev/null
+++ b/info/control/driver/USBCard.ml
@@ -0,0 +1,503 @@
+(*
+ * USBCard.ml
+ * ----------
+ * Copyright : (c) 2009-2010, Jeremie Dimino <je...@di...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+let section = Lwt_log.Section.make "card"
+
+open Lwt
+
+let error_messages = [
+ PcInterface.err_unknown_cmd, "unknown command";
+ PcInterface.err_unknown_get, "unknown get request";
+ PcInterface.err_unknown_set, "unknown set request";
+ PcInterface.err_invalid_response, "invalid response";
+ PcInterface.err_ax12_wrong_packet, "invalid AX12 packet";
+ PcInterface.err_ax12_error, "AX12 error";
+ PcInterface.err_ax12_chksum, "invalid checksum of AX12 packet";
+ PcInterface.err_cmp03_not_responding, "cmp03 not responding";
+ PcInterface.err_adjd_s371_not_responding, "adjd_s371 not responding";
+ PcInterface.err_lm_command_error, "lm command error";
+ PcInterface.err_lm_position_error, "lm position error";
+ PcInterface.err_invalid_axis, "invalid axis";
+]
+
+let error_message error =
+ try
+ List.assoc error error_messages
+ with Not_found ->
+ Printf.sprintf "unknown error (%d)" error
+
+exception Error of string
+
+(* +-----------------------------------------------------------------+
+ | Serialization |
+ +-----------------------------------------------------------------+ *)
+
+type pointer = {
+ mutable offset : int;
+ buffer : string;
+}
+
+let get_uint8 pointer =
+ let offset = pointer.offset in
+ pointer.offset <- offset + 1;
+ Char.code pointer.buffer.[offset]
+let put_uint8 pointer value =
+ let offset = pointer.offset in
+ pointer.offset <- offset + 1;
+ pointer.buffer.[offset] <- Char.unsafe_chr value
+let get_sint8 = get_uint8
+let put_sint8 = put_uint8
+
+let get_sint16 pointer =
+ let v0 = get_uint8 pointer in
+ let v1 = get_uint8 pointer in
+ (v0 lsl 8) lor v1
+let get_uint16 = get_sint16
+
+let put_sint16 pointer value =
+ put_uint8 pointer ((value lsr 8) land 0xff);
+ put_uint8 pointer (value land 0xff)
+let put_uint16 = put_sint16
+
+let get_sint32 pointer =
+ let v0 = get_uint8 pointer in
+ let v1 = get_uint8 pointer in
+ let v2 = get_uint8 pointer in
+ let v3 = get_uint8 pointer in
+ (v0 lsl 24) lor (v1 lsl 16) lor (v2 lsl 8) lor v3
+let get_uint32 = get_sint32
+
+let put_sint32 pointer value =
+ put_uint8 pointer ((value lsr 24) land 0xff);
+ put_uint8 pointer ((value lsr 16) land 0xff);
+ put_uint8 pointer ((value lsr 8) land 0xff);
+ put_uint8 pointer (value land 0xff)
+let put_uint32 = put_sint32
+
+let get_string pointer =
+ let index =
+ try
+ String.index_from pointer.buffer pointer.offset '\000'
+ with Not_found ->
+ String.length pointer.buffer
+ in
+ let offset = pointer.offset in
+ pointer.offset <- index + 1;
+ String.sub pointer.buffer offset (index - offset)
+
+let put_string pointer value =
+ let len = String.length value in
+ if len > String.length pointer.buffer - pointer.offset then
+ invalid_arg "RW.put_string: string too long"
+ else begin
+ String.blit value 0 pointer.buffer pointer.offset len;
+ let offset = pointer.offset + len in
+ if offset < String.length pointer.buffer then begin
+ pointer.buffer.[offset] <- '\x00';
+ pointer.offset <- offset + 1
+ end else
+ pointer.offset <- offset
+ end
+
+open Value
+
+let rec put_single ptr = function
+ | V.Int(Sint8, x) -> put_sint8 ptr x
+ | V.Int(Uint8, x) -> put_uint8 ptr x
+ | V.Int(Sint16, x) -> put_sint16 ptr x
+ | V.Int(Uint16, x) -> put_uint16 ptr x
+ | V.Int(Sint32, x) -> put_sint32 ptr x
+ | V.Int(Uint32, x) -> put_uint32 ptr x
+ | V.Boolean true -> put_uint8 ptr 1
+ | V.Boolean false -> put_uint8 ptr 0
+ | V.String str -> put_string ptr str
+ | V.Array a -> Array.iter (put_single ptr) a
+
+let put_sequence ptr l =
+ List.iter (put_single ptr) l
+
+let rec get_single ptr = function
+ | T.Int Sint8 | T.Enum(Sint8, _) | T.Bit_mask(Sint8, _) -> V.Int(Sint8, get_sint8 ptr)
+ | T.Int Uint8 | T.Enum(Uint8, _) | T.Bit_mask(Uint8, _) -> V.Int(Uint8, get_uint8 ptr)
+ | T.Int Sint16 | T.Enum(Sint16, _) | T.Bit_mask(Sint16, _) -> V.Int(Sint16, get_sint16 ptr)
+ | T.Int Uint16 | T.Enum(Uint16, _) | T.Bit_mask(Uint16, _) -> V.Int(Uint16, get_uint16 ptr)
+ | T.Int Sint32 | T.Enum(Sint32, _) | T.Bit_mask(Sint32, _) -> V.Int(Sint32, get_sint32 ptr)
+ | T.Int Uint32 | T.Enum(Uint32, _) | T.Bit_mask(Uint32, _) -> V.Int(Uint32, get_uint32 ptr)
+ | T.Boolean -> V.Boolean(get_uint8 ptr <> 0)
+ | T.String -> V.String(get_string ptr)
+ | T.Array(size, typ) -> V.Array(Array.init size (fun _ -> get_single ptr typ))
+
+let rec get_sequence ptr = function
+ | [] -> []
+ | t :: l -> get_single ptr t :: get_sequence ptr l
+
+(* +-----------------------------------------------------------------+
+ | Messages |
+ +-----------------------------------------------------------------+ *)
+
+let data_length = 52
+ (* Taille en octet du corps d'un message *)
+
+type serial = int
+ (* Type d'un numéro de série d'un message *)
+
+type message = {
+ host_serial : serial;
+ (* Le numéro de série du message, émis par l'ordinateur. Vaut 0 pour
+ les messages émis par le PIC. *)
+
+ device_serial : serial;
+ (* Le numéro de série du message, émis par le PIC. Vaut 0 pour les
+ messages émis par l'ordinateur. *)
+
+ command : int;
+ (* La commande, en fait c'est plutôt le type du message *)
+
+ error : int;
+ (* Si c'est un message d'erreur ce flag est non-nul *)
+
+ data : string;
+ (* Les données du messages, il y a 52 octets. *)
+}
+
+let make_buffer () = String.make data_length '\000'
+
+(* Parse un message depuis un buffer brut: *)
+let parse_message buf = {
+ host_serial = Char.code buf.[PcInterface.up_hseq];
+ device_serial = Char.code buf.[PcInterface.up_dseq];
+ command = Char.code buf.[PcInterface.up_cmd];
+ error = Char.code buf.[PcInterface.up_err];
+ data = String.sub buf PcInterface.up_data 52;
+}
+
+(* Créé un buffer brut depuis un message: *)
+let forge_message msg =
+ let buf = String.make 64 '\000' in
+ buf.[PcInterface.up_hseq] <- Char.chr msg.host_serial;
+ buf.[PcInterface.up_dseq] <- Char.chr msg.device_serial;
+ buf.[PcInterface.up_cmd] <- Char.chr msg.command;
+ buf.[PcInterface.up_err] <- Char.chr msg.error;
+ if String.length msg.data > 52 then
+ Printf.ksprintf invalid_arg "message body too big: %d > 52" (String.length msg.data)
+ else begin
+ String.blit msg.data 0 buf PcInterface.up_data (String.length msg.data);
+ buf
+ end
+
+(* +-----------------------------------------------------------------+
+ | Definitions |
+ +-----------------------------------------------------------------+ *)
+
+exception Card_closed
+exception Card_crashed of string
+
+module Int_map = Map.Make(struct type t = int let compare = compare end)
+
+(* Type of a up and running card *)
+type card = {
+ mutable serial_pool : serial list;
+ (* Pool de serial disponibles, comme il n'y a que 256 serial
+ disponibles on évite de juste incrémenter un compteur au pif. *)
+
+ mutable reply_waiters : string Lwt.u Int_map.t;
+ (* Threads en attente d'une réponse *)
+
+ handle : USB.handle;
+ (* Handle pour le périphérique usb *)
+
+ kernel_active : bool;
+ (* Est-ce qu'un driver noyau était attaché à la carte avant qu'on
+ l'utilise ? *)
+
+ mutex : Lwt_mutex.t;
+ (* Mutex pour envoyer des commandes, les cartes n'aiment pas les
+ appels parallèles. *)
+
+ abort_waiter : int Lwt.t;
+ abort_wakener : int Lwt.u;
+ (* Sleeping thread which is wakeup when the card is closed *)
+
+ errors : string React.event;
+ push_error : string -> unit;
+
+ commands : (int * string) React.event;
+ push_command : int * string -> unit;
+
+ wrapper : wrapper;
+ (* The associated wrapper *)
+}
+
+and state =
+ | Opened of card
+ | Closed of exn
+
+and wrapper = {
+ mutable state : state;
+ name : string;
+ watch : [ `Error of exn | `Closed ] Lwt.t;
+}
+
+type t = wrapper
+
+let name wrapper = wrapper.name
+let closed wrapper = match wrapper.state with
+ | Opened _ -> false
+ | Closed _ -> true
+
+let watch wrapper = wrapper.watch
+
+(* Return a running card, if possible. *)
+let get_card wrapper = match wrapper.state with
+ | Opened card ->
+ return card
+ | Closed exn ->
+ fail exn
+
+let errors wrapper = match wrapper.state with
+ | Opened card -> card.errors
+ | Closed exn -> raise exn
+
+let commands wrapper = match wrapper.state with
+ | Opened card -> card.commands
+ | Closed exn -> raise exn
+
+(* +-----------------------------------------------------------------+
+ | Aborting |
+ +-----------------------------------------------------------------+ *)
+
+let abort wrapper exn =
+ match wrapper.state with
+ | Closed exn ->
+ return exn
+ | Opened card ->
+ wrapper.state <- Closed exn;
+ try_lwt
+ lwt () = USB.release_interface card.handle 0 in
+ if card.kernel_active then USB.attach_kernel_driver card.handle 0;
+ USB.close card.handle;
+ return exn
+ finally
+ wakeup_exn card.abort_wakener exn;
+ Int_map.iter (fun serial w -> wakeup_exn w exn) card.reply_waiters;
+ return ()
+
+(* +-----------------------------------------------------------------+
+ | Dispatching |
+ +-----------------------------------------------------------------+ *)
+
+let dropped typ card msg =
+ lwt () = Lwt_log.warning_f ~section "%s dropped on card %s" typ card.wrapper.name in
+ lwt () = Lwt_log.warning_f ~section "=====
+host_serial = %d
+device_serial = %d
+command = %d
+error = %s
+data:" msg.host_serial msg.device_serial msg.command (if msg.error <> 0 then error_message msg.error else "none") in
+ Lwt_stream.iter_s (fun line -> Lwt_log.warning ~section line) (Lwt_stream.hexdump (Lwt_stream.of_string msg.data))
+
+(* Dispatch incomming messages continously *)
+let rec dispatch card =
+ let buffer = String.create 64 in
+ begin
+ try_lwt
+ pick [card.abort_waiter;
+ USB.interrupt_recv
+ ~handle:card.handle
+ ~endpoint:1
+ buffer 0 64] >|= fun len -> `OK len
+ with exn ->
+ return (`Error exn)
+ end >>= function
+ | `Error exn ->
+ lwt () = Lwt_log.error_f ~section ~exn "stop dispatching on %s card" card.wrapper.name in
+ lwt _ = abort card.wrapper exn in
+ return ()
+ | `OK len ->
+ if len <> 64 then begin
+ let msg = Printf.sprintf "read on %s card returned %d instead of 64" card.wrapper.name len in
+ lwt () = Lwt_log.error ~section msg in
+ lwt _ = abort card.wrapper (Card_crashed msg) in
+ return ()
+ end else begin
+ let msg = parse_message buffer in
+ if msg.error <> 0 then begin
+ if msg.command = PcInterface.cmd_respond then
+ card.push_error ("response: " ^ error_message msg.error)
+ else
+ card.push_error ("spontaneous: " ^ error_message msg.error)
+ end;
+ if msg.command = PcInterface.cmd_respond then begin
+ match try Some(Int_map.find msg.host_serial card.reply_waiters) with Not_found -> None with
+ | Some wakener ->
+ card.reply_waiters <- Int_map.remove msg.host_serial card.reply_waiters;
+ card.serial_pool <- card.serial_pool @ [msg.host_serial];
+ if msg.error <> 0 then
+ Lwt.wakeup_exn wakener (Error(error_message msg.error))
+ else
+ Lwt.wakeup wakener msg.data
+ | None ->
+ ignore (dropped "response" card msg)
+ end else begin
+ try
+ card.push_command (msg.command, msg.data)
+ with exn ->
+ ignore (Lwt_log.error_f ~section ~exn "pushing event %d from %s card failed with" msg.command card.wrapper.name)
+ end;
+ dispatch card
+ end
+
+(* +-----------------------------------------------------------------+
+ | Opening and closing |
+ +-----------------------------------------------------------------+ *)
+
+let close wrapper = match wrapper.state with
+ | Opened _ ->
+ lwt _ = abort wrapper Card_closed in
+ return ()
+ | Closed _ ->
+ return ()
+
+let rec make ~name ~handle =
+ lwt () = USB.reset_device handle in
+ let kernel_active = USB.kernel_driver_active handle 0 in
+ if kernel_active then USB.detach_kernel_driver handle 0;
+ lwt () = USB.set_configuration handle 1 in
+ lwt () = USB.claim_interface handle 0 in
+ let abort_waiter, abort_wakener = wait () in
+ let errors, push_error = React.E.create () in
+ let commands, push_command = React.E.create () in
+ let rec card = {
+ serial_pool = (let rec loop = function
+ | 256 -> []
+ | n -> n :: loop (n + 1)
+ in
+ loop 1);
+ reply_waiters = Int_map.empty;
+ handle = handle;
+ kernel_active = kernel_active;
+ mutex = Lwt_mutex.create ();
+ abort_waiter = abort_waiter;
+ abort_wakener = abort_wakener;
+ wrapper = wrapper;
+ errors = errors;
+ push_error = push_error;
+ commands = commands;
+ push_command = push_command;
+ } and wrapper = {
+ state = Opened card;
+ name = name;
+ watch = (try_lwt
+ lwt _ = abort_waiter in
+ (* Never happen: *)
+ return `Closed
+ with
+ | Card_closed ->
+ return `Closed
+ | exn ->
+ return (`Error exn))
+ } in
+ ignore (dispatch card);
+ return wrapper
+
+(* +-----------------------------------------------------------------+
+ | Sending/receiving messages |
+ +-----------------------------------------------------------------+ *)
+
+let send card buffer =
+ lwt len = pick [card.abort_waiter; USB.interrupt_send ~handle:card.handle ~endpoint:1 buffer 0 64] in
+ if len <> 64 then begin
+ let msg = Printf.sprintf "write on %s card returned %d instead of 64" card.wrapper.name len in
+ lwt () = Lwt_log.error ~section msg in
+ fail =<< abort card.wrapper (Card_crashed msg)
+ end else
+ return ()
+
+(* Send a command without waiting for the reply: *)
+let call_without_reply cmd wrapper x =
+ let data = make_buffer () in
+ let ptr = { buffer = data; offset = 0 } in
+ let () =
+ match cmd.Commands.request with
+ | Some r ->
+ put_uint8 ptr r
+ | None ->
+ ()
+ in
+ put_sequence ptr (Value.C.make_sequence (Value.arg_types cmd.Commands.send) x);
+ lwt card = get_card wrapper in
+ let buffer = forge_message { host_serial = 0;
+ device_serial = 0;
+ command = cmd.Commands.command;
+ error = 0;
+ data = data } in
+ try_lwt
+ Lwt_mutex.with_lock card.mutex
+ (fun () ->
+ lwt () = send card buffer in
+ return (Value.C.cast_sequence (Value.arg_types cmd.Commands.recv) []))
+ with
+ | Canceled ->
+ fail Canceled
+ | exn ->
+ lwt () = Lwt_log.error_f ~section ~exn "write to %s card failed with" wrapper.name in
+ fail =<< abort wrapper exn
+
+(* Send a command and wait for the response: *)
+let call_with_reply cmd wrapper x =
+ let data = make_buffer () in
+ let ptr = { buffer = data; offset = 0 } in
+ let () =
+ match cmd.Commands.request with
+ | Some r ->
+ put_uint8 ptr r
+ | None ->
+ ()
+ in
+ put_sequence ptr (Value.C.make_sequence (Value.arg_types cmd.Commands.send) x);
+ lwt card = get_card wrapper in
+ let serial = match card.serial_pool with
+ | [] ->
+ failwith "Card.send_command_with_reply: no more serial available!"
+ | serial :: rest ->
+ card.serial_pool <- rest;
+ serial
+ in
+ let waiter, wakener = Lwt.task () in
+ card.reply_waiters <- Int_map.add serial wakener card.reply_waiters;
+ on_cancel waiter (fun () ->
+ card.reply_waiters <- Int_map.remove serial card.reply_waiters;
+ card.serial_pool <- card.serial_pool @ [serial]);
+ let buffer = forge_message { host_serial = serial;
+ device_serial = 0;
+ command = cmd.Commands.command;
+ error = 0;
+ data = data } in
+ try_lwt
+ Lwt_mutex.with_lock card.mutex
+ (fun () ->
+ lwt () = send card buffer in
+ lwt data = waiter in
+ let ptr = { buffer = data; offset = 0 } in
+ return (Value.C.cast_sequence (Value.arg_types cmd.Commands.recv)
+ (get_sequence ptr
+ (Value.C.type_sequence
+ (Value.arg_types cmd.Commands.recv)))))
+ with
+ | Canceled ->
+ fail Canceled
+ | exn ->
+ lwt () = Lwt_log.error_f ~section ~exn "write to %s card failed with" wrapper.name in
+ fail =<< abort wrapper exn
+
+let call cmd wrapper x =
+ if cmd.Commands.response then
+ call_with_reply cmd wrapper x
+ else
+ call_without_reply cmd wrapper x
diff --git a/info/control/driver/USBCard.mli b/info/control/driver/USBCard.mli
new file mode 100644
index 0000000..b5fdd0d
--- /dev/null
+++ b/info/control/driver/USBCard.mli
@@ -0,0 +1,56 @@
+(*
+ * USBCard.mli
+ * -----------
+ * Copyright : (c) 2009-2010, Jeremie Dimino <je...@di...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+(** Lowlevel card interfaces *)
+
+type t
+ (** Type of a card *)
+
+val name : t -> string
+ (** Returns the name of a card. It can be applied on a closed
+ card. *)
+
+val closed : t -> bool
+ (** Returns [true] iff the card has been closed *)
+
+val watch : t -> [ `Error of exn | `Closed ] Lwt.t
+ (** [watch card] is a thread which is wakeup when the card is
+ closed, or when a fatal error happen. The argument describe the
+ reason. *)
+
+exception Card_closed
+ (** Exception raised when trying to use a closed card *)
+
+exception Card_crashed of string
+ (** Exception raised when a fatal error happen on the card *)
+
+exception Error of string
+ (** An error returned by a card *)
+
+(** {6 Card opening/closing} *)
+
+val make : name : string -> handle : USB.handle -> t Lwt.t
+ (** [make ~name ~handle] creates a card using the given USB
+ handle. [name] is used for debug messages. *)
+
+val close : t -> unit Lwt.t
+ (** Close the given card *)
+
+(** {6 Sending/receving messages} *)
+
+val call : ('a, 'b) Commands.t -> t -> 'a -> 'b Lwt.t
+ (** Call a command on a device*)
+
+val commands : t -> (int * string) React.event
+ (** [comamnds card] returns the event which occurs each a message is
+ received from the card *)
+
+val errors : t -> string React.event
+ (** [errors card] is an event which occurs each time a card send an
+ error *)
diff --git a/info/control/driver/driver.ml b/info/control/driver/driver.ml
new file mode 100644
index 0000000..70fbde6
--- /dev/null
+++ b/info/control/driver/driver.ml
@@ -0,0 +1,1009 @@
+(*
+ * driver.ml
+ * ---------
+ * Copyright : (c) 2009-2010, Jeremie Dimino <je...@di...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+(* Driver for USB cards *)
+
+let section = Lwt_log.Section.make "driver"
+
+open Types
+open Lwt
+
+let make_signal ?(update_delay=Config.update_delay) get card =
+ lwt value = get card in
+ return (React.S.hold value (Lwt_event.from
+ (fun () ->
+ lwt () = Lwt_unix.sleep update_delay in
+ get card)))
+
+(* THe notification mode for all interfaces: *)
+let notify_mode () = OBus_object.notify_update "PropertiesChanged"
+
+(* +-----------------------------------------------------------------+
+ | Power |
+ +-----------------------------------------------------------------+ *)
+
+module Power =
+struct
+ type t = {
+ obus : t OBus_object.t;
+ card : USBCard.t;
+ }
+
+ let make card path =
+ let dev = {
+ obus =
+ OBus_object.make
+ ~interfaces:[Export_unsafe.interface "power" (fun dev -> dev.card)]
+ path;
+ card = card;
+ } in
+ OBus_object.attach dev.obus dev;
+ return dev
+end
+
+(* +-----------------------------------------------------------------+
+ | Compass |
+ +-----------------------------------------------------------------+ *)
+
+module Compass =
+struct
+ type t = {
+ obus : t OBus_object.t;
+ card : USBCard.t;
+ value : int React.signal;
+ }
+
+ let make card path =
+ lwt value = make_signal ~update_delay:(Config.update_delay *. 2.) (fun card -> snd =|< USBCard.call Commands.Compass.get card ()) card in
+ let dev = {
+ obus =
+ OBus_object.make
+ ~interfaces:[Export_unsafe.interface "compass" (fun dev -> dev.card);
+ Krobot_interfaces.Fr_krobot_Device_Compass.make
+ ~notify_mode:(OBus_object.notify_update "PropertiesChanged")
+ ~p_Value:(fun dev -> React.S.map Int32.of_int dev.value)
+ ()]
+ path;
+ card = card;
+ value = value;
+ } in
+ OBus_object.attach dev.obus dev;
+ return dev
+end
+
+(* +-----------------------------------------------------------------+
+ | LCD |
+ +-----------------------------------------------------------------+ *)
+
+module LCD =
+struct
+ type t = {
+ obus : t OBus_object.t;
+ card : USBCard.t;
+ }
+
+ let set_lcd dev lines =
+ if List.length lines > 4 || List.exists (fun line -> String.length line > 20) lines then
+ invalid_arg "SetLCD"
+ else begin
+ lwt () = USBCard.call Commands.LCD.clear dev.card () in
+ lwt () = USBCard.call Commands.LCD.cursor_off dev.card () in
+ let rec loop i = function
+ | [] ->
+ return ()
+ | line :: lines ->
+ lwt () = USBCard.call Commands.LCD.write_line dev.card (i, line) in
+ loop (i + 1) lines
+ in
+ loop 1 lines
+ end
+
+ let backlight_on dev = USBCard.call Commands.LCD.backlight_on dev.card ()
+ let backlight_off dev = USBCard.call Commands.LCD.backlight_off dev.card ()
+
+ let make card path =
+ let dev = {
+ obus =
+ OBus_object.make
+ ~interfaces:[Export_unsafe.interface "LCD" (fun dev -> dev.card);
+ Krobot_interfaces.Fr_krobot_Device_LCD.make
+ ~notify_mode:(OBus_object.notify_update "PropertiesChanged")
+ ~m_SetLCD:(fun ctx -> set_lcd)
+ ~m_BacklightOn:(fun ctx obj () -> backlight_on obj)
+ ~m_BacklightOff:(fun ctx obj () -> backlight_off obj)
+ ()]
+ path;
+ card = card;
+ } in
+ OBus_object.attach dev.obus dev;
+ return dev
+end
+
+(* +-----------------------------------------------------------------+
+ | Servo |
+ +-----------------------------------------------------------------+ *)
+
+module Servo =
+struct
+ type t = {
+ obus : t OBus_object.t;
+ card : USBCard.t;
+ }
+
+ let claws_enable dev =
+ USBCard.call Commands.Servo.set_config dev.card (0b10100, 0)
+
+ let claws_disable dev =
+ USBCard.call Commands.Servo.set_config dev.card (0, 0xff)
+
+ let claws_open dev =
+ USBCard.call Commands.Servo.set_state dev.card (0b10100, 0, 0, 18, 0, -69)
+
+ let claws_close dev =
+ USBCard.call Commands.Servo.set_state dev.card (0b10100, 0, 0, -100, 0, 45)
+
+ let claws_take dev =
+ USBCard.call Commands.Servo.set_state dev.card (0b10100, 0, 0, -40, 0, -20)
+
+ let make card path =
+ let dev = {
+ obus =
+ OBus_object.make
+ ~interfaces:[Export_unsafe.interface "servo" (fun dev -> dev.card);
+ Krobot_interfaces.Fr_krobot_Device_Servo.make
+ ~notify_mode:(OBus_object.notify_update "PropertiesChanged")
+ ~m_ClawsEnable:(fun ctx obj () -> claws_enable obj)
+ ~m_ClawsDisable:(fun ctx obj () -> claws_disable obj)
+ ~m_ClawsOpen:(fun ctx obj () -> claws_open obj)
+ ~m_ClawsClose:(fun ctx obj () -> claws_close obj)
+ ~m_ClawsTake:(fun ctx obj () -> claws_take obj)
+ ()]
+ path;
+ card = card;
+ } in
+ OBus_object.attach dev.obus dev;
+ return dev
+end
+
+(* +-----------------------------------------------------------------+
+ | AX12 |
+ +-----------------------------------------------------------------+ *)
+
+module AX12 =
+struct
+ type t = {
+ obus : t OBus_object.t;
+ card : USBCard.t;
+ }
+
+ let grip_up_position = 880
+ let grip_down_position = 580
+ let ax12_default_velocity = 50
+
+ (* +---------------------------------------------------------------+
+ | High-level commands |
+ +---------------------------------------------------------------+ *)
+
+ type ax12_action = {
+ aa_id : int;
+ aa_position : int;
+ aa_velocity : int;
+ }
+
+ let set_ax12 dev actions =
+ lwt () =
+ Lwt_list.iter_p
+ (fun action ->
+ USBCard.call Commands.AX12.goto
+ dev.card
+ (action.aa_id,
+ action.aa_position,
+ action.aa_velocity,
+ `Action))
+ actions
+ in
+ USBCard.call Commands.AX12.action dev.card 0xfe
+
+ let grip_up dev =
+ set_ax12 dev [{ aa_id = 1;
+ aa_position = 880;
+ aa_velocity = 50 };
+ { aa_id = 2;
+ aa_position = 180;
+ aa_velocity = 50 }]
+
+ let grip_down dev =
+ set_ax12 dev [{ aa_id = 1;
+ aa_position = 580;
+ aa_velocity = 50 };
+ { aa_id = 2;
+ aa_position = 510;
+ aa_velocity = 50 };
+ { aa_id = 3;
+ aa_position = 390;
+ aa_velocity = 200 }]
+
+ let grip_open dev =
+ set_ax12 dev [{ aa_id = 2;
+ aa_position = 610;
+ aa_velocity = 100 };
+ { aa_id = 3;
+ aa_position = 265;
+ aa_velocity = 200 }]
+
+ let grip_close dev =
+ set_ax12 dev [{ aa_id = 2;
+ aa_position = 510;
+ aa_velocity = 100 };
+ { aa_id = 3;
+ aa_position = 390;
+ aa_velocity = 200 }]
+
+ let grip_release dev =
+ USBCard.call Commands.AX12.goto dev.card (3, 200, 200, `Now)
+
+ let make card path =
+ let dev = {
+ obus =
+ OBus_object.make
+ ~interfaces:[Export_unsafe.interface "AX12" (fun dev -> dev.card);
+ Krobot_interfaces.Fr_krobot_Device_AX12.make
+ ~notify_mode:(OBus_object.notify_update "PropertiesChanged")
+ ~m_SetAX12:(fun ctx obj positions ->
+ let positions =
+ List.map
+ (fun (x1, x2, x3) ->
+ { aa_id = Int32.to_int x1;
+ aa_position = Int32.to_int x2;
+ aa_velocity = Int32.to_int x3 })
+ positions
+ in
+ ...
[truncated message content] |
