Menu
▾
▴
[Krobot-commits] [SCM] krobot branch, master, updated. 275e297d6df8697614623704b7f136683ea71af1
|
From: Xavier L. <Ba...@us...> - 2011-04-02 15:17:01
|
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 275e297d6df8697614623704b7f136683ea71af1 (commit)
from c3191f76df8f1fa57a4f46ce16720611da4cc01c (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 275e297d6df8697614623704b7f136683ea71af1
Author: Xavier Lagorce <Xav...@cr...>
Date: Sat Apr 2 15:53:44 2011 +0200
[Controller_Motor_STM32] All angles are now in radians.
-----------------------------------------------------------------------
Changes:
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/can_monitor.c b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/can_monitor.c
index b1d7395..2617638 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/can_monitor.c
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/can_monitor.c
@@ -34,7 +34,7 @@ typedef struct {
typedef struct {
int16_t x __attribute__((__packed__)); // X position in mm (fixed point representation...)
int16_t y __attribute__((__packed__)); // Y position in mm
- int16_t theta __attribute__((__packed__)); // angle in 1/100 degrees
+ int16_t theta __attribute__((__packed__)); // angle in 1/10000 radians
} odometry_msg_t;
typedef struct {
@@ -44,9 +44,9 @@ typedef struct {
} move_msg_t;
typedef struct {
- int32_t angle __attribute__((__packed__)); // angle in 1/100 degrees (fixed point representation...)
- uint16_t speed __attribute__((__packed__)); // Speed in 1/100 degrees/s
- uint16_t acceleration __attribute__((__packed__)); // Acceleration in 1/100 degrees/s^2
+ int32_t angle __attribute__((__packed__)); // angle in 1/10000 radians (fixed point representation...)
+ uint16_t speed __attribute__((__packed__)); // Speed in 1/1000 rad/s
+ uint16_t acceleration __attribute__((__packed__)); // Acceleration in 1/1000 radians/s^2
} turn_msg_t;
typedef struct {
@@ -175,12 +175,12 @@ static void NORETURN canMonitor_process(void) {
odo_getState(&odometry);
msg_odo.data.x = (int16_t)(odometry.x * 1000.0);
msg_odo.data.y = (int16_t)(odometry.y * 1000.0);
- odometry.theta = fmodf(odometry.theta, 360.0);
- if (odometry.theta > 180.0)
- odometry.theta -= 360.0;
- if (odometry.theta < -180.0)
- odometry.theta += 360.0;
- msg_odo.data.theta = (int16_t)(odometry.theta * 100.0);
+ odometry.theta = fmodf(odometry.theta, 2*M_PI);
+ if (odometry.theta > M_PI)
+ odometry.theta -= 2*M_PI;
+ if (odometry.theta < -M_PI)
+ odometry.theta += 2*M_PI;
+ msg_odo.data.theta = (int16_t)(odometry.theta * 10000.0);
txm.data32[0] = msg_odo.data32[0];
txm.data32[1] = msg_odo.data32[1];
@@ -205,20 +205,12 @@ static void NORETURN canMonitorListen_process(void) {
odometry_can_msg_t odometry_msg;
stop_can_msg_t stop_msg;
- tc_robot_t robot;
-
// Initialize constant parameters of TX frame
txm.dlc = 8;
txm.rtr = 0;
txm.ide = 1;
txm.sid = 0;
- // Initialize robot representation
- robot.left_wheel = 2;
- robot.right_wheel = 3;
- robot.wheel_radius = 0.049245;
- robot.shaft_width = 0.259;
-
while (1) {
received = can_receive(CAND1, &frame, ms_to_ticks(100));
if (received) {
@@ -239,14 +231,14 @@ static void NORETURN canMonitorListen_process(void) {
case CAN_MSG_MOVE:
move_msg.data32[0] = frame.data32[0];
move_msg.data32[1] = frame.data32[1];
- if (!tc_is_working(TC_MASK(2) | TC_MASK(3)))
- tc_move(&robot, move_msg.data.distance / 1000.0, move_msg.data.speed / 1000.0, move_msg.data.acceleration / 1000.0);
+ if (!tc_is_working(TC_MASK(DD_LINEAR_SPEED_TC) | TC_MASK(DD_ROTATIONAL_SPEED_TC)))
+ dd_move(move_msg.data.distance / 1000.0, move_msg.data.speed / 1000.0, move_msg.data.acceleration / 1000.0);
break;
case CAN_MSG_TURN:
turn_msg.data32[0] = frame.data32[0];
turn_msg.data32[1] = frame.data32[1];
- if (!tc_is_working(TC_MASK(2) | TC_MASK(3)))
- tc_turn(&robot, turn_msg.data.angle / 100.0, turn_msg.data.speed / 100.0, turn_msg.data.acceleration / 100.0);
+ if (!tc_is_working(TC_MASK(DD_LINEAR_SPEED_TC) | TC_MASK(DD_ROTATIONAL_SPEED_TC)))
+ dd_turn(turn_msg.data.angle / 10000.0, turn_msg.data.speed / 1000.0, turn_msg.data.acceleration / 1000.0);
break;
case CAN_MSG_STOP:
stop_msg.data32[0] = frame.data32[0];
@@ -261,7 +253,7 @@ static void NORETURN canMonitorListen_process(void) {
odometry_msg.data32[1] = frame.data32[1];
odometry.x = ((float)odometry_msg.data.x) / 1000.0;
odometry.y = ((float)odometry_msg.data.y) / 1000.0;
- odometry.theta = ((float)odometry_msg.data.theta) / 100.0;
+ odometry.theta = ((float)odometry_msg.data.theta) / 10000.0;
odo_setState(&odometry);
break;
}
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/can_monitor.h b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/can_monitor.h
index fa8f973..2c83df0 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/can_monitor.h
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/can_monitor.h
@@ -17,6 +17,7 @@
#include "motor_controller.h"
#include "trajectory_controller.h"
#include "odometry.h"
+#include "differential_drive.h"
void canMonitorInit(void);
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/command_generator.h b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/command_generator.h
index 2c80192..38d4a00 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/command_generator.h
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/command_generator.h
@@ -14,6 +14,7 @@
#define __COMMAND_GENERATOR_H
#include <drv/timer.h>
+#include <math.h>
// Generator types
#define GEN_NONE 0 // No type, the generator is not initialized.
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/controller_motor_stm32_user.mk b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/controller_motor_stm32_user.mk
index 3a0c9a1..6f52c1d 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/controller_motor_stm32_user.mk
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/controller_motor_stm32_user.mk
@@ -17,6 +17,7 @@ controller_motor_stm32_USER_CSRC = \
$(controller_motor_stm32_SRC_PATH)/motor_controller.c \
$(controller_motor_stm32_SRC_PATH)/command_generator.c \
$(controller_motor_stm32_SRC_PATH)/trajectory_controller.c \
+ $(controller_motor_stm32_SRC_PATH)/differential_drive.c \
$(controller_motor_stm32_SRC_PATH)/can_monitor.c \
$(controller_motor_stm32_SRC_PATH)/main.c \
#
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/differential_drive.c b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/differential_drive.c
new file mode 100644
index 0000000..ccf28e7
--- /dev/null
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/differential_drive.c
@@ -0,0 +1,114 @@
+/*
+ * differential_drive.c
+ * --------------------
+ * Copyright : (c) 2011, Xavier Lagorce <Xav...@cr...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ */
+
+#include "differential_drive.h"
+
+typedef struct {
+ uint8_t initialized, enabled;
+ float wheel_radius, shaft_radius;
+ float last_lin_acceleration, last_rot_acceleration;
+ command_generator_t left_wheel_speed, right_wheel_speed;
+ command_generator_t left_wheel, right_wheel;
+} dd_params_t;
+
+static dd_params_t params;
+
+void dd_start(float wheel_radius, float shaft_width) {
+ params.wheel_radius = wheel_radius;
+ params.shaft_radius = shaft_width;
+ params.last_lin_acceleration = 0.0;
+ params.last_rot_acceleration = 0.0;
+
+ tc_new_controller(DD_LINEAR_SPEED_TC);
+ tc_new_controller(DD_ROTATIONAL_SPEED_TC);
+ new_dd_generator(¶ms.left_wheel_speed,
+ tc_get_speed_generator(DD_LINEAR_SPEED_TC),
+ tc_get_speed_generator(DD_ROTATIONAL_SPEED_TC),
+ wheel_radius, shaft_width,
+ -1);
+ new_dd_generator(¶ms.right_wheel_speed,
+ tc_get_speed_generator(DD_LINEAR_SPEED_TC),
+ tc_get_speed_generator(DD_ROTATIONAL_SPEED_TC),
+ wheel_radius, shaft_width,
+ 1);
+ new_ramp2_generator(¶ms.left_wheel, 0.0, ¶ms.left_wheel_speed);
+ new_ramp2_generator(¶ms.right_wheel, 0.0, ¶ms.right_wheel_speed);
+
+ start_generator(¶ms.left_wheel_speed);
+ start_generator(¶ms.right_wheel_speed);
+ start_generator(¶ms.left_wheel);
+ start_generator(¶ms.right_wheel);
+
+ params.initialized = 1;
+ params.enabled = 1;
+}
+
+void dd_pause(void) {
+ if (params.initialized) {
+ dd_set_linear_speed(0.0, params.last_lin_acceleration);
+ dd_set_rotational_speed(0.0, params.last_rot_acceleration);
+ params.enabled = 0;
+ }
+}
+
+void dd_resume(void) {
+ if (params.initialized && params.enabled) {
+ params.enabled = 1;
+ }
+}
+
+void dd_stop(void) {
+ if (params.initialized) {
+ pause_generator(¶ms.left_wheel);
+ pause_generator(¶ms.right_wheel);
+ pause_generator(¶ms.left_wheel_speed);
+ pause_generator(¶ms.right_wheel_speed);
+ tc_delete_controller(DD_LINEAR_SPEED_TC);
+ tc_delete_controller(DD_ROTATIONAL_SPEED_TC);
+ params.enabled = 0;
+ params.initialized = 0;
+ }
+}
+
+command_generator_t* dd_get_left_wheel_generator(void) {
+ return ¶ms.left_wheel;
+}
+
+command_generator_t* dd_get_right_wheel_generator(void) {
+ return ¶ms.right_wheel;
+}
+
+void dd_move(float distance, float speed, float acceleration) {
+ if (params.enabled) {
+ params.last_lin_acceleration = acceleration;
+ tc_goto(DD_LINEAR_SPEED_TC, distance, speed, params.last_lin_acceleration);
+ }
+}
+
+void dd_turn(float angle, float speed, float acceleration) {
+ if (params.enabled) {
+ params.last_rot_acceleration = acceleration;
+ tc_goto(DD_ROTATIONAL_SPEED_TC, angle, speed, params.last_rot_acceleration);
+ }
+}
+
+void dd_set_linear_speed(float speed, float acceleration) {
+ if (params.enabled) {
+ params.last_lin_acceleration = acceleration;
+ tc_goto_speed(DD_LINEAR_SPEED_TC, speed, params.last_lin_acceleration);
+ }
+}
+
+void dd_set_rotational_speed(float speed, float acceleration) {
+ if (params.enabled) {
+ params.last_rot_acceleration = acceleration;
+ tc_goto_speed(DD_ROTATIONAL_SPEED_TC, speed, params.last_rot_acceleration);
+ }
+}
+
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/differential_drive.h b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/differential_drive.h
new file mode 100644
index 0000000..4e68b0a
--- /dev/null
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/differential_drive.h
@@ -0,0 +1,76 @@
+/*
+ * differential_drive.h
+ * --------------------
+ * Copyright : (c) 2011, Xavier Lagorce <Xav...@cr...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ */
+
+#ifndef __DIFFERENTIAL_DRIVE_H
+#define __DIFFERENTIAL_DRIVE_H
+
+#include <math.h>
+#include "trajectory_controller.h"
+#include "command_generator.h"
+
+#ifndef DD_LINEAR_SPEED_TC
+ #define DD_LINEAR_SPEED_TC 0
+#endif
+#ifndef DD_ROTATIONAL_SPEED_TC
+ #define DD_ROTATIONAL_SPEED_TC 1
+#endif
+
+/* Initializes the differential drive
+ * - wheel_radius : radius of the wheels (in meters)
+ * - shaft_width : propulsion shaft radius (in meters)
+ *
+ * Note : the differential drive system will use Trajectory controllers
+ * DD_LINEAR_SPEED_TC and DD_ROTATIONAL_SPEED_TC
+ */
+void dd_start(float wheel_radius, float shaft_width);
+
+/* Pauses or Resumes the differential drive system.
+ * In pause mode, the drive will accept no further command and actions will be
+ * stopped.
+ * If the robot is moving, wheels will be slowed down with the last used
+ * acceleration (note that this won't necessarily give a straight line) when
+ * pausing.
+ */
+void dd_pause(void);
+void dd_resume(void);
+
+/*
+ * Stops the differential drive system.
+ */
+void dd_stop(void);
+
+/* Gets the generator correspondig to the wheels positions
+ * to use them in motor_control for instance
+ */
+command_generator_t* dd_get_left_wheel_generator(void);
+command_generator_t* dd_get_right_wheel_generator(void);
+
+/* Moves along a line
+ * - distance : distance to move of (in meters), can be positive (forward)
+ * or negative (backward).
+ * - speed : moving speed (in meters per second) should be positive.
+ * - acceleration : in meters per second per second, should be positive.
+ */
+void dd_move(float distance, float speed, float acceleration);
+
+/* Turns around the propulsion shaft center
+ * - angle : angle to turn of (in degrees), can be positive (CCW)
+ * or negative (CW).
+ * - speed : turning speed (in degrees per second) should be positive.
+ * - acceleration : in degrees per second per second, should be positive.
+ */
+void dd_turn(float angle, float speed, float acceleration);
+
+/*
+ * Modify a given speed of the robot using the specified acceleration
+ */
+void dd_set_linear_speed(float speed, float acceleration);
+void dd_set_rotational_speed(float speed, float acceleration);
+
+#endif /* __DIFFERENTIAL_DRIVE_H */
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/main.c b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/main.c
index 357f7b8..5584084 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/main.c
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/main.c
@@ -12,11 +12,13 @@
#include <math.h>
#include "hw/hw_led.h"
-#include "motor.h"
#include "motor_controller.h"
#include "can_monitor.h"
#include "command_generator.h"
-#include "trajectory_controller.h"
+#include "differential_drive.h"
+
+#define WHEEL_RADIUS 0.049245
+#define SHAFT_WIDTH 0.259
PROC_DEFINE_STACK(stack_ind, KERN_MINSTACKSIZE * 2);
@@ -45,12 +47,13 @@ static void init(void)
// Start control of drive motors
tc_init();
+ dd_start(WHEEL_RADIUS, SHAFT_WIDTH);
// Common parameters
- params.encoder_gain = -360.0/2000.0/15.0;
- params.G0 = 0.833;
+ params.encoder_gain = -2.0*M_PI/2000.0/15.0;
+ params.G0 = 0.0145;
params.tau = 0.015;
- params.k[0] = -68.0325;
- params.k[1] = -1.0205;
+ params.k[0] = -3898.0;
+ params.k[1] = -58.4696;
params.l = -params.k[0];
params.l0[0] = 0.0236;
params.l0[1] = 3.9715;
@@ -58,16 +61,14 @@ static void init(void)
// Initialize left motor
params.motor = MOTOR3;
params.encoder = ENCODER3;
- tc_new_controller(2);
- mc_new_controller(¶ms, tc_get_position_generator(2));
+ mc_new_controller(¶ms, dd_get_left_wheel_generator());
// Initialize right motor
params.motor = MOTOR4;
params.encoder = ENCODER4;
- tc_new_controller(3);
- mc_new_controller(¶ms, tc_get_position_generator(3));
+ mc_new_controller(¶ms, dd_get_right_wheel_generator());
// Start odometry
- odometryInit(1e-3, 0.049245, 0.259, -2.0*M_PI/2000.0/15.0);
+ odometryInit(1e-3, WHEEL_RADIUS, SHAFT_WIDTH, -2.0*M_PI/2000.0/15.0);
// Blink to say we are ready
for (uint8_t i=0; i < 5; i++) {
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/odometry.c b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/odometry.c
index 46f58b5..f6241fa 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/odometry.c
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/odometry.c
@@ -99,12 +99,12 @@ static void NORETURN odometry_process(void) {
void odo_setState(robot_state_t *new_state) {
state.robot_state.x = new_state->x;
state.robot_state.y = new_state->y;
- state.robot_state.theta = new_state->theta * M_PI / 180.0;
+ state.robot_state.theta = new_state->theta;
}
void odo_getState(robot_state_t *robot_state) {
robot_state->x = state.robot_state.x;
robot_state->y = state.robot_state.y;
- robot_state->theta = state.robot_state.theta * 180.0 / M_PI;
+ robot_state->theta = state.robot_state.theta;
}
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/trajectory_controller.c b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/trajectory_controller.c
index 5bd2734..5b01741 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/trajectory_controller.c
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/trajectory_controller.c
@@ -200,6 +200,19 @@ command_generator_t* tc_get_position_generator(uint8_t cntr_index) {
return &cont->position;
}
+command_generator_t* tc_get_speed_generator(uint8_t cntr_index) {
+ trajectory_controller_t *cont;
+
+ // Get the controller and verifies it is enabled
+ if (cntr_index >= NUM_TC_MAX)
+ return NULL;
+ cont = &controllers[cntr_index];
+ if (!cont->enabled)
+ return NULL;
+
+ return &cont->speed;
+}
+
void tc_goto(uint8_t cntr_index, float angle, float speed, float acceleration) {
float acc_dist, t_acc, t_end;
trajectory_controller_t *cont;
@@ -294,9 +307,9 @@ void tc_move(tc_robot_t *robot, float distance, float speed, float acceleration)
pause_generator(&controllers[robot->right_wheel].speed);
// Compute parameters
- dis_s = distance / robot->wheel_radius * 180.0 / M_PI;
- spe_s = speed / robot->wheel_radius * 180.0 / M_PI;
- acc_s = acceleration / robot->wheel_radius * 180.0 / M_PI;
+ dis_s = distance / robot->wheel_radius;
+ spe_s = speed / robot->wheel_radius;
+ acc_s = acceleration / robot->wheel_radius;
// Planify movements
tc_goto(robot->left_wheel, dis_s, spe_s, acc_s);
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/trajectory_controller.h b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/trajectory_controller.h
index 00b3a63..a4901a3 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/trajectory_controller.h
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/trajectory_controller.h
@@ -55,6 +55,13 @@ uint8_t tc_is_working(uint8_t cntr_indexes);
*/
command_generator_t* tc_get_position_generator(uint8_t cntr_index);
+/*
+ * Returns the speed generator outputting the intermediate result of the 'cntr_index'th
+ * Trajectory Generator.
+ */
+command_generator_t* tc_get_speed_generator(uint8_t cntr_index);
+
+
/* Moves of a given angle
* - angle : angle to move of, can be positive (forward) or negative
* (backward). The units corresponds to the given scaling factor
diff --git a/info/control2011/src/lib/krobot_message.ml b/info/control2011/src/lib/krobot_message.ml
index 16db1d2..60d13dd 100644
--- a/info/control2011/src/lib/krobot_message.ml
+++ b/info/control2011/src/lib/krobot_message.ml
@@ -137,7 +137,7 @@ let encode = function
let data = String.create 6 in
put_sint16 data 0 (truncate (x *. 1000.));
put_sint16 data 2 (truncate (y *. 1000.));
- put_sint16 data 4 (truncate (theta /. pi *. 18000.));
+ put_sint16 data 4 (truncate (theta *. 10000.));
frame
~identifier:104
~kind:Data
@@ -157,9 +157,9 @@ let encode = function
~data
| Motor_turn(angle, speed, acc) ->
let data = String.create 8 in
- put_sint32 data 0 (truncate (angle /. pi *. 18000.));
- put_uint16 data 4 (truncate (speed /. pi *. 18000.));
- put_uint16 data 6 (truncate (acc /. pi *. 18000.));
+ put_sint32 data 0 (truncate (angle *. 10000.));
+ put_uint16 data 4 (truncate (speed *. 1000.));
+ put_uint16 data 6 (truncate (acc *. 1000.));
frame
~identifier:202
~kind:Data
@@ -170,7 +170,7 @@ let encode = function
let data = String.create 6 in
put_sint16 data 0 (truncate (x *. 1000.));
put_sint16 data 2 (truncate (y *. 1000.));
- put_sint16 data 4 (truncate (theta /. pi *. 18000.));
+ put_sint16 data 4 (truncate (theta *. 10000.));
frame
~identifier:203
~kind:Data
@@ -242,7 +242,7 @@ let decode frame =
Odometry
(float (get_sint16 frame.data 0) /. 1000.,
float (get_sint16 frame.data 2) /. 1000.,
- float (get_sint16 frame.data 4) *. pi /. 18000.)
+ float (get_sint16 frame.data 4) /. 10000.)
| 201 ->
Motor_move
(float (get_sint32 frame.data 0) /. 1000.,
@@ -250,14 +250,14 @@ let decode frame =
float (get_uint16 frame.data 6) /. 1000.)
| 202 ->
Motor_turn
- (float (get_sint32 frame.data 0) *. pi /. 18000.,
- float (get_uint16 frame.data 4) *. pi /. 18000.,
- float (get_uint16 frame.data 6) *. pi /. 18000.)
+ (float (get_sint32 frame.data 0) /. 10000.,
+ float (get_uint16 frame.data 4) /. 1000.,
+ float (get_uint16 frame.data 6) /. 1000.)
| 203 ->
Set_odometry
(float (get_sint16 frame.data 0) /. 1000.,
float (get_sint16 frame.data 2) /. 1000.,
- float (get_sint16 frame.data 4) *. pi /. 18000.)
+ float (get_sint16 frame.data 4) /. 10000.)
| 204 ->
Motor_stop
| _ ->
diff --git a/info/control2011/src/tools/krobot_simulator.ml b/info/control2011/src/tools/krobot_simulator.ml
index 0c1b484..d0f31d7 100644
--- a/info/control2011/src/tools/krobot_simulator.ml
+++ b/info/control2011/src/tools/krobot_simulator.ml
@@ -226,7 +226,7 @@ lwt () =
return ()
| Req_motor_status ->
let moving = sim.command <> Idle in
- Krobot_message.send bus (Unix.gettimeofday (), Motor_status(false, false, moving, moving))
+ Krobot_message.send bus (Unix.gettimeofday (), Motor_status(moving, moving, false, false))
| Set_odometry(x, y, theta) ->
sim.state <- { x; y; theta };
return ()
diff --git a/info/control2011/src/tools/krobot_viewer.ml b/info/control2011/src/tools/krobot_viewer.ml
index fcc9a27..cdda66b 100644
--- a/info/control2011/src/tools/krobot_viewer.ml
+++ b/info/control2011/src/tools/krobot_viewer.ml
@@ -529,7 +529,7 @@ module Board = struct
queue_draw board
end
| Motor_status(m1, m2, m3, m4) ->
- board.moving <- m3 || m4;
+ board.moving <- m1 || m2;
board.ui#entry_moving1#set_text (if m1 then "yes" else "no");
board.ui#entry_moving2#set_text (if m2 then "yes" else "no");
board.ui#entry_moving3#set_text (if m3 then "yes" else "no");
hooks/post-receive
--
krobot
|
