From: Xavier L. <Ba...@us...> - 2013-04-18 15:21:12
|
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 06a1b0b57845e008428267506e5d70a34da92261 (commit) via 68147d413ca6994b81ac03b00beac5a52c77c325 (commit) via 2366595d8df4e04ad9ff413a31af4cc705f1fc30 (commit) from a10ecb5f43d6247ced5b9ee5ac1ef8909e9f92a7 (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 06a1b0b57845e008428267506e5d70a34da92261 Author: Xavier Lagorce <Xav...@cr...> Date: Thu Apr 18 17:24:27 2013 +0200 [SensorActuator] Take into account new simulation mode message commit 68147d413ca6994b81ac03b00beac5a52c77c325 Author: Xavier Lagorce <Xav...@cr...> Date: Thu Apr 18 17:11:10 2013 +0200 [ControllerMotorSTM32] Take into account new simulation mode message commit 2366595d8df4e04ad9ff413a31af4cc705f1fc30 Author: Xavier Lagorce <Xav...@cr...> Date: Thu Apr 18 16:55:27 2013 +0200 [info/krobot-simulator] Changed mode message to include more simulation modes ----------------------------------------------------------------------- Changes: diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/can_messages.h b/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/can_messages.h index 156bf47..0b2cc5d 100644 --- a/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/can_messages.h +++ b/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/can_messages.h @@ -31,7 +31,7 @@ #define CAN_MSG_TURN 202 // turn_can_msg_t #define CAN_MSG_ODOMETRY_SET 203 // odometry_can_msg_t #define CAN_MSG_STOP 204 // stop_can_msg_t -#define CAN_MSG_CONTROLLER_MODE 205 // controller_mode_can_msg_t +#define CAN_MSG_SIMULATION_MODE 205 // simulation_mode_can_msg_t #define CAN_MSG_BEZIER_ADD 206 // bezier_can_msg_t #define CAN_MSG_BEZIER_LIMITS 207 // bezier_limits_can_msg_t #define CAN_MSG_MOTOR_COMMAND 208 // motor_command_can_msg_t @@ -42,6 +42,13 @@ #define CAN_MSG_DRIVE_TORQUE_LIMIT 213 // drive_torque_limit_can_msg_t /* +-----------------------------------------------------------------+ + | Constants for messages | + +-----------------------------------------------------------------+ */ +#define SIMULATION_MODE_NO 0 +#define SIMULATION_MODE_NORMAL 1 +#define SIMULATION_MODE_HIL 2 + +/* +-----------------------------------------------------------------+ | CAN messages data structures | +-----------------------------------------------------------------+ */ @@ -148,10 +155,10 @@ typedef struct { float rot_acc __attribute__((__packed__)); // Rotational acceleration for braking } stop_msg_t; -// Select robot mode (normal or HIL) +// Select robot mode (normal, simulation or HIL) typedef struct { uint8_t mode; -} controller_mode_msg_t; +} simulation_mode_msg_t; // Command the speed of a particular motor typedef struct { @@ -239,9 +246,9 @@ typedef union { } stop_can_msg_t; typedef union { - controller_mode_msg_t data; + simulation_mode_msg_t data; uint32_t data32[2]; -} controller_mode_can_msg_t; +} simulation_mode_can_msg_t; typedef union { motor_command_msg_t data; diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/can_monitor.c b/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/can_monitor.c index 3d83b64..ee2d2b1 100644 --- a/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/can_monitor.c +++ b/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/can_monitor.c @@ -10,9 +10,10 @@ #include "can_monitor.h" #include "hw/hw_led.h" -#define ROBOT_MODE_NORMAL 0 -#define ROBOT_MODE_HIL 1 -#define ROBOT_MODE_FAULT 2 +#define ROBOT_MODE_NORMAL 0 +#define ROBOT_MODE_HIL 1 +#define ROBOT_MODE_FAULT 2 +#define ROBOT_MODE_SIMULATION 3 #define CONTROL_ODOMETRY 0 @@ -89,9 +90,9 @@ static void NORETURN canMonitor_process(void) { txm.eid = CAN_MSG_ENCODERS34; can_transmit(CAND1, &txm, ms_to_ticks(10));*/ - // Sending odometry data if not in HIL mode or motor commands if in HIL mode - if (mode != ROBOT_MODE_HIL || - (mode == ROBOT_MODE_FAULT && old_mode == ROBOT_MODE_HIL)) { + // Sending odometry data if not in simulation mode or motor commands if in HIL mode + if (mode != ROBOT_MODE_HIL || mode != ROBOT_MODE_SIMULATION || + (mode == ROBOT_MODE_FAULT && (old_mode == ROBOT_MODE_HIL || old_mode == ROBOT_MODE_SIMULATION))) { odo_getState(0, &odometry); msg_odo.data.x = (int16_t)(odometry.x * 1000.0); msg_odo.data.y = (int16_t)(odometry.y * 1000.0); @@ -121,7 +122,7 @@ static void NORETURN canMonitor_process(void) { txm.data32[1] = msg_odo.data32[1]; txm.eid = CAN_MSG_ODOMETRY_INDEP; can_transmit(CAND1, &txm, ms_to_ticks(10)); - } else { + } else if (mode == ROBOT_MODE_HIL || (mode == ROBOT_MODE_FAULT && old_mode == ROBOT_MODE_HIL)){ // Sending MOTOR3 data msg_mot.data.position = mc_getPosition(MOTOR3); msg_mot.data.speed = mc_getSpeed(MOTOR3); @@ -146,6 +147,7 @@ static void NORETURN canMonitor_process(void) { timer_add(&timer_can); // Sending ghost state + if (mode != ROBOT_MODE_SIMULATION || (mode == ROBOT_MODE_FAULT && old_mode != ROBOT_MODE_SIMULATION)) msg_ghost.data.state = dd_get_ghost_state(&odometry, &u); msg_ghost.data.x = (int16_t)(odometry.x * 1000.0); msg_ghost.data.y = (int16_t)(odometry.y * 1000.0); @@ -313,12 +315,20 @@ static void NORETURN canMonitorListen_process(void) { case CAN_MSG_CONTROLLER_MODE: controller_mode_msg.data32[0] = frame.data32[0]; controller_mode_msg.data32[1] = frame.data32[0]; - if (controller_mode_msg.data.mode == 1) { + switch (controller_mode_msg.data.mode) { + case SIMULATION_MODE_NORMAL: + mc_change_mode(MOTOR3, CONTROLLER_MODE_HIL); + mc_change_mode(MOTOR4, CONTROLLER_MODE_HIL); + odo_disable(CONTROL_ODOMETRY); + mode = ROBOT_MODE_SIMULATION; + break; + case SIMULATION_MODE_HIL: mc_change_mode(MOTOR3, CONTROLLER_MODE_HIL); mc_change_mode(MOTOR4, CONTROLLER_MODE_HIL); odo_disable(CONTROL_ODOMETRY); mode = ROBOT_MODE_HIL; - } else { + break; + default: mc_change_mode(MOTOR3, CONTROLLER_MODE_NORMAL); mc_change_mode(MOTOR4, CONTROLLER_MODE_NORMAL); odo_restart(CONTROL_ODOMETRY); diff --git a/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/sensor_actuator/can/can_messages.h b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/sensor_actuator/can/can_messages.h index 48c3b90..057833f 100644 --- a/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/sensor_actuator/can/can_messages.h +++ b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/sensor_actuator/can/can_messages.h @@ -59,6 +59,13 @@ //#define CAN_AX12_RESET 344 // Not used anymore #define CAN_AX12_SET_TORQUE_ENABLE 345 // ax12_set_torque_enable +// Simulation control +#define CAN_MSG_SIMULATION_MODE 205 // simulation mode +#define SIMULATION_MODE_NO 0 +#define SIMULATION_MODE_NORMAL 1 +#define SIMULATION_MODE_HIL 2 + + /****************************************************************************/ /** @@ -159,6 +166,13 @@ struct ax12_set_torque_enable_pkt { uint8_t enable; }; +/** + * Simulation mode control + */ +struct simulation_mode_pkt { + uint8_t mode; +} __attribute__((packed)); + /****************************************************************************/ /** @@ -220,4 +234,9 @@ typedef union { uint32_t d[2]; } ax12_set_torque_enable; +typedef union { + struct simulation_mode_pkt p; + uint32_t d[2]; +} simulation_mode; + #endif diff --git a/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/sensor_actuator/can/can_monitor.c b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/sensor_actuator/can/can_monitor.c index f8cf675..3f3e670 100644 --- a/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/sensor_actuator/can/can_monitor.c +++ b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/sensor_actuator/can/can_monitor.c @@ -73,6 +73,9 @@ static void NORETURN can_sender_process(void) { ax12_state ax12_st; + simulation_mode sim_mode_msg; + uint8_t sim_mode = SIMULATION_MODE_NO; + int i = 0; /* Initialize can frame */ @@ -86,51 +89,53 @@ static void NORETURN can_sender_process(void) { for (;;) { timer_add(&timer_send); - /* Beacon */ - get_beacon_positions(&pos, &pos_ll); + if (sim_mode == SIMULATION_MODE_NO) { + /* Beacon */ + get_beacon_positions(&pos, &pos_ll); - SET_PACKET(f, CAN_BEACON_POSITION, pos); - can_transmit(CAND1, &f, ms_to_ticks(10)); + SET_PACKET(f, CAN_BEACON_POSITION, pos); + can_transmit(CAND1, &f, ms_to_ticks(10)); - SET_PACKET(f, CAN_BEACON_LOWLEVEL_POSITION, pos_ll); - can_transmit(CAND1, &f, ms_to_ticks(10)); + SET_PACKET(f, CAN_BEACON_LOWLEVEL_POSITION, pos_ll); + can_transmit(CAND1, &f, ms_to_ticks(10)); - /* Switches */ + /* Switches */ - get_switch_status(&st1, &st2); + get_switch_status(&st1, &st2); - SET_PACKET(f, CAN_SWITCH_STATUS_1, st1); - can_transmit(CAND1, &f, ms_to_ticks(10)); + SET_PACKET(f, CAN_SWITCH_STATUS_1, st1); + can_transmit(CAND1, &f, ms_to_ticks(10)); - SET_PACKET(f, CAN_SWITCH_STATUS_2, st2); - can_transmit(CAND1, &f, ms_to_ticks(10)); + SET_PACKET(f, CAN_SWITCH_STATUS_2, st2); + can_transmit(CAND1, &f, ms_to_ticks(10)); - /* ADC */ + /* ADC */ - get_adc_values(&adc1, &adc2); + get_adc_values(&adc1, &adc2); - SET_PACKET(f, CAN_ADC_VALUES_1, adc1); - can_transmit(CAND1, &f, ms_to_ticks(10)); + SET_PACKET(f, CAN_ADC_VALUES_1, adc1); + can_transmit(CAND1, &f, ms_to_ticks(10)); - SET_PACKET(f, CAN_ADC_VALUES_2, adc2); - can_transmit(CAND1, &f, ms_to_ticks(10)); + SET_PACKET(f, CAN_ADC_VALUES_2, adc2); + can_transmit(CAND1, &f, ms_to_ticks(10)); - /* Battery monitoring */ + /* Battery monitoring */ - if (get_battery_monitoring(&battery1, &battery2) == 0) { + if (get_battery_monitoring(&battery1, &battery2) == 0) { - SET_PACKET(f, CAN_BATTERY_STATUS_1, battery1); - can_transmit(CAND1, &f, ms_to_ticks(10)); + SET_PACKET(f, CAN_BATTERY_STATUS_1, battery1); + can_transmit(CAND1, &f, ms_to_ticks(10)); - SET_PACKET(f, CAN_BATTERY_STATUS_2, battery2); - can_transmit(CAND1, &f, ms_to_ticks(10)); - } + SET_PACKET(f, CAN_BATTERY_STATUS_2, battery2); + can_transmit(CAND1, &f, ms_to_ticks(10)); + } - /* AX-12 */ + /* AX-12 */ - while (ax12_dequeue_state(&ax12_st) == 0) { - SET_PACKET(f, CAN_AX12_STATE, ax12_st); - can_transmit(CAND1, &f, ms_to_ticks(10)); + while (ax12_dequeue_state(&ax12_st) == 0) { + SET_PACKET(f, CAN_AX12_STATE, ax12_st); + can_transmit(CAND1, &f, ms_to_ticks(10)); + } } if (i) @@ -158,46 +163,58 @@ static void NORETURN can_receiver_process(void) { if (!ret || f.ide != 1) continue; - switch (f.eid) { + // These messages are to be interpreted only when not in simulation mode + if (sim_mode == SIMULATION_MODE_NO) { + switch (f.eid) { case CAN_SWITCH_SET: do { - GET_PACKET(switch_request, req, f); - set_switch(&req); + GET_PACKET(switch_request, req, f); + set_switch(&req); } while (0); break; case CAN_AX12_REQUEST_STATE: do { - struct ax12_hl_command hlc; - GET_PACKET(ax12_request_state, ax12_req, f); - hlc.command = AX12_HL_GET_STATE; - hlc.address = ax12_req.p.address; - ax12_queue_command(&hlc); + struct ax12_hl_command hlc; + GET_PACKET(ax12_request_state, ax12_req, f); + hlc.command = AX12_HL_GET_STATE; + hlc.address = ax12_req.p.address; + ax12_queue_command(&hlc); } while (0); break; case CAN_AX12_GOTO: do { - struct ax12_hl_command hlc; - GET_PACKET(ax12_goto, ax12_g, f); - hlc.command = AX12_HL_GOTO; - hlc.address = ax12_g.p.address; - hlc.args[0] = ax12_g.p.position; - hlc.args[1] = ax12_g.p.speed; - ax12_queue_command(&hlc); + struct ax12_hl_command hlc; + GET_PACKET(ax12_goto, ax12_g, f); + hlc.command = AX12_HL_GOTO; + hlc.address = ax12_g.p.address; + hlc.args[0] = ax12_g.p.position; + hlc.args[1] = ax12_g.p.speed; + ax12_queue_command(&hlc); } while (0); break; case CAN_AX12_SET_TORQUE_ENABLE: do { - struct ax12_hl_command hlc; - GET_PACKET(ax12_set_torque_enable, ax12_ste, f); - hlc.command = AX12_HL_SET_TORQUE_ENABLE; - hlc.address = ax12_ste.p.address; - hlc.args[0] = ax12_ste.p.enable; - ax12_queue_command(&hlc); + struct ax12_hl_command hlc; + GET_PACKET(ax12_set_torque_enable, ax12_ste, f); + hlc.command = AX12_HL_SET_TORQUE_ENABLE; + hlc.address = ax12_ste.p.address; + hlc.args[0] = ax12_ste.p.enable; + ax12_queue_command(&hlc); } while (0); break; default: break; + } + } + // General messages + switch (f.eid) { + case CAN_MSG_SIMULATIOn_MODE: + do { + GET_PACKET(simulation_mode, sim_mode_msg, f); + sim_mode = sim_mode_msg.mode; + } while (0); + break; } } diff --git a/info/control2011/src/lib/krobot_message.ml b/info/control2011/src/lib/krobot_message.ml index 85a32c6..358c399 100644 --- a/info/control2011/src/lib/krobot_message.ml +++ b/info/control2011/src/lib/krobot_message.ml @@ -18,6 +18,8 @@ open Lwt_react type direction = Forward | Backward +type simulation_mode = Sim_no | Sim_normal | Sim_HIL + type t = | Battery1_voltages of float * float * float * float | Battery2_voltages of float * float * float * float @@ -54,7 +56,7 @@ type t = | Odometry_ghost of float * float * float * int * bool | Set_odometry of float * float * float | Set_odometry_indep of float * float * float - | Set_controller_mode of bool + | Set_simulation_mode of simulation_mode | Elevator of float * float | Req_motor_status | Unknown of frame @@ -67,6 +69,11 @@ let string_of_direction = function | Forward -> "Forward" | Backward -> "Backward" +let string_of_simulation_mode = function + | Sim_no -> "Normal mode" + | Sim_normal -> "Simulation mode" + | Sim_HIL -> "HIL mode" + let to_string = function | Battery1_voltages(elem1, elem2, elem3, elem4) -> sprintf @@ -222,10 +229,10 @@ let to_string = function sprintf "Set_odometry_indep(%f, %f, %f)" x y theta - | Set_controller_mode b -> + | Set_simulation_mode m -> sprintf - "Set_controller_mode(%B)" - b + "Set_simulation_mode(%s)" + (string_of_simulation_mode m) | Elevator(p1, p2) -> sprintf "Elevator(%f, %f)" @@ -641,13 +648,16 @@ let encode = function ~remote:false ~format:F29bits ~data - | Set_controller_mode b -> + | Set_simulation_mode m -> frame ~identifier:205 ~kind:Data ~remote:false ~format:F29bits - ~data:(if b then "\x01" else "\x00") + ~data:(match m with + | Sim_no -> "\x00" + | Sim_normal -> "\x01" + | Sim_HIL -> "\x02") | Elevator(p1, p2) -> let data = String.create 8 in put_float32 data 0 p1; @@ -761,8 +771,9 @@ let decode frame = (get_float32 frame.data 0, get_float32 frame.data 4) | 205 -> - Set_controller_mode - (get_uint8 frame.data 0 <> 0) + Set_simulation_mode + (let v = get_uint8 frame.data 0 in + if v == 1 then Sim_normal else if v == 2 then Sim_HIL else Sim_no) | 206 -> let x, y, d1, d2, theta, v = decode_bezier frame.data in Motor_bezier(float x /. 1000., diff --git a/info/control2011/src/lib/krobot_message.mli b/info/control2011/src/lib/krobot_message.mli index f55a58f..5486768 100644 --- a/info/control2011/src/lib/krobot_message.mli +++ b/info/control2011/src/lib/krobot_message.mli @@ -12,6 +12,9 @@ type direction = Forward | Backward (** Type of directions. *) +type simulation_mode = Sim_no | Sim_normal | Sim_HIL + (** Type of simulation. *) + (** Type of messages. *) type t = | Battery1_voltages of float * float * float * float @@ -117,8 +120,8 @@ type t = | Set_odometry_indep of float * float * float (** [set_odometry_ident(x, y, theta)] sets the parameters of the independant odometry to the given ones. *) - | Set_controller_mode of bool - (** Put the card into simulation mode or not. *) + | Set_simulation_mode of simulation_mode + (** Put the cards into a given simulation mode. *) | Elevator of float * float (** Set the position of the elevators, between 0 and 1. Negative positions are ignored. *) diff --git a/info/control2011/src/tools/krobot_simulator.ml b/info/control2011/src/tools/krobot_simulator.ml index 2ba335f..ce5ab48 100644 --- a/info/control2011/src/tools/krobot_simulator.ml +++ b/info/control2011/src/tools/krobot_simulator.ml @@ -27,12 +27,19 @@ let fork = ref true let hil = ref false let sensors_emu = ref true let robot_sim = ref true +let go_normal = ref false +let go_simu = ref false +let go_hil = ref false let options = Arg.align [ "-no-fork", Arg.Clear fork, " Run in foreground"; "-no-sensor", Arg.Clear sensors_emu, " Don't emulate sensor inputs"; "-no-simulation", Arg.Clear robot_sim, " Don't simulate the robot"; "-hil", Arg.Set hil, " Run in hardware in the loop mode"; + "-go-normal", Arg.Set go_normal, " Put the card in normal mode and exit"; + "-go-simulation", Arg.Set go_simu, " Put the cards in simulation mode and exit"; + "-go-normal", Arg.Set go_normal, " Put the cards in normal mode and exit"; + "-go-hil", Arg.Set go_hil, " Put the cards in hardware in the loop mode and exit"; ] let usage = "\ @@ -500,6 +507,17 @@ lwt () = (* Open the krobot bus. *) lwt bus = Krobot_bus.get () in + if (!go_hil or !go_simu or !go_normal) then begin + if !go_normal then + ignore(Krobot_message.send bus (Unix.gettimeofday (), Set_simulation_mode Sim_no)) + else if !go_hil then + ignore(Krobot_message.send bus (Unix.gettimeofday (), Set_simulation_mode Sim_HIL)) + else if !go_simu then + ignore(Krobot_message.send bus (Unix.gettimeofday (), Set_simulation_mode Sim_normal)) + else + exit 0 + end; + (* Fork if not prevented. *) if !fork then Krobot_daemon.daemonize bus; @@ -531,13 +549,19 @@ lwt () = } in sim := Some local_sim; + (* Set the correct simulation mode *) + if !robot_sim || !sensors_emu then + (if !hil then + ignore(Krobot_message.send bus (Unix.gettimeofday (), Set_simulation_mode Sim_HIL)) + else + ignore(Krobot_message.send bus (Unix.gettimeofday (), Set_simulation_mode Sim_normal)) + ); + (* Launch robot simulation *) if !robot_sim then begin - (* Set the motor_controller card in HIL mode if necessary *) - if !hil then - ignore (Krobot_message.send bus (Unix.gettimeofday (), Set_controller_mode true)) ; ignore(send_CAN_messages local_sim bus); ignore(loop bus local_sim) end; + (* Launch sensor simulation *) if !sensors_emu then begin ignore(loop_urg local_sim bus) end; diff --git a/info/control2011/src/tools/krobot_viewer.ml b/info/control2011/src/tools/krobot_viewer.ml index 564a27c..140b292 100644 --- a/info/control2011/src/tools/krobot_viewer.ml +++ b/info/control2011/src/tools/krobot_viewer.ml @@ -614,11 +614,10 @@ let handle_message viewer (timestamp, message) = viewer.ui#beacon_period#set_text (string_of_int period); queue_draw viewer - | Set_controller_mode hil -> - if hil then - viewer.ui#menu_mode_hil#set_active true - else - viewer.ui#menu_mode_normal#set_active true + | Set_simulation_mode m -> + match m with + | Sim_HIL -> viewer.ui#menu_mode_hil#set_active true + | _ -> viewer.ui#menu_mode_normal#set_active true | _ -> () @@ -830,7 +829,7 @@ lwt () = ignore ( Krobot_message.send bus (Unix.gettimeofday (), - Set_controller_mode false) + Set_simulation_mode Sim_no) ); false)); @@ -841,7 +840,7 @@ lwt () = ignore_result ( Krobot_message.send bus (Unix.gettimeofday (), - Set_controller_mode true) + Set_simulation_mode Sim_HIL) ); false)); hooks/post-receive -- krobot |