Menu
▾
▴
[Krobot-commits] [SCM] krobot branch, master, updated. 29443c43d4f4f00011fa3e0b3c8e07dbae034732
|
From: Pierre C. <Ba...@us...> - 2012-10-26 20:40:37
|
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 29443c43d4f4f00011fa3e0b3c8e07dbae034732 (commit)
via 035f9d52b1b2e95562b0af441e542e2fa91ff38c (commit)
via d3646d8988b275d635808e856f27a8169fcbe5b1 (commit)
via 645a18abb1e663350086d537520888245e5644c2 (commit)
via e700577fe24962963fb1943e57d404269ab76fa9 (commit)
via 78af501cf61256d87e4fa09b6a5205b8397082fb (commit)
via 666e0dc488f66ed5318fd5e841be1a537ee45560 (commit)
via 82f57164333960d5b04bbc124c86a6daf344355b (commit)
via 46c0ef8dfeb43e00ed50d72edad767ee4cf54cab (commit)
via 53bf72d4ef903e5d87c698b52be0f39860f89d06 (commit)
via 3d59652b59b7621cb9f011073ce7e1f923b8410e (commit)
via bdeff879b8ed42276763573967dc7ed28dd50ce9 (commit)
via 96d08ed82366e7c0b2599bd7035675e0cec7ed4e (commit)
via bf9296f621e8ab25375fe4bdfa8a23197ddb9cb4 (commit)
via 7a344e7ce89be0492587d0cba6bc32509456ed4f (commit)
via ae0e0bc5d8cce9c4fe7b38773f5d99ee45ed2e7d (commit)
via d2e02e09948f8e2f38183d4aea5441fb42092115 (commit)
via 864db7bdf520c38dcf04dfb809e53b7a474e4cea (commit)
via 2147180a73cd3e00b5eb7e35a9b2decb614bb0ff (commit)
via 88d667e7e67cd64256ef567631bf788a32a2db1a (commit)
via 2ee981deaba2c5a24d2b9d64f52f7554440d5832 (commit)
via cce7084f6572f1a73bc8f9c1e2815f063525041a (commit)
via 4ee8b067e5a5f92c38c96a8f069a9f02dc088ee3 (commit)
via 000b1c651d916fbe7fe4de92c47f9d535f294a0e (commit)
via 05a4ea204296802b2d8235f2519ebc7020d9372c (commit)
via 650368da1ad2db0410ddcc397c2c31a06ca33674 (commit)
via 0ad19a68def0b2fabafeb0e5e15ee1e6177590bc (commit)
via 3ae218d6a175f384974252c5dd2eaa928fa13f6c (commit)
via 7b0cf7382c5779a9a3048bbc185785181a8aa151 (commit)
via 4bbe513d86ee6fd90bd17203354ad4f773955a44 (commit)
via b302c56062cbf220bba5ed93b8289976ada3926e (commit)
via 658408babb5310adde2136f174db84e380b51772 (commit)
via dc78cd457b1b62d932798cd8274bf57f770daea2 (commit)
via 0b246ee805ed4f163fb9579213451fce33aa51f4 (commit)
via ed691e2747ff70813a0282e24ae45bc528a4e6de (commit)
via 1f2f8c16bfb4dae9311823a60b86495466c59bab (commit)
via 97bb881fbf21d84c503a1614299dd9a0a86b7e26 (commit)
via c3258de737a58cc28457ba3dfdff35d6d269a203 (commit)
via a66b5b7a6574372066c3bda8ae352262192cd19c (commit)
via 6a43241bc8a4cfb99751ff0200319d03b0acfaf6 (commit)
via 4efddb34ffcc7c46f83c34fde0ba3d02b7f3c6cd (commit)
via f021f7d3fc24b091b5707b5ebf6c8268ee2f41d7 (commit)
via 6c07178363e06313500119b9f5544e1c88567baa (commit)
via 928d72ced8991fb8738d19e159eec18edf867fdd (commit)
via a00e289b2347abcce9a6513603322e341ae9e785 (commit)
via 0a079cd8bf2574072c85ff858e5974a202cd38b3 (commit)
via af11ff63690880602b4351c6a630d1f0d3260433 (commit)
via 2aea2c681aeb19aca037ee85a6f97a1b30070d64 (commit)
via e99b9813d3aed62947cb5a371b6e8c96527e9368 (commit)
via 89f78177ab4494c17be6d21c8c8ed4d6ce365ada (commit)
via 69133de534b5a299380fd905600970fd3c82a365 (commit)
via c1c21ceb36150b0970fdc2323679fcfe8dbc494e (commit)
via 41ef32f16fb0f5bd7afe33b46dc9620b1fe48de9 (commit)
from 39d27a4038f8f91b6469513e63ba3ccb4da8d3e8 (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 29443c43d4f4f00011fa3e0b3c8e07dbae034732
Author: chicco <cha...@cr...>
Date: Sat May 19 23:26:21 2012 +0200
[calibration] add a calibration action to the vm
commit 035f9d52b1b2e95562b0af441e542e2fa91ff38c
Author: Jérémie Dimino <je...@di...>
Date: Sat May 19 12:06:37 2012 +0200
Add krobot_collision
commit d3646d8988b275d635808e856f27a8169fcbe5b1
Author: Jérémie Dimino <je...@di...>
Date: Sat May 19 11:57:49 2012 +0200
add robot.motors_moving in krobot-vm
commit 645a18abb1e663350086d537520888245e5644c2
Author: Jérémie Dimino <je...@di...>
Date: Sat May 19 05:20:29 2012 +0200
Try to do random stuff
commit e700577fe24962963fb1943e57d404269ab76fa9
Author: Jérémie Dimino <je...@di...>
Date: Fri May 18 20:53:10 2012 +0200
Display collisions
commit 78af501cf61256d87e4fa09b6a5205b8397082fb
Author: Jérémie Dimino <je...@di...>
Date: Fri May 18 19:24:38 2012 +0200
Better collision detection
commit 666e0dc488f66ed5318fd5e841be1a537ee45560
Author: chicco <cha...@cr...>
Date: Fri May 18 23:40:10 2012 +0200
[krobot_pet] that all folks
commit 82f57164333960d5b04bbc124c86a6daf344355b
Author: Jérémie Dimino <je...@di...>
Date: Fri May 18 22:39:08 2012 +0200
Send logs in background in joystick control
commit 46c0ef8dfeb43e00ed50d72edad767ee4cf54cab
Author: [Kro]bot <kr...@wa...>
Date: Fri May 18 22:31:06 2012 +0200
Enable motors in joystick control
commit 53bf72d4ef903e5d87c698b52be0f39860f89d06
Author: Jérémie Dimino <je...@di...>
Date: Fri May 18 21:52:43 2012 +0200
Add joystick control
commit 3d59652b59b7621cb9f011073ce7e1f923b8410e
Author: chicco <cha...@cr...>
Date: Fri May 18 18:02:26 2012 +0200
[homo] push bottle
commit bdeff879b8ed42276763573967dc7ed28dd50ce9
Author: chicco <cha...@cr...>
Date: Fri May 18 17:48:18 2012 +0200
plop
commit 96d08ed82366e7c0b2599bd7035675e0cec7ed4e
Author: chicco <cha...@cr...>
Date: Fri May 18 13:43:40 2012 +0200
go
commit bf9296f621e8ab25375fe4bdfa8a23197ddb9cb4
Author: Jérémie Dimino <je...@di...>
Date: Fri May 18 13:09:30 2012 +0200
Fix logging in daemons
commit 7a344e7ce89be0492587d0cba6bc32509456ed4f
Author: chicco <cha...@cr...>
Date: Fri May 18 12:36:10 2012 +0200
[pathfinding] correct bezier curve correction
commit ae0e0bc5d8cce9c4fe7b38773f5d99ee45ed2e7d
Author: Jérémie Dimino <je...@di...>
Date: Fri May 18 12:14:10 2012 +0200
Optimize krobot-record
Send all messages to log to the writer thread in one batch
commit d2e02e09948f8e2f38183d4aea5441fb42092115
Author: Jérémie Dimino <je...@di...>
Date: Fri May 18 11:57:54 2012 +0200
Add a flag for opencv
commit 864db7bdf520c38dcf04dfb809e53b7a474e4cea
Author: Jérémie Dimino <je...@di...>
Date: Fri May 18 11:53:50 2012 +0200
Remove generated stuff from the repository
commit 2147180a73cd3e00b5eb7e35a9b2decb614bb0ff
Author: Jérémie Dimino <je...@di...>
Date: Fri May 18 11:34:36 2012 +0200
Modify the message logging system
* krobot-record records everything to stdout (so it can be piped to gzip)
* krobot-replay filter-out non-can messages and Info can frames
* add krobot-read to pretty-print the contents of record files
commit 88d667e7e67cd64256ef567631bf788a32a2db1a
Author: chicco <cha...@cr...>
Date: Fri May 18 11:00:09 2012 +0200
[vm] correct typo
commit 2ee981deaba2c5a24d2b9d64f52f7554440d5832
Author: chicco <cha...@cr...>
Date: Fri May 18 10:36:07 2012 +0200
[info] modify bezier coefs such that it avoid colliding
commit cce7084f6572f1a73bc8f9c1e2815f063525041a
Author: chicco <cha...@cr...>
Date: Fri May 18 02:47:59 2012 +0200
[info] add coins positions
commit 4ee8b067e5a5f92c38c96a8f069a9f02dc088ee3
Author: chicco <cha...@cr...>
Date: Fri May 18 01:48:21 2012 +0200
[krobot_vm] restart after some time when no trajectory is found
commit 000b1c651d916fbe7fe4de92c47f9d535f294a0e
Author: chicco <cha...@cr...>
Date: Fri May 18 01:40:29 2012 +0200
[info] fix pathfinder
commit 05a4ea204296802b2d8235f2519ebc7020d9372c
Author: chicco <cha...@cr...>
Date: Thu May 17 21:21:27 2012 +0200
[control] fix things with ennemy avoiding
commit 650368da1ad2db0410ddcc397c2c31a06ca33674
Author: chicco <cha...@cr...>
Date: Thu May 17 17:39:03 2012 +0200
[control] add krobot_stop_beacon to stop the annoying sound...
commit 0ad19a68def0b2fabafeb0e5e15ee1e6177590bc
Author: Xavier Lagorce <Xav...@cr...>
Date: Fri May 18 02:09:22 2012 +0200
[control2011] Added CAN packets descriptions for new features
commit 3ae218d6a175f384974252c5dd2eaa928fa13f6c
Author: Xavier Lagorce <Xav...@cr...>
Date: Fri May 18 02:08:35 2012 +0200
[Controller_Motor_STM32] New commands and forgotten mail
commit 7b0cf7382c5779a9a3048bbc185785181a8aa151
Author: Xavier Lagorce <Xav...@cr...>
Date: Fri May 18 00:31:03 2012 +0200
[Controller_Motor_STM32] Suspend motor controllers when the fault button is pressed.
commit 4bbe513d86ee6fd90bd17203354ad4f773955a44
Author: Xavier Lagorce <Xav...@cr...>
Date: Fri May 18 00:29:39 2012 +0200
[Controller_Motor_STM32] Added functions to suspend and re-enable a controller,
taking into account the possible movement of the motor between the two operations
commit b302c56062cbf220bba5ed93b8289976ada3926e
Author: Olivier BICHLER <oli...@gm...>
Date: Thu May 17 21:52:42 2012 +0200
[Sensor_Actuator] Multiple beacons handling
commit 658408babb5310adde2136f174db84e380b51772
Author: Olivier BICHLER <oli...@gm...>
Date: Thu May 17 19:11:45 2012 +0200
[Sensor_Actuator] Comment blinky leds
commit dc78cd457b1b62d932798cd8274bf57f770daea2
Author: Olivier BICHLER <oli...@gm...>
Date: Thu May 17 18:07:43 2012 +0200
[Sensor_Actuator] Bug correction on smooth
commit 0b246ee805ed4f163fb9579213451fce33aa51f4
Author: Jérémie Dimino <je...@di...>
Date: Thu May 17 16:18:16 2012 +0200
Allow to put fake beacons
commit ed691e2747ff70813a0282e24ae45bc528a4e6de
Author: chicco <cha...@cr...>
Date: Thu May 17 15:02:20 2012 +0200
[homologation] homologation startegy restart when changing team
commit 1f2f8c16bfb4dae9311823a60b86495466c59bab
Author: chicco <cha...@cr...>
Date: Thu May 17 11:39:24 2012 +0200
[control] last minute fix for homologation
commit 97bb881fbf21d84c503a1614299dd9a0a86b7e26
Author: chicco <cha...@cr...>
Date: Thu May 17 09:59:58 2012 +0200
[homologation] wait after pluging the jack
commit c3258de737a58cc28457ba3dfdff35d6d269a203
Author: chicco <cha...@cr...>
Date: Thu May 17 09:33:31 2012 +0200
[control] code from the whooole long night
commit a66b5b7a6574372066c3bda8ae352262192cd19c
Author: Xavier Lagorce <Xav...@cr...>
Date: Thu May 17 10:21:09 2012 +0200
[control2011] Take into account the double beacon detection
commit 6a43241bc8a4cfb99751ff0200319d03b0acfaf6
Author: Xavier Lagorce <Xav...@cr...>
Date: Thu May 17 10:16:23 2012 +0200
[Controller_Motor_STM32] Correct independant encoder parameters
commit 4efddb34ffcc7c46f83c34fde0ba3d02b7f3c6cd
Author: Xavier Lagorce <Xav...@cr...>
Date: Thu May 17 10:15:49 2012 +0200
[Controller_Motor_STM32] Corrected Makefile
commit f021f7d3fc24b091b5707b5ebf6c8268ee2f41d7
Author: Xavier Lagorce <Xav...@cr...>
Date: Wed May 16 22:56:46 2012 +0200
[Controller_Motor_STM32] Always respond to a Bezier command with a CAN packet
commit 6c07178363e06313500119b9f5544e1c88567baa
Author: Olivier BICHLER <oli...@gm...>
Date: Thu May 17 09:30:15 2012 +0200
[Sensor_Actuator] Changed CAN message to send two beacon positions
commit 928d72ced8991fb8738d19e159eec18edf867fdd
Author: Olivier BICHLER <oli...@gm...>
Date: Thu May 17 00:13:12 2012 +0200
[Sensor_Actuator] Correction of bugs in some limit cases
commit a00e289b2347abcce9a6513603322e341ae9e785
Author: Olivier BICHLER <oli...@gm...>
Date: Thu May 17 00:20:08 2012 +0200
[Sensor_Actuator] Nouvelle calibration OK
commit 0a079cd8bf2574072c85ff858e5974a202cd38b3
Author: chicco <cha...@cr...>
Date: Wed May 16 21:03:17 2012 +0200
[viewer] draw the robot using independent encoder informations
commit af11ff63690880602b4351c6a630d1f0d3260433
Author: chicco <cha...@cr...>
Date: Wed May 16 20:51:13 2012 +0200
[viewer/vm] handler Odometry_indep/Set_odometry_indep
commit 2aea2c681aeb19aca037ee85a6f97a1b30070d64
Author: Xavier Lagorce <Xav...@cr...>
Date: Wed May 16 20:09:36 2012 +0200
[Controller_Motor_STM32] Allow user choice of encoders for odometry
commit e99b9813d3aed62947cb5a371b6e8c96527e9368
Author: Xavier Lagorce <Xav...@cr...>
Date: Wed May 16 19:11:27 2012 +0200
[Krobot_message] Added CAN messages for independant odometry
commit 89f78177ab4494c17be6d21c8c8ed4d6ce365ada
Author: Xavier Lagorce <Xav...@cr...>
Date: Wed May 16 19:01:29 2012 +0200
[Controller_Motor_STM32] Added messages to set independant odometry
commit 69133de534b5a299380fd905600970fd3c82a365
Author: Xavier Lagorce <Xav...@cr...>
Date: Wed May 16 18:23:56 2012 +0200
[Controller_Motor_STM32] Verify bezier spline validity
commit c1c21ceb36150b0970fdc2323679fcfe8dbc494e
Author: Xavier Lagorce <Xav...@cr...>
Date: Wed May 16 18:22:20 2012 +0200
[Controller_Motor_STM32] Compute and broadcast second odometry
commit 41ef32f16fb0f5bd7afe33b46dc9620b1fe48de9
Author: Olivier BICHLER <oli...@gm...>
Date: Wed May 16 20:09:27 2012 +0200
[Sensor_Actuator] Updated beacon system to use an index switch and handle multiple beacons
-----------------------------------------------------------------------
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 94ebea0..156bf47 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
@@ -24,6 +24,7 @@
#define CAN_MSG_ODOMETRY 104 // odometry_can_msg_t
#define CAN_MSG_GHOST 105 // ghost_can_msg_t
#define CAN_MSG_CONTROL_ERROR 106 // error_can_msg_t
+#define CAN_MSG_ODOMETRY_INDEP 107 // odometry_can_msg_t
// Received commands
#define CAN_MSG_MOVE 201 // move_can_msg_t
@@ -34,6 +35,11 @@
#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
+#define CAN_MSG_ODOMETRY_INDEP_SET 209 // odometry_can_msg_t
+#define CAN_MSG_CONTROLLER_ACTIVATION 210 // controller_activation_can_msg_t
+#define CAN_MSG_DRIVE_ACTIVATION 211 // controller_activation_can_msg_t
+#define CAN_MSG_TORQUE_LIMIT 212 // torque_limit_can_msg_t
+#define CAN_MSG_DRIVE_TORQUE_LIMIT 213 // drive_torque_limit_can_msg_t
/* +-----------------------------------------------------------------+
| CAN messages data structures |
@@ -81,6 +87,17 @@ typedef struct {
uint8_t state; // 1 if trajectory in progress, 0 else
} ghost_msg_t;
+// Torque limitation on some motors
+typedef struct {
+ uint8_t motor __attribute__((__packed__)); // Logic OR of bits corresponding to motors
+ uint16_t limit __attribute__((__packed__)); // Torque limit (between 0 and 3600)
+} torque_limit_msg_t;
+
+// Torque limitation on propulsion drive
+typedef struct {
+ uint16_t limit __attribute__((__packed__)); // Torque limit (between 0 and 3600)
+} drive_torque_limit_msg_t;
+
// Move command
typedef struct {
int32_t distance __attribute__((__packed__)); // Distance in mm (fixed point representation...)
@@ -95,6 +112,19 @@ typedef struct {
uint16_t acceleration __attribute__((__packed__)); // Acceleration in 1/1000 radians/s^2
} turn_msg_t;
+// Controller activation command
+typedef struct {
+ uint8_t motor __attribute__((__packed__)); // Motor ID of the motor to act on
+ uint8_t activate __attribute__((__packed__)); // Activate the controller if non zero
+ // else release the control.
+} controller_activation_msg_t;
+
+// Propulsion Drive activation command
+typedef struct {
+ uint8_t activate __attribute__((__packed__)); // Activate the propulsion drive if non zero
+ // else release the control.
+} drive_activation_msg_t;
+
// Add a new Bezier Spline to the wait queue
typedef struct {
uint16_t x_end:12 __attribute__((__packed__)); // end point x coordinate in mm
@@ -125,8 +155,8 @@ typedef struct {
// Command the speed of a particular motor
typedef struct {
- uint8_t motor_id;
- int32_t speed;
+ uint8_t motor_id __attribute__((__packed__));
+ int32_t speed __attribute__((__packed__));
} motor_command_msg_t;
/* +-----------------------------------------------------------------+
@@ -164,6 +194,16 @@ typedef union {
} ghost_can_msg_t;
typedef union {
+ torque_limit_msg_t data;
+ uint32_t data32[2];
+} torque_limit_can_msg_t;
+
+typedef union {
+ drive_torque_limit_msg_t data;
+ uint32_t data32[2];
+} drive_torque_limit_can_msg_t;
+
+typedef union {
move_msg_t data;
uint32_t data32[2];
} move_can_msg_t;
@@ -174,6 +214,16 @@ typedef union {
} turn_can_msg_t;
typedef union {
+ controller_activation_msg_t data;
+ uint32_t data32[2];
+} controller_activation_can_msg_t;
+
+typedef union {
+ drive_activation_msg_t data;
+ uint32_t data32[2];
+} drive_activation_can_msg_t;
+
+typedef union {
bezier_msg_t data;
uint32_t data32[2];
} bezier_can_msg_t;
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/can_messages_sensors.h b/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/can_messages_sensors.h
new file mode 100644
index 0000000..48c3b90
--- /dev/null
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/can_messages_sensors.h
@@ -0,0 +1,223 @@
+/**
+ * Sensor-Actuator board messages
+ *
+ * Header file for the Sensor and Actuator CAN messages
+ *
+ * Copyright © 2011 Nicolas Dandrimont <ol...@cr...>
+ * Authors: Nicolas Dandrimont <ol...@cr...>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#ifndef CAN_MESSAGES_H__
+#define CAN_MESSAGES_H__
+
+/**
+ * Message IDs
+ */
+
+// Beacon
+
+#define CAN_BEACON_POSITION 301 // beacon_position
+#define CAN_BEACON_LOWLEVEL_POSITION 302 // beacon_lowlevel_position
+#define CAN_BEACON_CALIBRATION 303 // beacon_calibration
+
+// Switches
+
+#define CAN_SWITCH_STATUS_1 311 // switch_status
+#define CAN_SWITCH_STATUS_2 312 // switch_status
+#define CAN_SWITCH_SET 313 // switch_request
+
+
+// ADC
+
+#define CAN_ADC_VALUES_1 321 // adc_values
+#define CAN_ADC_VALUES_2 322 // adc_values
+
+// Battery monitoring
+
+#define CAN_BATTERY_STATUS_1 331 // battery_status
+#define CAN_BATTERY_STATUS_2 332 // battery_status
+
+// AX12
+
+#define CAN_AX12_STATE 341 // ax12_state
+#define CAN_AX12_REQUEST_STATE 342 // ax12_request_state
+#define CAN_AX12_GOTO 343 // ax12_goto
+//#define CAN_AX12_RESET 344 // Not used anymore
+#define CAN_AX12_SET_TORQUE_ENABLE 345 // ax12_set_torque_enable
+
+/****************************************************************************/
+
+/**
+ * Message Packets
+ */
+
+/**
+ * Beacon messages
+ */
+
+// Position of the opponent
+struct beacon_position_pkt {
+ uint16_t angle[2]; // in 1/10000th of radians [0; 2*Pi[
+ uint16_t distance[2]; // in mm [0; 65536[
+} __attribute__((packed));
+
+
+
+// Beacon low-level position
+struct beacon_lowlevel_position_pkt {
+ uint16_t angle; // in 1/10000th of radians [0; 2*Pi[
+ uint16_t width; // in 1/100000th of radians [0; Pi/5[
+ uint32_t period; // in timer ticks
+} __attribute__((packed));
+
+
+
+// Beacon calibration
+struct beacon_calibration_pkt {
+ uint16_t width; // in 1/100000th of radians [0; Pi/5[
+ uint16_t distance; // in mm [0; 65536[
+} __attribute__((packed));
+
+
+/**
+ * Switch messages
+ */
+
+// Switch status
+struct switch_status_pkt {
+ uint8_t sw1;
+ uint8_t sw2;
+ uint8_t sw3;
+ uint8_t sw4;
+ uint8_t sw5;
+ uint8_t sw6;
+ uint8_t sw7;
+ uint8_t sw8;
+};
+
+// Switch request
+struct switch_request_pkt {
+ uint8_t num;
+ uint8_t state;
+};
+
+/**
+ * ADC messages
+ */
+// ADC Values
+struct adc_values_pkt {
+ uint16_t val1;
+ uint16_t val2;
+ uint16_t val3;
+ uint16_t val4;
+} __attribute__((packed));
+
+/**
+ * Battery monitoring
+ */
+struct battery_status_pkt {
+ uint16_t elem[4]; // in 1/10000th volts [0; 6.5536[
+} __attribute__((packed));
+
+
+/**
+ * AX-12
+ */
+struct ax12_state_pkt {
+ uint8_t address;
+ uint16_t position;
+ uint16_t speed;
+ uint16_t torque;
+} __attribute__((packed));
+
+struct ax12_request_state_pkt {
+ uint8_t address;
+} __attribute__((packed));
+
+struct ax12_goto_pkt {
+ uint8_t address;
+ uint16_t position;
+ uint16_t speed;
+} __attribute__((packed));
+
+struct ax12_set_torque_enable_pkt {
+ uint8_t address;
+ uint8_t enable;
+};
+
+/****************************************************************************/
+
+/**
+ * Typedefs
+ */
+
+typedef union {
+ struct beacon_position_pkt p;
+ uint32_t d[2];
+} beacon_position;
+
+typedef union {
+ struct beacon_lowlevel_position_pkt p;
+ uint32_t d[2];
+} beacon_lowlevel_position;
+
+typedef union {
+ struct beacon_calibration_pkt p;
+ uint32_t d[2];
+} beacon_calibration;
+
+typedef union {
+ struct switch_status_pkt p;
+ uint32_t d[2];
+} switch_status;
+
+typedef union {
+ struct switch_request_pkt p;
+ uint32_t d[2];
+} switch_request;
+
+typedef union {
+ struct adc_values_pkt p;
+ uint32_t d[2];
+} adc_values;
+
+typedef union {
+ struct battery_status_pkt p;
+ uint32_t d[2];
+} battery_status;
+
+typedef union {
+ struct ax12_state_pkt p;
+ uint32_t d[2];
+} ax12_state;
+
+typedef union {
+ struct ax12_request_state_pkt p;
+ uint32_t d[2];
+} ax12_request_state;
+
+typedef union {
+ struct ax12_goto_pkt p;
+ uint32_t d[2];
+} ax12_goto;
+
+typedef union {
+ struct ax12_set_torque_enable_pkt p;
+ uint32_t d[2];
+} ax12_set_torque_enable;
+
+#endif
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 5ed50a6..3d83b64 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
@@ -12,6 +12,7 @@
#define ROBOT_MODE_NORMAL 0
#define ROBOT_MODE_HIL 1
+#define ROBOT_MODE_FAULT 2
#define CONTROL_ODOMETRY 0
@@ -19,7 +20,7 @@
PROC_DEFINE_STACK(stack_can_send, KERN_MINSTACKSIZE * 8);
PROC_DEFINE_STACK(stack_can_receive, KERN_MINSTACKSIZE * 8);
// globals
-volatile uint8_t mode;
+volatile uint8_t mode, old_mode;
volatile uint8_t err1, err2, send_err;
// Process for communication
@@ -40,6 +41,7 @@ void canMonitorInit(void) {
can_start(CAND1, &cfg);
mode = ROBOT_MODE_NORMAL;
+ old_mode = ROBOT_MODE_NORMAL;
// Start communication process
proc_new(canMonitor_process, NULL, sizeof(stack_can_send), stack_can_send);
@@ -88,8 +90,9 @@ static void NORETURN canMonitor_process(void) {
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) {
- odo_getState(CONTROL_ODOMETRY, &odometry);
+ if (mode != ROBOT_MODE_HIL ||
+ (mode == ROBOT_MODE_FAULT && old_mode == ROBOT_MODE_HIL)) {
+ odo_getState(0, &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, 2*M_PI);
@@ -103,6 +106,21 @@ static void NORETURN canMonitor_process(void) {
txm.data32[1] = msg_odo.data32[1];
txm.eid = CAN_MSG_ODOMETRY;
can_transmit(CAND1, &txm, ms_to_ticks(10));
+
+ odo_getState(1, &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, 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];
+ txm.eid = CAN_MSG_ODOMETRY_INDEP;
+ can_transmit(CAND1, &txm, ms_to_ticks(10));
} else {
// Sending MOTOR3 data
msg_mot.data.position = mc_getPosition(MOTOR3);
@@ -166,6 +184,7 @@ static void NORETURN canMonitorListen_process(void) {
bool received = false;
can_tx_frame txm;
robot_state_t odometry;
+ uint8_t res;
move_can_msg_t move_msg;
turn_can_msg_t turn_msg;
@@ -175,6 +194,11 @@ static void NORETURN canMonitorListen_process(void) {
bezier_can_msg_t bezier_msg;
bezier_limits_can_msg_t bezier_limits_msg;
motor_command_can_msg_t motor_command_msg;
+ switch_status switches;
+ torque_limit_can_msg_t torque_limit_msg;
+ drive_torque_limit_can_msg_t drive_torque_limit_msg;
+ controller_activation_can_msg_t cont_act_msg;
+ drive_activation_can_msg_t drive_activation_msg;
// Initialize constant parameters of TX frame
txm.dlc = 8;
@@ -194,6 +218,16 @@ static void NORETURN canMonitorListen_process(void) {
} else {
// Handle commands and other informations
switch (frame.eid) {
+ case CAN_MSG_TORQUE_LIMIT:
+ torque_limit_msg.data32[0] = frame.data32[0];
+ torque_limit_msg.data32[1] = frame.data32[0];
+ motorSetMaxPWM(torque_limit_msg.data.motor, torque_limit_msg.data.limit);
+ break;
+ case CAN_MSG_DRIVE_TORQUE_LIMIT:
+ drive_torque_limit_msg.data32[0] = frame.data32[0];
+ drive_torque_limit_msg.data32[1] = frame.data32[1];
+ motorSetMaxPWM(MOTOR3|MOTOR4, torque_limit_msg.data.limit);;
+ break;
case CAN_MSG_MOVE:
move_msg.data32[0] = frame.data32[0];
move_msg.data32[1] = frame.data32[1];
@@ -206,12 +240,36 @@ static void NORETURN canMonitorListen_process(void) {
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_CONTROLLER_ACTIVATION:
+ cont_act_msg.data32[0] = frame.data32[0];
+ cont_act_msg.data32[1] = frame.data32[1];
+ if (!cont_act_msg.data.activate)
+ mc_suspend_controller(1 << (cont_act_msg.data.motor-1));
+ else
+ mc_reactivate_controller(1 << (cont_act_msg.data.motor-1));
+ break;
+ case CAN_MSG_DRIVE_ACTIVATION:
+ drive_activation_msg.data32[0] = frame.data32[0];
+ drive_activation_msg.data32[1] = frame.data32[1];
+ if (!drive_activation_msg.data.activate) {
+ mc_suspend_controller(MOTOR3);
+ mc_suspend_controller(MOTOR4);
+ dd_interrupt_trajectory(0., 0.);
+ } else {
+ mc_reactivate_controller(MOTOR3);
+ mc_reactivate_controller(MOTOR4);
+ }
+ break;
case CAN_MSG_BEZIER_ADD:
bezier_msg.data32[0] = frame.data32[0];
bezier_msg.data32[1] = frame.data32[1];
- dd_add_bezier(bezier_msg.data.x_end/1000.0, bezier_msg.data.y_end/1000.0,
- bezier_msg.data.d1/100.0, bezier_msg.data.d2/100.0,
- bezier_msg.data.theta_end/100.0, bezier_msg.data.v_end/1000.0);
+ res = dd_add_bezier(bezier_msg.data.x_end/1000.0,
+ bezier_msg.data.y_end/1000.0,
+ bezier_msg.data.d1/100.0,
+ bezier_msg.data.d2/100.0,
+ bezier_msg.data.theta_end/100.0,
+ bezier_msg.data.v_end/1000.0);
+ can_send_error(res,0);
break;
case CAN_MSG_BEZIER_LIMITS:
bezier_limits_msg.data32[0] = frame.data32[0];
@@ -231,7 +289,15 @@ static void NORETURN canMonitorListen_process(void) {
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) / 10000.0;
- odo_setState(CONTROL_ODOMETRY, &odometry);
+ odo_setState(0, &odometry);
+ break;
+ case CAN_MSG_ODOMETRY_INDEP_SET:
+ odometry_msg.data32[0] = frame.data32[0];
+ 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) / 10000.0;
+ odo_setState(1, &odometry);
break;
case CAN_MSG_ODOMETRY:
// We should only receive such message in HIL mode
@@ -265,6 +331,20 @@ static void NORETURN canMonitorListen_process(void) {
motorSetSpeed(motor_command_msg.data.motor_id,
motor_command_msg.data.speed);
break;
+ case CAN_SWITCH_STATUS_1:
+ switches.d[0] = frame.data32[0];
+ switches.d[1] = frame.data32[1];
+ if (mode != ROBOT_MODE_FAULT && !switches.p.sw3) {
+ dd_interrupt_trajectory(0., 0.);
+ old_mode = mode;
+ mode = ROBOT_MODE_FAULT;
+ mc_suspend_controller(MOTOR3);
+ mc_suspend_controller(MOTOR4);
+ } else if (mode == ROBOT_MODE_FAULT && switches.p.sw3) {
+ mode = old_mode;
+ mc_reactivate_controller(MOTOR3);
+ mc_reactivate_controller(MOTOR4);
+ }
}
}
}
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/can_monitor.h b/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/can_monitor.h
index c157f58..c829c10 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/can_monitor.h
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/can_monitor.h
@@ -19,6 +19,7 @@
#include "odometry.h"
#include "differential_drive.h"
#include "can_messages.h"
+#include "can_messages_sensors.h"
void canMonitorInit(void);
void can_send_error(uint8_t err1, uint8_t err2);
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/controller_motor_stm32.mk b/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/controller_motor_stm32.mk
index ac9aa9a..37d3ed0 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/controller_motor_stm32.mk
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/controller_motor_stm32.mk
@@ -9,7 +9,7 @@ Firmware_DEBUG = 1
# Our target application
TRG += controller_motor_stm32
-controller_motor_stm32_PREFIX = "/media/data/krobot/arm-i386/bin/arm-none-eabi-"
+controller_motor_stm32_PREFIX = "$$HOME/opt/arm/bin/arm-none-eabi-"
controller_motor_stm32_SUFFIX = ""
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/main.c b/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/main.c
index 48846a3..e4b87bf 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/main.c
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/main.c
@@ -17,8 +17,11 @@
#include "command_generator.h"
#include "differential_drive.h"
-#define WHEEL_RADIUS 0.049245
-#define SHAFT_WIDTH 0.224
+#define PROP_WHEEL_RADIUS 0.049245
+#define PROP_SHAFT_WIDTH 0.22264
+
+#define INDEP_WHEEL_RADIUS 0.0359
+#define INDEP_SHAFT_WIDTH 0.266
#define CONTROL_ODOMETRY 0
@@ -50,7 +53,7 @@ static void init(void)
// Start control of drive motors
tc_init();
dd_start(CONTROL_ODOMETRY, // Use odometry CONTROL_ODOMETRY for control
- WHEEL_RADIUS, SHAFT_WIDTH, // Structural parameters
+ PROP_WHEEL_RADIUS, PROP_SHAFT_WIDTH, // Structural parameters
8*2*M_PI, // Absolute wheel speed limitation
0.5, // Linear velocity limitation
1.0, // Linear acceleration limitation
@@ -77,8 +80,15 @@ static void init(void)
params.encoder_gain = -2.0*M_PI/2000.0/15; // Left motor is reversed
mc_new_controller(¶ms, dd_get_right_wheel_generator(), CONTROLLER_MODE_NORMAL);
- // Start odometry
- odometryInit(CONTROL_ODOMETRY, 1e-3, WHEEL_RADIUS, SHAFT_WIDTH, 2.0*M_PI/2000.0/15, -2.0*M_PI/2000.0/15);
+ // Start odometrys
+ odometryInit(0, 1e-3,
+ PROP_WHEEL_RADIUS, PROP_SHAFT_WIDTH,
+ ENCODER3, ENCODER4,
+ 2.0*M_PI/2000.0/15, -2.0*M_PI/2000.0/15);
+ odometryInit(1, 1e-3,
+ INDEP_WHEEL_RADIUS, INDEP_SHAFT_WIDTH,
+ ENCODER1, ENCODER2,
+ 2.0*M_PI/1024.0, -2.0*M_PI/1024.0);
// Init beacon motor
enableMotor(MOTOR2);
@@ -96,6 +106,7 @@ static void init(void)
LED4_OFF();
timer_delay(100);
}
+
}
static void NORETURN ind_process(void)
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/lib/differential_drive.c b/elec/boards/Controller_Motor_STM32/Firmwares/lib/differential_drive.c
index 487ae75..ab84dce 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/lib/differential_drive.c
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/lib/differential_drive.c
@@ -304,6 +304,10 @@ uint8_t dd_add_bezier(float x_end, float y_end, float d1, float d2, float end_an
if (params.trajs[t_ind].initialized == 1) {
return DD_TRAJECTORY_ALREADY_USED;
} else {
+ // Test the validity of the spline
+ if (fabsf(d1) < 0.01 || fabsf(d2) < 0.01) {
+ return DD_BAD_TRAJECTORY;
+ }
traj = ¶ms.trajs[t_ind];
// New trajectory is not enabled
traj->enabled = 0;
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/lib/differential_drive.h b/elec/boards/Controller_Motor_STM32/Firmwares/lib/differential_drive.h
index df33321..2c0a7e4 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/lib/differential_drive.h
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/lib/differential_drive.h
@@ -26,6 +26,7 @@
#define DD_NO_ERROR 0
#define DD_TRAJECTORY_ALREADY_USED 1
+#define DD_BAD_TRAJECTORY 2
#define DD_GHOST_STOPPED 0
#define DD_GHOST_MOVING 1
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/lib/motor_controller.c b/elec/boards/Controller_Motor_STM32/Firmwares/lib/motor_controller.c
index 2c2ae81..c106343 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/lib/motor_controller.c
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/lib/motor_controller.c
@@ -21,6 +21,7 @@ typedef struct
{
uint8_t enable; // Is this controller enabled ?
uint8_t running; // Is this controller running ?
+ uint8_t suspended; // Is this controller suspended ?
uint8_t mode; // Is this a real controller or an HIL controller
uint8_t motor; // Motor ID to control
uint8_t encoder; // Encoder ID to measure motor position from
@@ -178,13 +179,6 @@ static void NORETURN motorController_process(void) {
// Measure motor rotation
encoder_pos = (float)getEncoderPosition(params->encoder);
delta = encoder_pos - params->last_encoder_pos;
- /*if (getEncoderDirection(params->encoder) == ENCODER_DIR_UP) {
- if (delta < 0)
- delta = delta + 65535;
- } else {
- if (delta > 0)
- delta = delta - 65535;
- }*/
if (delta > 32767) {
delta = delta - 65535;
} else if (delta < - 32767) {
@@ -208,12 +202,14 @@ static void NORETURN motorController_process(void) {
params->last_estimate[1] = estimate[1];
params->last_encoder_pos = encoder_pos;
- // Apply command
- if (isnan(params->last_command)) {
- motorSetSpeed(params->motor, 0);
- motor_led_on(params->motor);
- } else {
- motorSetSpeed(params->motor, (int32_t)params->last_command);
+ // Apply command if necessary
+ if (!params->suspended) {
+ if (isnan(params->last_command)) {
+ motorSetSpeed(params->motor, 0);
+ motor_led_on(params->motor);
+ } else {
+ motorSetSpeed(params->motor, (int32_t)params->last_command);
+ }
}
}
timer_waitEvent(&timer); // Wait for the remaining of the sample period
@@ -416,3 +412,28 @@ void mc_change_mode(uint8_t motor, uint8_t new_mode) {
}
}
}
+
+void mc_suspend_controller(uint8_t motor) {
+ control_params_t *params;
+
+ params = &(controllers[get_motor_index(motor)]);
+ if (params->enable)
+ params->suspended = 1;
+ disableMotor(motor);
+}
+
+void mc_reactivate_controller(uint8_t motor) {
+ control_params_t *params;
+
+ params = &(controllers[get_motor_index(motor)]);
+ if (params->enable && params->suspended) {
+ params->last_command = 0;
+ params->last_estimate[0] = get_output_value(params->reference);
+ params->last_estimate[1] = 0;
+ params->last_output = params->last_estimate[0];
+ params->last_encoder_pos = getEncoderPosition(params->encoder);
+ params->suspended = 0;
+ motorSetSpeed(motor, 0);
+ enableMotor(motor);
+ }
+}
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/lib/motor_controller.h b/elec/boards/Controller_Motor_STM32/Firmwares/lib/motor_controller.h
index f78cde4..0d793b2 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/lib/motor_controller.h
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/lib/motor_controller.h
@@ -89,6 +89,13 @@ uint8_t mc_is_controller_running(uint8_t motor);
* Returns 'motor' 's controller mode
*/
uint8_t mc_controller_mode(uint8_t motor);
-
+/*
+ * Suspends 'motor' 's controller
+ */
+void mc_suspend_controller(uint8_t motor);
+/*
+ * Get back on-line a controller
+ */
+void mc_reactivate_controller(uint8_t motor);
#endif
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/lib/odometry.c b/elec/boards/Controller_Motor_STM32/Firmwares/lib/odometry.c
index 6b33924..33a597d 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/lib/odometry.c
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/lib/odometry.c
@@ -15,6 +15,7 @@ static cpu_stack_t stack_odometry[MAX_ODOMETRY_PROCESSES][(ODOMETRY_STACK_SIZE +
typedef struct {
robot_state_t robot_state;
float wheel_radius, shaft_width, left_encoder_gain, right_encoder_gain;
+ uint8_t left_encoder, right_encoder;
float Ts;
uint8_t enable;
uint8_t running;
@@ -24,7 +25,10 @@ odometry_state_t state[MAX_ODOMETRY_PROCESSES];
static void NORETURN odometry_process(void);
-void odometryInit(uint8_t process_num, float Ts, float wheel_radius, float shaft_width, float left_encoder_gain, float right_encoder_gain) {
+void odometryInit(uint8_t process_num, float Ts,
+ float wheel_radius, float shaft_width,
+ uint8_t left_encoder, uint8_t right_encoder,
+ float left_encoder_gain, float right_encoder_gain) {
// Initialize initial state
state[process_num].robot_state.x = 0.;
@@ -34,6 +38,8 @@ void odometryInit(uint8_t process_num, float Ts, float wheel_radius, float shaft
// Initialize robot parameters
state[process_num].wheel_radius = wheel_radius;
state[process_num].shaft_width = shaft_width;
+ state[process_num].left_encoder = left_encoder;
+ state[process_num].right_encoder = right_encoder;
state[process_num].left_encoder_gain = left_encoder_gain;
state[process_num].right_encoder_gain = right_encoder_gain;
state[process_num].Ts = Ts;
@@ -57,7 +63,6 @@ void odo_restart(uint8_t process_num) {
static void NORETURN odometry_process(void) {
float pos_l, pos_r, last_pos_l, last_pos_r, delta_l, delta_r;
- uint8_t dir_l, dir_r;
Timer timer;
odometry_state_t *state;
@@ -72,8 +77,8 @@ static void NORETURN odometry_process(void) {
state->running = 1;
// State initialization
- last_pos_l = (float)getEncoderPosition(ENCODER3);
- last_pos_r = (float)getEncoderPosition(ENCODER4);
+ last_pos_l = (float)getEncoderPosition(state->left_encoder);
+ last_pos_r = (float)getEncoderPosition(state->right_encoder);
while (1) {
if (state->enable == 0) {
@@ -83,8 +88,8 @@ static void NORETURN odometry_process(void) {
timer_add(&timer);
// Measure motors rotation and correct wrapping
- pos_l = (float)getEncoderPosition(ENCODER3); dir_l = getEncoderDirection(ENCODER3);
- pos_r = (float)getEncoderPosition(ENCODER4); dir_r = getEncoderDirection(ENCODER4);
+ pos_l = (float)getEncoderPosition(state->left_encoder);
+ pos_r = (float)getEncoderPosition(state->right_encoder);
delta_l = pos_l - last_pos_l;
delta_r = pos_r - last_pos_r;
if (delta_l > 32767) {
@@ -97,20 +102,6 @@ static void NORETURN odometry_process(void) {
} else if (delta_r < - 32767) {
delta_r = delta_r + 65535;
}
- /*if (dir_l == ENCODER_DIR_UP) {
- if (delta_l < 0)
- delta_l = delta_l + 65535;
- } else {
- if (delta_l > 0)
- delta_l = delta_l - 65535;
- }
- if (dir_r == ENCODER_DIR_UP) {
- if (delta_r < 0)
- delta_r = delta_r + 65535;
- } else {
- if (delta_r > 0)
- delta_r = delta_r - 65535;
- }*/
delta_l *= state->left_encoder_gain;
delta_r *= state->right_encoder_gain;
last_pos_l = pos_l;
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/lib/odometry.h b/elec/boards/Controller_Motor_STM32/Firmwares/lib/odometry.h
index cac8c83..dcafed4 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/lib/odometry.h
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/lib/odometry.h
@@ -22,7 +22,10 @@ typedef struct {
float theta;
} robot_state_t;
-void odometryInit(uint8_t process_num, float Ts, float wheel_radius, float shaft_width, float left_encoder_gain, float right_encoder_gain);
+void odometryInit(uint8_t process_num, float Ts,
+ float wheel_radius, float shaft_width,
+ uint8_t left_encoder, uint8_t right_encoder,
+ float left_encoder_gain, float right_encoder_gain);
void odo_disable(uint8_t process_num);
void odo_restart(uint8_t process_num);
diff --git a/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/cpu/cortex-m3/drv/clock_stm32.h b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/cpu/cortex-m3/drv/clock_stm32.h
index e91753d..5c9203b 100644
--- a/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/cpu/cortex-m3/drv/clock_stm32.h
+++ b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/cpu/cortex-m3/drv/clock_stm32.h
@@ -234,12 +234,30 @@
#define RCC_APB2_GPIOC (0x00000010)
#define RCC_APB2_GPIOD (0x00000020)
#define RCC_APB2_GPIOE (0x00000040)
+#define RCC_APB2_GPIOF (0x00000080)
+#define RCC_APB2_GPIOG (0x00000100)
#define RCC_APB2_ADC1 (0x00000200)
#define RCC_APB2_ADC2 (0x00000400)
#define RCC_APB2_TIM1 (0x00000800)
#define RCC_APB2_SPI1 (0x00001000)
+#define RCC_APB2_TIM8 (0x00002000)
#define RCC_APB2_USART1 (0x00004000)
-#define RCC_APB2_ALL (0x00005E7D)
+#define RCC_APB2_ADC3 (0x00008000)
+#define RCC_APB2_ALL (RCC_APB2_AFIO \
+ | RCC_APB2_GPIOA \
+ | RCC_APB2_GPIOB \
+ | RCC_APB2_GPIOC \
+ | RCC_APB2_GPIOD \
+ | RCC_APB2_GPIOE \
+ | RCC_APB2_GPIOF \
+ | RCC_APB2_GPIOG \
+ | RCC_APB2_ADC1 \
+ | RCC_APB2_ADC2 \
+ | RCC_APB2_TIM1 \
+ | RCC_APB2_SPI1 \
+ | RCC_APB2_TIM8 \
+ | RCC_APB2_USART1 \
+ | RCC_APB2_ADC3)
/**
* RCC register: BCDR
diff --git a/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/cpu/cortex-m3/drv/irq_cm3.c b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/cpu/cortex-m3/drv/irq_cm3.c
index 14311d9..bb8ac66 100644
--- a/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/cpu/cortex-m3/drv/irq_cm3.c
+++ b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/cpu/cortex-m3/drv/irq_cm3.c
@@ -95,8 +95,10 @@ static void sysirq_enable(sysirq_t irq)
/* Enable the IRQ line (only for generic IRQs) */
if (irq >= 16 && irq < 48)
NVIC_EN0_R = 1 << (irq - 16);
- else if (irq >= 48)
+ else if (irq >= 48 && irq < 80)
NVIC_EN1_R = 1 << (irq - 48);
+ else if (irq >= 80)
+ NVIC_EN2_R = 1 << (irq - 80);
}
void sysirq_setHandler(sysirq_t irq, sysirq_handler_t handler)
diff --git a/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/cpu/cortex-m3/io/stm32_ints.h b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/cpu/cortex-m3/io/stm32_ints.h
index 7fde933..c425b59 100644
--- a/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/cpu/cortex-m3/io/stm32_ints.h
+++ b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/cpu/cortex-m3/io/stm32_ints.h
@@ -83,8 +83,25 @@
#define EXTI15_10_IRQHANDLER 56 /* External Line[15:10] Interrupts */
#define RTCALARM_IRQHANDLER 57 /* RTC Alarm through EXTI Line Interrupt */
#define USBWAKEUP_IRQHANDLER 58 /* USB WakeUp from suspend through EXTI Line Interrupt */
+#define TIM8_BRK_IRQHANDLER 59 /*TIM8 Break Interrupt */
+#define TIM8_UP_IRQHANDLER 60 /*TIM8 Update Interrupt */
+#define TIM8_TRG_COM_IRQHANDLER 61 /*TIM8 Trigger and Commutation Interrupt */
+#define TIM8_CC_IRQHANDLER 62 /*TIM8 Capture Compare Interrupt */
+#define ADC3_IRQHANDLER 63 /*ADC3 global Interrupt */
+#define FSMC_IRQHANDLER 64 /*FSMC global Interrupt */
+#define SDIO_IRQHANDLER 65 /*SDIO global Interrupt */
+#define TIM5_IRQHANDLER 66 /*TIM5 global Interrupt */
+#define SPI3_IRQHANDLER 67 /*SPI3 global Interrupt */
+#define UART4_IRQHANDLER 68 /*UART4 global Interrupt */
+#define UART5_IRQHANDLER 69 /*UART5 global Interrupt */
+#define TIM6_IRQHANDLER 70 /*TIM6 global Interrupt */
+#define TIM7_IRQHANDLER 71 /*TIM7 global Interrupt */
+#define DMA2_Channel1_IRQHANDLER 72 /*DMA2 Channel 1 global Interrupt */
+#define DMA2_Channel2_IRQHANDLER 73 /*DMA2 Channel 2 global Interrupt */
+#define DMA2_Channel3_IRQHANDLER 74 /*DMA2 Channel 3 global Interrupt */
+#define DMA2_Channel4_5_IRQHANDLER 75 /* DMA2 Channel 4 and Channel 5 global Interrupt */
/*\}*/
-#define NUM_INTERRUPTS 66
+#define NUM_INTERRUPTS 76
#endif /* STM32_INTS_H */
diff --git a/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/cpu/cortex-m3/io/stm32_nvic.h b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/cpu/cortex-m3/io/stm32_nvic.h
index 9a209f3..6b35d10 100644
--- a/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/cpu/cortex-m3/io/stm32_nvic.h
+++ b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/bertos/cpu/cortex-m3/io/stm32_nvic.h
@@ -63,6 +63,7 @@
#define NVIC_ST_CAL_R (*((reg32_t *)0xE000E01C))
#define NVIC_EN0_R (*((reg32_t *)0xE000E100))
#define NVIC_EN1_R (*((reg32_t *)0xE000E104))
+#define NVIC_EN2_R (*((reg32_t *)0xE000E108))
#define NVIC_DIS0_R (*((reg32_t *)0xE000E180))
#define NVIC_DIS1_R (*((reg32_t *)0xE000E184))
#define NVIC_PEND0_R (*((reg32_t *)0xE000E200))
diff --git a/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/sensor_actuator.cbp b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/sensor_actuator.cbp
new file mode 100644
index 0000000..ae3b3e2
--- /dev/null
+++ b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/sensor_actuator.cbp
@@ -0,0 +1,202 @@
+<?xml version="1.0" encoding="UTF-8" standalone="yes" ?>
+<CodeBlocks_project_file>
+ <FileVersion major="1" minor="6" />
+ <Project>
+ <Option title="sensor_actuator" />
+ <Option makefile_is_custom="1" />
+ <Option pch_mode="2" />
+ <Option compiler="gcc" />
+ <MakeCommands>
+ <Build command="make" />
+ <CompileFile command="make" />
+ <Clean command="make clean" />
+ <DistClean command="$make -f $makefile distclean$target" />
+ <AskRebuildNeeded command="$make -q -f $makefile $target" />
+ <SilentBuild command="$make -s -f $makefile $target" />
+ </MakeCommands>
+ <Build>
+ <Target title=" ">
+ <Option output="bin/Release/sensor_actuator" prefix_auto="1" extension_auto="1" />
+ <Option object_output="obj/Release/" />
+ <Option type="1" />
+ <Option compiler="gcc" />
+ <Compiler>
+ <Add option="-O2" />
+ </Compiler>
+ <Linker>
+ <Add option="-s" />
+ </Linker>
+ <MakeCommands>
+ <Build command="make" />
+ <CompileFile command="make" />
+ <Clean command="make clean" />
+ <DistClean command="$make -f $makefile distclean$target" />
+ <AskRebuildNeeded command="$make -q -f $makefile $target" />
+ <SilentBuild command="$make -s -f $makefile $target" />
+ </MakeCommands>
+ </Target>
+ </Build>
+ <Compiler>
+ <Add option="-Wall" />
+ </Compiler>
+ <Unit filename="sensor_actuator/adc/adc.c">
+ <Option compilerVar="CC" />
+ </Unit>
+ <Unit filename="sensor_actuator/adc/adc.h" />
+ <Unit filename="sensor_actuator/ax12/ax12.c">
+ <Option compilerVar="CC" />
+ </Unit>
+ <Unit filename="sensor_actuator/ax12/ax12.h" />
+ <Unit filename="sensor_actuator/ax12/ax12_highlevel.c">
+ <Option compilerVar="CC" />
+ </Unit>
+ <Unit filename="sensor_actuator/ax12/ax12_highlevel.h" />
+ <Unit filename="sensor_actuator/ax12/serial.c">
+ <Option compilerVar="CC" />
+ </Unit>
+ <Unit filename="sensor_actuator/ax12/serial.h" />
+ <Unit filename="sensor_actuator/battery_monitoring/ads7828.c">
+ <Option compilerVar="CC" />
+ </Unit>
+ <Unit filename="sensor_actuator/battery_monitoring/ads7828.h" />
+ <Unit filename="sensor_actuator/battery_monitoring/battery_monitoring.c">
+ <Option compilerVar="CC" />
+ </Unit>
+ <Unit filename="sensor_actuator/battery_monitoring/battery_monitoring.h" />
+ <Unit filename="sensor_actuator/beacon/beacon.c">
+ <Option compilerVar="CC" />
+ </Unit>
+ <Unit filename="sensor_actuator/beacon/beacon.h" />
+ <Unit filename="sensor_actuator/beacon/stm32lib/core_cm3.h" />
+ <Unit filename="sensor_actuator/beacon/stm32lib/stm32f10x.h" />
+ <Unit filename="sensor_actuator/beacon/stm32lib/stm32f10x_rcc.c">
+ <Option compilerVar="CC" />
+ </Unit>
+ <Unit filename="sensor_actuator/beacon/stm32lib/stm32f10x_rcc.h" />
+ <Unit filename="sensor_actuator/beacon/stm32lib/stm32f10x_tim.c">
+ <Option compilerVar="CC" />
+ </Unit>
+ <Unit filename="sensor_actuator/beacon/stm32lib/stm32f10x_tim.h" />
+ <Unit filename="sensor_actuator/can/can_messages.h" />
+ <Unit filename="sensor_actuator/can/can_monitor.c">
+ <Option compilerVar="CC" />
+ </Unit>
+ <Unit filename="sensor_actuator/can/can_monitor.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_adc.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_afsk.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_ax25.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_battfs.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_can.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_context_switch.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_dac.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_dataflash.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_dc_motor.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_debug.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_eeprom.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_emb_flash.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_eth.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_fat.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_flash25.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_formatwr.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_gfx.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_hashtable.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_heap.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_i2c.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_i2s.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_ini_reader.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_kbd.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_keytag.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_kfile.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_lcd_32122a.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_lcd_hd44.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_led_7seg.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_lm75.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_lwip.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_md2.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_monitor.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_nand.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_nmea.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_parser.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_phase.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_pocketbus.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_proc.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_pwm.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_ramp.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_random.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_randpool.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_sd.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_sem.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_ser.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_signal.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_spi_bitbang.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_stepper.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_tas5706a.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_tftp.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_thermo.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_timer.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_usb.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_usbkbd.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_usbmouse.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_usbser.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_wdt.h" />
+ <Unit filename="sensor_actuator/cfg/cfg_xmodem.h" />
+ <Unit filename="sensor_actuator/hw/hw_afsk.h" />
+ <Unit filename="sensor_actuator/hw/hw_buzzer.h" />
+ <Unit filename="sensor_actuator/hw/hw_dataflash.c">
+ <Option compilerVar="CC" />
+ </Unit>
+ <Unit filename="sensor_actuator/hw/hw_dataflash.h" />
+ <Unit filename="sensor_actuator/hw/hw_dc_motor.h" />
+ <Unit filename="sensor_actuator/hw/hw_ft245rl.h" />
+ <Unit filename="sensor_actuator/hw/hw_hx8347.h" />
+ <Unit filename="sensor_actuator/hw/hw_i2c_bitbang.h" />
+ <Unit filename="sensor_actuator/hw/hw_ili9225.h" />
+ <Unit filename="sensor_actuator/hw/hw_kbd.h" />
+ <Unit filename="sensor_actuator/hw/hw_lcd_32122a.h" />
+ <Unit filename="sensor_actuator/hw/hw_lcd_hd44.h" />
+ <Unit filename="sensor_actuator/hw/hw_led.h" />
+ <Unit filename="sensor_actuator/hw/hw_led_7seg.h" />
+ <Unit filename="sensor_actuator/hw/hw_lm75.h" />
+ <Unit filename="sensor_actuator/hw/hw_mcp41.c">
+ <Option compilerVar="CC" />
+ </Unit>
+ <Unit filename="sensor_actuator/hw/hw_mcp41.h" />
+ <Unit filename="sensor_actuator/hw/hw_ntc.c">
+ <Option compilerVar="CC" />
+ </Unit>
+ <Unit filename="sensor_actuator/hw/hw_ntc.h" />
+ <Unit filename="sensor_actuator/hw/hw_phase.c">
+ <Option compilerVar="CC" />
+ </Unit>
+ <Unit filename="sensor_actuator/hw/hw_phase.h" />
+ <Unit filename="sensor_actuator/hw/hw_rit128x96.h" />
+ <Unit filename="sensor_actuator/hw/hw_sd.h" />
+ <Unit filename="sensor_actuator/hw/hw_ser.h" />
+ <Unit filename="sensor_actuator/hw/hw_sipo.h" />
+ <Unit filename="sensor_actuator/hw/hw_spi.h" />
+ <Unit filename="sensor_actuator/hw/hw_stepper.h" />
+ <Unit filename="sensor_actuator/hw/hw_tas5706a.h" />
+ <Unit filename="sensor_actuator/hw/hw_thermo.h" />
+ <Unit filename="sensor_actuator/hw/hw_tlv5618.h" />
+ <Unit filename="sensor_actuator/hw/hw_tmp123.h" />
+ <Unit filename="sensor_actuator/hw/kbd_map.h" />
+ <Unit filename="sensor_actuator/hw/mcp41_map.h" />
+ <Unit filename="sensor_actuator/hw/ntc_map.h" />
+ <Unit filename="sensor_actuator/hw/phase_map.h" />
+ <Unit filename="sensor_actuator/hw/pwm_map.h" />
+ <Unit filename="sensor_actuator/hw/thermo_map.h" />
+ <Unit filename="sensor_actuator/main.c">
+ <Option compilerVar="CC" />
+ </Unit>
+ <Unit filename="sensor_actuator/switch/switch.c">
+ <Option compilerVar="CC" />
+ </Unit>
+ <Unit filename="sensor_actuator/switch/switch.h" />
+ <Extensions>
+ <envvars />
+ <code_completion />
+ <debugger />
+ <lib_finder disable_auto="1" />
+ </Extensions>
+ </Project>
+</CodeBlocks_project_file>
diff --git a/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/sensor_actuator/beacon/beacon.c b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/sensor_actuator/beacon/beacon.c
index 6d180ea..be33ec7 100644
--- a/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/sensor_actuator/beacon/beacon.c
+++ b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/sensor_actuator/beacon/beacon.c
@@ -26,149 +26,158 @@
static struct Event got_capture;
static struct Event updated_beacon;
-static uint16_t cur_period = 0;
-static uint16_t cur_width = 0;
-
+static uint16_t beacon_idx = 0;
+static uint16_t beacon_measured = 0;
+static uint32_t beacon_start[MAX_BEACONS] = {0};
+static uint32_t beacon_stop[MAX_BEACONS] = {0};
static uint32_t period = 0;
-static uint16...
[truncated message content] |
