Menu
▾
▴
[Krobot-commits] [SCM] krobot branch, master, updated. 3fd27c9f52ebfe4588a658f93d641636e39bdc1b
|
From: Xavier L. <Ba...@us...> - 2014-05-12 00:23:50
|
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 3fd27c9f52ebfe4588a658f93d641636e39bdc1b (commit)
via 1cea11f47cb9ceb0701bb017264f4d8492754773 (commit)
via 0c63a2e2a45bfe4a14222541a555befa5bb55437 (commit)
via 5f3876a31395d1f29dfaf44c4445995d7ab24722 (commit)
via 994929ba5bce7ee3387ea81fc0332104f6b3ff9d (commit)
via 5a83dc24890f992491b5ef7582ff8e76a3120a08 (commit)
via 315d201eac4a75dec3795a0f8cd5e1e168c74d79 (commit)
via 5a4a54b8904d0781dd7fe6d37b8123e158a16996 (commit)
via 474b5f30be79772343e5073ae5cd7d31b4f1f773 (commit)
via 66cbb39e243aee04550be39bbc86f333523e46c9 (commit)
via 92b82b451da8b0bc99264986f5a431c2ef8f5f89 (commit)
via 95f83897c7d2531942b06e5d56bac9220e364856 (commit)
via d37942788c1bfd5edde52e2dd975534e66ec80c6 (commit)
via ecc660db1c451278ffd064ec1cfc8a0385e090e9 (commit)
via eb03d55164987db98e4b345d7b928e511607ba05 (commit)
via 76c179c01a444c72d7d58a2fbfbf0960423c4385 (commit)
via 41e6e5c280a84237c99a1713b414fc5ef286c27a (commit)
via ca92842c71d8b988cc21e896212d0a1960ebd339 (commit)
via 685980f1ed1e10de6d368434feac3947b5a1b910 (commit)
from 233667799df4892865de6877584ef3c9ac89dc6c (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 3fd27c9f52ebfe4588a658f93d641636e39bdc1b
Author: Xavier Lagorce <Xav...@cr...>
Date: Mon May 12 02:25:06 2014 +0200
[Controller_Motor_STM32/Propulsion_Drive] Change motor direction (added functionnality in motor library)
commit 1cea11f47cb9ceb0701bb017264f4d8492754773
Author: Xavier Lagorce <Xav...@cr...>
Date: Mon May 12 00:53:36 2014 +0200
[Krobot_config] Added new size of the robot (without arms) and adapted viewer
commit 0c63a2e2a45bfe4a14222541a555befa5bb55437
Author: Xavier Lagorce <Xav...@cr...>
Date: Sun May 11 23:03:53 2014 +0200
[info] Added new CAN messages for lifts and pumps
commit 5f3876a31395d1f29dfaf44c4445995d7ab24722
Author: Xavier Lagorce <Xav...@cr...>
Date: Sun May 11 22:32:34 2014 +0200
[Controller_Motor_STM32/Effectors] Send correct CAN id for lift states
commit 994929ba5bce7ee3387ea81fc0332104f6b3ff9d
Author: Xavier Lagorce <Xav...@cr...>
Date: Sun May 11 22:18:39 2014 +0200
[Controller_Motor_STM32/Effectors] Update for 2014's configuration
commit 5a83dc24890f992491b5ef7582ff8e76a3120a08
Author: Xavier Lagorce <Xav...@cr...>
Date: Sun May 11 22:17:33 2014 +0200
[Controller_Motor_STM32/lib] Added a function to get current command applyed to a MOTOR
commit 315d201eac4a75dec3795a0f8cd5e1e168c74d79
Author: Xavier Lagorce <Xav...@cr...>
Date: Sun May 11 22:17:06 2014 +0200
[Controller_Motor_STM32/Effectors] Updated openocd configuration
commit 5a4a54b8904d0781dd7fe6d37b8123e158a16996
Author: Xavier Lagorce <Xav...@cr...>
Date: Sun May 11 22:16:23 2014 +0200
[Controller_Motor_STM32] Update lifts configuration for 2014's robot.
commit 474b5f30be79772343e5073ae5cd7d31b4f1f773
Author: Xavier Lagorce <Xav...@cr...>
Date: Sun May 11 22:15:42 2014 +0200
[Controller_Motor_STM32] No more motor on output MOTOR2
commit 66cbb39e243aee04550be39bbc86f333523e46c9
Author: Xavier Lagorce <Xav...@cr...>
Date: Sun May 11 22:13:55 2014 +0200
[Controller_Motor_STM32] Moved can messages description to a common location for different firmwares
commit 92b82b451da8b0bc99264986f5a431c2ef8f5f89
Author: Pierre Chambart <pie...@cr...>
Date: Sun May 11 22:30:04 2014 +0200
[info/urg] Add config to limit visible points
commit 95f83897c7d2531942b06e5d56bac9220e364856
Author: Xavier Lagorce <Xav...@cr...>
Date: Sun May 11 17:02:58 2014 +0200
[info/viewer] Replaced Blue by Yellow in Viewer UI
commit d37942788c1bfd5edde52e2dd975534e66ec80c6
Author: Xavier Lagorce <Xav...@cr...>
Date: Fri May 9 19:25:17 2014 +0200
[elec/Controller_Motor_STM32] Lift FRONT/BACK are now LEFT/RIGHT
commit ecc660db1c451278ffd064ec1cfc8a0385e090e9
Author: Xavier Lagorce <Xav...@cr...>
Date: Fri May 9 19:24:06 2014 +0200
[info/control2011] New table drawing, configuration, obstacles and objects
commit eb03d55164987db98e4b345d7b928e511607ba05
Author: Xavier Lagorce <Xav...@cr...>
Date: Fri May 9 16:49:17 2014 +0200
[elec/boards] Added motherboard documentation
commit 76c179c01a444c72d7d58a2fbfbf0960423c4385
Author: Pierre Chambart <pie...@cr...>
Date: Fri May 9 00:10:48 2014 +0200
Krobot_example: loop
commit 41e6e5c280a84237c99a1713b414fc5ef286c27a
Author: Pierre Chambart <pie...@cr...>
Date: Fri May 9 00:10:30 2014 +0200
Hackish help for the pathfinder
commit ca92842c71d8b988cc21e896212d0a1960ebd339
Author: Pierre Chambart <pie...@cr...>
Date: Fri May 9 00:08:00 2014 +0200
Typo in krobot_action doc
commit 685980f1ed1e10de6d368434feac3947b5a1b910
Author: Pierre Chambart <pie...@cr...>
Date: Thu May 8 18:37:03 2014 +0200
[info/src/tools] simple startegy launcher example
-----------------------------------------------------------------------
Changes:
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/can_messages.h b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/can_messages.h
deleted file mode 100644
index 3fe2b50..0000000
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/can_messages.h
+++ /dev/null
@@ -1,176 +0,0 @@
-/*
- * can_messages.h
- * --------------
- * Copyright : (c) 2011, Xavier Lagorce <Xav...@cr...>
- * Licence : BSD3
- *
- * This file is a part of [kro]bot.
- */
-
-#ifndef __CAN_MESSAGES_H
-#define __CAN_MESSAGES_H
-
-#include <stdint.h>
-
-/* +-----------------------------------------------------------------+
- | CAN messages IDs |
- +-----------------------------------------------------------------+ */
-
-// Data sent by the card
-#define CAN_MSG_ENCODERS34 100 // encoder_can_msg_t
-#define CAN_MSG_MOTOR3 101 // motor_can_msg_t
-#define CAN_MSG_MOTOR4 102 // motor_can_msg_t
-#define CAN_MSG_STATUS 103 // status_can_msg_t
-#define CAN_MSG_ODOMETRY 104 // odometry_can_msg_t
-#define CAN_MSG_GHOST 105 // ghost_can_msg_t
-
-// Received commands
-#define CAN_MSG_MOVE 201 // move_can_msg_t
-#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_BEZIER_ADD 206 // bezier_can_msg_t
-
-#define CAN_MSG_LIFT_CMD 231 // lift_cmd_msg_t
-/* +-----------------------------------------------------------------+
- | CAN messages data structures |
- +-----------------------------------------------------------------+ */
-
-// Information about two encoders
-typedef struct {
- uint16_t encoder1_pos __attribute__((__packed__));
- uint16_t encoder2_pos __attribute__((__packed__));
- uint8_t encoder1_dir;
- uint8_t encoder2_dir;
- uint16_t padding __attribute__((__packed__));
-} encoder_msg_t;
-
-// Position and speed of one controller motor
-typedef struct {
- float position __attribute__((__packed__)); // angle in radians
- float speed __attribute__((__packed__)); // speed in rad/s (estimation)
-} motor_msg_t;
-
-// Trajectory controller states
-typedef struct {
- uint8_t is_moving;
-} status_msg_t;
-
-// Robot state
-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/10000 radians
-} odometry_msg_t;
-
-// Ghost state for differential drive system
-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/10000 radians
- uint8_t u; // Parameter on the spline between 0 and 255
- uint8_t state; // 1 if trajectory in progress, 0 else
-} ghost_msg_t;
-
-// Move command
-typedef struct {
- int32_t distance __attribute__((__packed__)); // Distance in mm (fixed point representation...)
- uint16_t speed __attribute__((__packed__)); // Speed in mm/s
- uint16_t acceleration __attribute__((__packed__)); // Acceleration in mm/s^2
-} move_msg_t;
-
-// Turn command
-typedef struct {
- 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;
-
-// Add a new Bezier Spline to the wait queue
-typedef struct {
- uint16_t x_end:12 __attribute__((__packed__)); // end point x coordinate in mm
- uint16_t y_end:12 __attribute__((__packed__)); // end point y coordinate in mm
- int16_t d1:9 __attribute__((__packed__)); // first branch length in cm
- uint8_t d2:8 __attribute__((__packed__)); // last branch length in cm
- int16_t theta_end:12 __attribute__((__packed__)); // end angle in 1/100 radians
- uint16_t v_end:11 __attribute__((__packed__)); // final speed in mm/s
-} bezier_msg_t;
-
-// Stop Bezier Spline following and brakes
-typedef struct {
- float lin_acc __attribute__((__packed__)); // Linear acceleration for braking
- float rot_acc __attribute__((__packed__)); // Rotational acceleration for braking
-} stop_msg_t;
-
-// Select robot mode (normal or HIL)
-typedef struct {
- uint8_t mode;
-} controller_mode_msg_t;
-
-// Lift command position, a value is ignored if < 0
-typedef struct {
- float front_lift __attribute__((__packed__));
- float back_lift __attribute__((__packed__));
-} lift_cmd_msg_t;
-
-/* +-----------------------------------------------------------------+
- | CAN messages unions for data representation |
- +-----------------------------------------------------------------+ */
-
-typedef union {
- encoder_msg_t data;
- uint32_t data32[2];
-} encoder_can_msg_t;
-
-typedef union {
- motor_msg_t data;
- uint32_t data32[2];
-} motor_can_msg_t;
-
-typedef union {
- status_msg_t data;
- uint32_t data32[2];
-} status_can_msg_t;
-
-typedef union {
- odometry_msg_t data;
- uint32_t data32[2];
-} odometry_can_msg_t;
-
-typedef union {
- ghost_msg_t data;
- uint32_t data32[2];
-} ghost_can_msg_t;
-
-typedef union {
- move_msg_t data;
- uint32_t data32[2];
-} move_can_msg_t;
-
-typedef union {
- turn_msg_t data;
- uint32_t data32[2];
-} turn_can_msg_t;
-
-typedef union {
- bezier_msg_t data;
- uint32_t data32[2];
-} bezier_can_msg_t;
-
-typedef union {
- stop_msg_t data;
- uint32_t data32[2];
-} stop_can_msg_t;
-
-typedef union {
- controller_mode_msg_t data;
- uint32_t data32[2];
-} controller_mode_can_msg_t;
-
-typedef union {
- lift_cmd_msg_t data;
- uint32_t data32[2];
-} lift_cmd_can_msg_t;
-
-#endif /* __CAN_MESSAGES_H */
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/can_messages_sensors.h b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/can_messages_sensors.h
deleted file mode 100644
index 512077e..0000000
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/can_messages_sensors.h
+++ /dev/null
@@ -1,154 +0,0 @@
-/**
- * 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
-
-
-/****************************************************************************/
-
-/**
- * Message Packets
- */
-
-/**
- * Beacon messages
- */
-
-// Position of the opponent
-struct beacon_position_pkt {
- uint16_t angle; // in 1/10000th of radians [0; 2*Pi[
- uint16_t distance; // in mm [0; 65536[
- uint16_t period; // in 1/10000th of seconds [0; 1[
-} __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));
-
-/****************************************************************************/
-
-/**
- * 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;
-
-#endif
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/can_monitor.c b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/can_monitor.c
index 43913c8..bd48102 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/can_monitor.c
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/can_monitor.c
@@ -45,6 +45,7 @@ void canMonitorInit(void) {
static void NORETURN canMonitor_process(void) {
encoder_can_msg_t msg_enc;
+ pump_state_can_msg_t msg_pumps;
can_tx_frame txm;
Timer timer_can;
@@ -60,15 +61,24 @@ static void NORETURN canMonitor_process(void) {
timer_add(&timer_can);
- // Sending ENCODER3 and ENCODER4 data
- msg_enc.data.encoder1_pos = getEncoderPosition(ENCODER3);
+ // Sending ENCODER2 and ENCODER4 data
+ msg_enc.data.encoder1_pos = getEncoderPosition(ENCODER2);
msg_enc.data.encoder2_pos = getEncoderPosition(ENCODER4);
- msg_enc.data.encoder1_dir = getEncoderDirection(ENCODER3);
+ msg_enc.data.encoder1_dir = getEncoderDirection(ENCODER2);
msg_enc.data.encoder2_dir = getEncoderDirection(ENCODER4);
txm.data32[0] = msg_enc.data32[0];
txm.data32[1] = msg_enc.data32[1];
- txm.eid = CAN_MSG_ENCODERS34;
+ txm.eid = CAN_MSG_LIFT_ENCODERS;
+ can_transmit(CAND1, &txm, ms_to_ticks(10));
+
+ // Sending pump states
+ msg_pumps.data.left_pump = motorGetSpeed(MOTOR1);
+ msg_pumps.data.right_pump = motorGetSpeed(MOTOR3);
+
+ txm.data32[0] = msg_pumps.data32[0];
+ txm.data32[1] = msg_pumps.data32[1];
+ txm.eid = CAN_MSG_PUMP_STATE;
can_transmit(CAND1, &txm, ms_to_ticks(10));
// Wait for the next transmission timer
@@ -87,6 +97,7 @@ static void NORETURN canMonitorListen_process(void) {
switch_status end_courses_msg;
lift_cmd_can_msg_t lift_cmd_msg;
+ pump_cmd_can_msg_t pump_cmd_msg;
// Initialize constant parameters of TX frame
txm.dlc = 8;
@@ -126,21 +137,29 @@ static void NORETURN canMonitorListen_process(void) {
end_courses_msg.d[1] = frame.data32[1];
end_stops = 0;
if (!end_courses_msg.p.sw3)
- end_stops |= LC_BACK_BOTTOM;
+ end_stops |= LC_RIGHT_BOTTOM;
if (!end_courses_msg.p.sw4)
- end_stops |= LC_BACK_UP;
+ end_stops |= LC_RIGHT_UP;
lc_end_stop_reached(end_stops);
//if (end_courses_msg.p.sw1) {
// lc_release();
// motorSetSpeed(MOTOR2,0);
break;
case CAN_MSG_LIFT_CMD:
- lift_cmd_msg.data32[0] = frame.data32[0];
- lift_cmd_msg.data32[1] = frame.data32[1];
- if (lift_cmd_msg.data.front_lift >= 0)
- lc_goto_position(LC_FRONT_LIFT, lift_cmd_msg.data.front_lift);
- if (lift_cmd_msg.data.back_lift >= 0)
- lc_goto_position(LC_BACK_LIFT, lift_cmd_msg.data.back_lift);
+ //lift_cmd_msg.data32[0] = frame.data32[0];
+ //lift_cmd_msg.data32[1] = frame.data32[1];
+ //if (lift_cmd_msg.data.front_lift >= 0)
+ // lc_goto_position(LC_LEFT_LIFT, lift_cmd_msg.data.front_lift);
+ //if (lift_cmd_msg.data.back_lift >= 0)
+ // lc_goto_position(LC_RIGHT_LIFT, lift_cmd_msg.data.back_lift);
+ break;
+ case CAN_MSG_PUMP_CMD:
+ pump_cmd_msg.data32[0] = frame.data32[0];
+ pump_cmd_msg.data32[1] = frame.data32[1];
+ if (pump_cmd_msg.data.left_pump >= 0)
+ motorSetSpeed(MOTOR1, pump_cmd_msg.data.left_pump);
+ if (pump_cmd_msg.data.right_pump >= 0)
+ motorSetSpeed(MOTOR3, pump_cmd_msg.data.right_pump);
break;
}
}
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/can_monitor.h b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/can_monitor.h
index c55f124..e01de3b 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/can_monitor.h
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/can_monitor.h
@@ -18,8 +18,8 @@
#include "trajectory_controller.h"
#include "odometry.h"
#include "differential_drive.h"
-#include "can_messages.h"
-#include "can_messages_sensors.h"
+#include "can_messages/can_messages.h"
+#include "can_messages/can_messages_sensors.h"
#include "lift_controller.h"
void canMonitorInit(void);
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/main.c b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/main.c
index f906450..15f8b65 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/main.c
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/main.c
@@ -15,7 +15,7 @@
#include "motor_controller.h"
#include "can_monitor.h"
#include "command_generator.h"
-#include "differential_drive.h"
+#include "lift_controller.h"
PROC_DEFINE_STACK(stack_ind, KERN_MINSTACKSIZE * 2);
@@ -42,17 +42,21 @@ static void init(void)
// Initialize Command generator
tc_init();
// Initialize Lift Controller
- lc_init();
+ //lc_init();
// Init lifts
- lc_homing(LC_BACK_LIFT);
- lc_goto_position(LC_BACK_LIFT, 0.5);
- while(tc_is_working(TC_MASK(LC_TC_BACK)))
- timer_delay(100);
+ //lc_homing(LC_RIGHT_LIFT);
+ //lc_goto_position(LC_RIGHT_LIFT, 0.5);
+ //while(tc_is_working(TC_MASK(LC_TC_RIGHT)))
+ // timer_delay(100);
+
+ // Setup Left pump motor
+ motorSetMaxPWM(MOTOR1, 1800); // Limit to 12V
+ enableMotor(MOTOR1);
- // Setup Beacon's motor
- motorSetMaxPWM(MOTOR2, 1080);
- enableMotor(MOTOR2);
+ // Setup Right pump motor
+ motorSetMaxPWM(MOTOR3, 1800); // Limit to 12V
+ enableMotor(MOTOR3);
// Blink to say we are ready
for (uint8_t i=0; i < 5; i++) {
@@ -73,13 +77,13 @@ static void NORETURN ind_process(void)
{
while(1) {
LED1_ON();
- lc_goto_position(LC_BACK_LIFT, 1.);
- while(tc_is_working(TC_MASK(LC_TC_BACK)))
+ lc_goto_position(LC_RIGHT_LIFT, 1.);
+ while(tc_is_working(TC_MASK(LC_TC_RIGHT)))
timer_delay(100);
timer_delay(1000);
LED1_OFF();
- lc_goto_position(LC_BACK_LIFT, 0);
- while(tc_is_working(TC_MASK(LC_TC_BACK)))
+ lc_goto_position(LC_RIGHT_LIFT, 0);
+ while(tc_is_working(TC_MASK(LC_TC_RIGHT)))
timer_delay(100);
timer_delay(1000);
}
@@ -92,9 +96,6 @@ int main(void)
/* Create a new child process */
//proc_new(ind_process, NULL, sizeof(stack_ind), stack_ind);
- // Tests
- motorSetSpeed(MOTOR2, 1000);
-
/*
* The main process is kept to periodically report the stack
* utilization of all the processes (1 probe per second).
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/jtag/flash.cfg b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/jtag/flash.cfg
index 2721d3e..b1c2352 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/jtag/flash.cfg
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/jtag/flash.cfg
@@ -1,7 +1,7 @@
init
reset halt
-stm32x mass_erase 0
+stm32f1x mass_erase 0
flash write_bank 0 fw.bin 0
reset init
reset run
-shutdown
\ No newline at end of file
+shutdown
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/jtag/openocd.cfg b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/jtag/openocd.cfg
index d6a58e9..0e09b2b 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/jtag/openocd.cfg
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/jtag/openocd.cfg
@@ -77,7 +77,7 @@ target create $_TARGETNAME cortex_m3 -endian $_ENDIAN -chain-position $_TARGETNA
$_TARGETNAME configure -work-area-phys 0x20000000 -work-area-size $_WORKAREASIZE -work-area-backup 0
set _FLASHNAME $_CHIPNAME.flash
-flash bank $_FLASHNAME stm32x 0 0 0 0 $_TARGETNAME
+flash bank $_FLASHNAME stm32f1x 0 0 0 0 $_TARGETNAME
# For more information about the configuration files, take a look at:
# openocd.texi
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
deleted file mode 100644
index b125c63..0000000
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/can_messages.h
+++ /dev/null
@@ -1,259 +0,0 @@
-/*
- * can_messages.h
- * --------------
- * Copyright : (c) 2011, Xavier Lagorce <Xav...@cr...>
- * Licence : BSD3
- *
- * This file is a part of [kro]bot.
- */
-
-#ifndef __CAN_MESSAGES_H
-#define __CAN_MESSAGES_H
-
-#include <stdint.h>
-
-/* +-----------------------------------------------------------------+
- | CAN messages IDs |
- +-----------------------------------------------------------------+ */
-
-// Data sent by the card
-#define CAN_MSG_ENCODERS34 100 // encoder_can_msg_t
-#define CAN_MSG_MOTOR3 101 // motor_can_msg_t
-#define CAN_MSG_MOTOR4 102 // motor_can_msg_t
-#define CAN_MSG_STATUS 103 // status_can_msg_t
-#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
-#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_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
-#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
-
-/* +-----------------------------------------------------------------+
- | Constants for messages |
- +-----------------------------------------------------------------+ */
-#define SIMULATION_MODE_NO 0
-#define SIMULATION_MODE_NORMAL 1
-#define SIMULATION_MODE_HIL 2
-
-/* +-----------------------------------------------------------------+
- | CAN messages data structures |
- +-----------------------------------------------------------------+ */
-
-// Information about two encoders
-typedef struct {
- uint16_t encoder1_pos __attribute__((__packed__));
- uint16_t encoder2_pos __attribute__((__packed__));
- uint8_t encoder1_dir;
- uint8_t encoder2_dir;
- uint16_t padding __attribute__((__packed__));
-} encoder_msg_t;
-
-// Position and speed of one controller motor
-typedef struct {
- float position __attribute__((__packed__)); // angle in radians
- float speed __attribute__((__packed__)); // speed in rad/s (estimation)
-} motor_msg_t;
-
-// Trajectory controller states
-typedef struct {
- uint8_t is_moving;
-} status_msg_t;
-
-// Error packet
-typedef struct {
- uint8_t err1;
- uint8_t err2;
-} error_msg_t;
-
-// Robot state
-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/10000 radians
-} odometry_msg_t;
-
-// Ghost state for differential drive system
-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/10000 radians
- uint8_t u; // Parameter on the spline between 0 and 255
- 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...)
- uint16_t speed __attribute__((__packed__)); // Speed in mm/s
- uint16_t acceleration __attribute__((__packed__)); // Acceleration in mm/s^2
-} move_msg_t;
-
-// Turn command
-typedef struct {
- 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;
-
-// 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
- uint16_t y_end:12 __attribute__((__packed__)); // end point y coordinate in mm
- int16_t d1:9 __attribute__((__packed__)); // first branch length in cm
- uint8_t d2:8 __attribute__((__packed__)); // last branch length in cm
- int16_t theta_end:12 __attribute__((__packed__)); // end angle in 1/100 radians
- uint16_t v_end:11 __attribute__((__packed__)); // final speed in mm/s
-} bezier_msg_t;
-
-// Modify Bezier Spline trajectory generation limits
-typedef struct {
- uint16_t v_max __attribute__((__packed__)); // max linear speed in mm/s
- uint16_t omega_max __attribute__((__packed__)); // max rotational speed in mrad/s
- uint16_t at_max __attribute__((__packed__)); // max linear acceleration in mm/s/s
- uint16_t ar_max __attribute__((__packed__)); // max radial acceleration in mm/s/s
-} bezier_limits_msg_t;
-
-// Stop Bezier Spline following and brakes
-typedef struct {
- float lin_acc __attribute__((__packed__)); // Linear acceleration for braking
- float rot_acc __attribute__((__packed__)); // Rotational acceleration for braking
-} stop_msg_t;
-
-// Select robot mode (normal, simulation or HIL)
-typedef struct {
- uint8_t mode;
-} simulation_mode_msg_t;
-
-// Command the speed of a particular motor
-typedef struct {
- uint8_t motor_id __attribute__((__packed__));
- int32_t speed __attribute__((__packed__));
-} motor_command_msg_t;
-
-/* +-----------------------------------------------------------------+
- | CAN messages unions for data representation |
- +-----------------------------------------------------------------+ */
-
-typedef union {
- encoder_msg_t data;
- uint32_t data32[2];
-} encoder_can_msg_t;
-
-typedef union {
- motor_msg_t data;
- uint32_t data32[2];
-} motor_can_msg_t;
-
-typedef union {
- status_msg_t data;
- uint32_t data32[2];
-} status_can_msg_t;
-
-typedef union {
- error_msg_t data;
- uint32_t data32[2];
-} error_can_msg_t;
-
-typedef union {
- odometry_msg_t data;
- uint32_t data32[2];
-} odometry_can_msg_t;
-
-typedef union {
- ghost_msg_t data;
- uint32_t data32[2];
-} 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;
-
-typedef union {
- turn_msg_t data;
- uint32_t data32[2];
-} 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;
-
-typedef union {
- bezier_limits_msg_t data;
- uint32_t data32[2];
-} bezier_limits_can_msg_t;
-
-typedef union {
- stop_msg_t data;
- uint32_t data32[2];
-} stop_can_msg_t;
-
-typedef union {
- simulation_mode_msg_t data;
- uint32_t data32[2];
-} simulation_mode_can_msg_t;
-
-typedef union {
- motor_command_msg_t data;
- uint32_t data32[2];
-} motor_command_can_msg_t;
-
-#endif /* __CAN_MESSAGES_H */
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
deleted file mode 100644
index 48c3b90..0000000
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/can_messages_sensors.h
+++ /dev/null
@@ -1,223 +0,0 @@
-/**
- * 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.h b/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/can_monitor.h
index c829c10..5ac94cb 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
@@ -18,8 +18,8 @@
#include "trajectory_controller.h"
#include "odometry.h"
#include "differential_drive.h"
-#include "can_messages.h"
-#include "can_messages_sensors.h"
+#include "can_messages/can_messages.h"
+#include "can_messages/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/main.c b/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/main.c
index 8955dc1..07f4307 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
@@ -18,8 +18,8 @@
#include "differential_drive.h"
//#define PROP_WHEEL_RADIUS 0.049245
-#define PROP_WHEEL_RADIUS_LEFT 0.049207
-#define PROP_WHEEL_RADIUS_RIGHT 0.049283
+#define PROP_WHEEL_RADIUS_RIGHT 0.049207
+#define PROP_WHEEL_RADIUS_LEFT 0.049283
#define PROP_WHEEL_RADIUS ((PROP_WHEEL_RADIUS_RIGHT+PROP_WHEEL_RADIUS_LEFT)/2)
#define PROP_SHAFT_WIDTH 0.22587
@@ -55,6 +55,9 @@ static void init(void)
// Start control of drive motors
tc_init();
+ // Invert motor direction
+ motorInvertDirection(MOTOR3, 1);
+ motorInvertDirection(MOTOR4, 1);
dd_start(CONTROL_ODOMETRY, // Use odometry CONTROL_ODOMETRY for control
PROP_WHEEL_RADIUS_LEFT, PROP_WHEEL_RADIUS_RIGHT, PROP_SHAFT_WIDTH, // Structural parameters
8*2*M_PI, // Absolute wheel speed limitation
@@ -65,9 +68,9 @@ static void init(void)
0.4, 0.7, 1.0, // Controller gains
0.005); // Sample period
// Common parameters
- params.encoder_gain = 2.0*M_PI/2000.0/15;
+ params.encoder_gain = -2.0*M_PI/2000.0/15;
params.T = 0.005;
- // Initialize left motor
+ // Initialize right motor
params.G0 = 0.011686;
params.tau = 0.118;
params.k[0] = -3735.7;
@@ -77,18 +80,18 @@ static void init(void)
params.l0[1] = 0.0108;
params.motor = MOTOR3;
params.encoder = ENCODER3;
- mc_new_controller(¶ms, dd_get_left_wheel_generator(), CONTROLLER_MODE_NORMAL);
- // Initialize right motor
+ mc_new_controller(¶ms, dd_get_right_wheel_generator(), CONTROLLER_MODE_NORMAL);
+ // Initialize left motor
params.motor = MOTOR4;
params.encoder = ENCODER4;
- 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);
+ params.encoder_gain = 2.0*M_PI/2000.0/15; // Left motor is reversed
+ mc_new_controller(¶ms, dd_get_left_wheel_generator(), CONTROLLER_MODE_NORMAL);
// Start odometrys
odometryInit(0, 1e-3,
PROP_WHEEL_RADIUS_LEFT, PROP_WHEEL_RADIUS_RIGHT,
PROP_SHAFT_WIDTH,
- ENCODER3, ENCODER4,
+ ENCODER4, ENCODER3,
2.0*M_PI/2000.0/15, -2.0*M_PI/2000.0/15);
odometryInit(1, 1e-3,
INDEP_WHEEL_RADIUS, INDEP_WHEEL_RADIUS,
@@ -111,7 +114,7 @@ static void init(void)
}
// Enable motor pump
- enableMotor(MOTOR2);
+ //enableMotor(MOTOR2);
}
static void NORETURN ind_process(void)
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/lib/can_messages/can_messages.h b/elec/boards/Controller_Motor_STM32/Firmwares/lib/can_messages/can_messages.h
new file mode 100644
index 0000000..660edd3
--- /dev/null
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/lib/can_messages/can_messages.h
@@ -0,0 +1,298 @@
+/*
+ * can_messages.h
+ * --------------
+ * Copyright : (c) 2011, Xavier Lagorce <Xav...@cr...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ */
+
+#ifndef __CAN_MESSAGES_H
+#define __CAN_MESSAGES_H
+
+#include <stdint.h>
+
+/* +-----------------------------------------------------------------+
+ | CAN messages IDs |
+ +-----------------------------------------------------------------+ */
+
+// Data sent by the card
+#define CAN_MSG_ENCODERS34 100 // encoder_can_msg_t
+#define CAN_MSG_MOTOR3 101 // motor_can_msg_t
+#define CAN_MSG_MOTOR4 102 // motor_can_msg_t
+#define CAN_MSG_STATUS 103 // status_can_msg_t
+#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
+
+#define CAN_MSG_LIFT_ENCODERS 131 // encoder_can_msg_t
+#define CAN_MSG_PUMP_STATE 132 // pump_state_can_msg_t
+
+// Received commands
+#define CAN_MSG_MOVE 201 // move_can_msg_t
+#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_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
+#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
+
+#define CAN_MSG_LIFT_CMD 231 // lift_cmd_msg_t
+#define CAN_MSG_PUMP_CMD 232 // pump_cmd_msg_t
+
+/* +-----------------------------------------------------------------+
+ | Constants for messages |
+ +-----------------------------------------------------------------+ */
+#define SIMULATION_MODE_NO 0
+#define SIMULATION_MODE_NORMAL 1
+#define SIMULATION_MODE_HIL 2
+
+/* +-----------------------------------------------------------------+
+ | CAN messages data structures |
+ +-----------------------------------------------------------------+ */
+
+// Information about two encoders
+typedef struct {
+ uint16_t encoder1_pos __attribute__((__packed__));
+ uint16_t encoder2_pos __attribute__((__packed__));
+ uint8_t encoder1_dir;
+ uint8_t encoder2_dir;
+ uint16_t padding __attribute__((__packed__));
+} encoder_msg_t;
+
+// Position and speed of one controller motor
+typedef struct {
+ float position __attribute__((__packed__)); // angle in radians
+ float speed __attribute__((__packed__)); // speed in rad/s (estimation)
+} motor_msg_t;
+
+// Trajectory controller states
+typedef struct {
+ uint8_t is_moving;
+} status_msg_t;
+
+// Error packet
+typedef struct {
+ uint8_t err1;
+ uint8_t err2;
+} error_msg_t;
+
+// Robot state
+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/10000 radians
+} odometry_msg_t;
+
+// Ghost state for differential drive system
+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/10000 radians
+ uint8_t u; // Parameter on the spline between 0 and 255
+ uint8_t state; // 1 if trajectory in progress, 0 else
+} ghost_msg_t;
+
+// Pumps voltage status
+typedef struct {
+ int16_t left_pump __attribute__((__packed__));
+ int16_t right_pump __attribute__((__packed__));
+} pump_state_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...)
+ uint16_t speed __attribute__((__packed__)); // Speed in mm/s
+ uint16_t acceleration __attribute__((__packed__)); // Acceleration in mm/s^2
+} move_msg_t;
+
+// Turn command
+typedef struct {
+ 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;
+
+// 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
+ uint16_t y_end:12 __attribute__((__packed__)); // end point y coordinate in mm
+ int16_t d1:9 __attribute__((__packed__)); // first branch length in cm
+ uint8_t d2:8 __attribute__((__packed__)); // last branch length in cm
+ int16_t theta_end:12 __attribute__((__packed__)); // end angle in 1/100 radians
+ uint16_t v_end:11 __attribute__((__packed__)); // final speed in mm/s
+} bezier_msg_t;
+
+// Modify Bezier Spline trajectory generation limits
+typedef struct {
+ uint16_t v_max __attribute__((__packed__)); // max linear speed in mm/s
+ uint16_t omega_max __attribute__((__packed__)); // max rotational speed in mrad/s
+ uint16_t at_max __attribute__((__packed__)); // max linear acceleration in mm/s/s
+ uint16_t ar_max __attribute__((__packed__)); // max radial acceleration in mm/s/s
+} bezier_limits_msg_t;
+
+// Stop Bezier Spline following and brakes
+typedef struct {
+ float lin_acc __attribute__((__packed__)); // Linear acceleration for braking
+ float rot_acc __attribute__((__packed__)); // Rotational acceleration for braking
+} stop_msg_t;
+
+// Select robot mode (normal, simulation or HIL)
+typedef struct {
+ uint8_t mode;
+} simulation_mode_msg_t;
+
+// Command the speed of a particular motor
+typedef struct {
+ uint8_t motor_id __attribute__((__packed__));
+ int32_t speed __attribute__((__packed__));
+} motor_command_msg_t;
+
+// Lifts command position, a value is ignored if < 0
+typedef struct {
+ float left_lift __attribute__((__packed__));
+ float right_lift __attribute__((__packed__));
+} lift_cmd_msg_t;
+
+// Pumps voltage command, a value is ignored if < 0
+typedef struct {
+ int16_t left_pump __attribute__((__packed__));
+ int16_t right_pump __attribute__((__packed__));
+} pump_cmd_msg_t;
+
+/* +-----------------------------------------------------------------+
+ | CAN messages unions for data representation |
+ +-----------------------------------------------------------------+ */
+
+typedef union {
+ encoder_msg_t data;
+ uint32_t data32[2];
+} encoder_can_msg_t;
+
+typedef union {
+ motor_msg_t data;
+ uint32_t data32[2];
+} motor_can_msg_t;
+
+typedef union {
+ status_msg_t data;
+ uint32_t data32[2];
+} status_can_msg_t;
+
+typedef union {
+ error_msg_t data;
+ uint32_t data32[2];
+} error_can_msg_t;
+
+typedef union {
+ odometry_msg_t data;
+ uint32_t data32[2];
+} odometry_can_msg_t;
+
+typedef union {
+ ghost_msg_t data;
+ uint32_t data32[2];
+} ghost_can_msg_t;
+
+typedef union {
+ pump_state_msg_t data;
+ uint32_t data32[2];
+} pump_state_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;
+
+typedef union {
+ turn_msg_t data;
+ uint32_t data32[2];
+} 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;
+
+typedef union {
+ bezier_limits_msg_t data;
+ uint32_t data32[2];
+} bezier_limits_can_msg_t;
+
+typedef union {
+ stop_msg_t data;
+ uint32_t data32[2];
+} stop_can_msg_t;
+
+typedef union {
+ simulation_mode_msg_t data;
+ uint32_t data32[2];
+} simulation_mode_can_msg_t;
+
+typedef union {
+ motor_command_msg_t data;
+ uint32_t data32[2];
+} motor_command_can_msg_t;
+
+typedef union {
+ lift_cmd_msg_t data;
+ uint32_t data32[2];
+} lift_cmd_can_msg_t;
+
+typedef union {
+ pump_cmd_msg_t data;
+ uint32_t data32[2];
+} pump_cmd_can_msg_t;
+
+#endif /* __CAN_MESSAGES_H */
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/lib/can_messages/can_messages_sensors.h b/elec/boards/Controller_Motor_STM32/Firmwares/lib/can_messages/can_messages_sensors.h
new file mode 100644
index 0000000..057833f
--- /dev/null
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/lib/can_messages/can_messages_sensors.h
@@ -0,0 +1,242 @@
+/**
+ * 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
+
+// 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
+
+
+/****************************************************************************/
+
+/**
+ * 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;
+};
+
+/**
+ * Simulation mode control
+ */
+struct simulation_mode_pkt {
+ uint8_t mode;
+} __attribute__((packed));
+
+/****************************************************************************/
+
+/**
+ * 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;
+
+typedef union {
+ struct simulation_mode_pkt p;
+ uint32_t d[2];
+} simulation_mode;
+
+#endif
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/lib/lift_controller.c b/elec/boards/Controller_Motor_STM32/Firmwares/lib/lift_controller.c
index 56cb686..7a50213 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/lib/lift_controller.c
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/lib/lift_controller.c
@@ -41,15 +41,15 @@ void lc_init(void) {
lc_state[i].extend = 0;
lc_state[i].end_stop_ind = 0;
}
- lc_state[LC_FRONT_LIFT].tc_ind = LC_TC_FRONT;
- lc_state[LC_BACK_LIFT].tc_ind = LC_TC_BACK;
+ lc_state[LC_LEFT_LIFT].tc_ind = LC_TC_LEFT;
+ lc_state[LC_RIGHT_LIFT].tc_ind = LC_TC_RIGHT;
// Init Trajectory controllers
- tc_new_controller(LC_TC_FRONT);
- tc_new_controller(LC_TC_BACK);
+ tc_new_controller(LC_TC_LEFT);
+ tc_new_controller(LC_TC_RIGHT);
// Limit PWM value
- motorSetMaxPWM(MOTOR3, 1600);
- motorSetMaxPWM(MOTOR4, 1600);
+ motorSetMaxPWM(MOTOR2, 1800); // Limit to 12V
+ motorSetMaxPWM(MOTOR4, 1800); // Limit to 12V
// Common parameters
params.encoder_gain = 2.0*M_PI/588.0;
params.G0 = 0.0035;
@@ -60,32 +60,32 @@ void lc_init(void) {
params.l0[0] = 0.0091;
params.l0[1] = 1.6361;
params.T = 0.005;
- // Initialize front lift
+ // Initialize left lift
+ params.motor = MOTOR2;
+ params.encoder = ENCODER2;
+ mc_new_controller(¶ms, tc_get_position_generator(LC_TC_LEFT), CONTROLLER_MODE_NORMAL);
+ // Initialize right lift
params.motor = MOTOR4;
params.encoder = ENCODER4;
- mc_new_controller(¶ms, tc_get_position_generator(LC_TC_FRONT), CONTROLLER_MODE_NORMAL);
- // Initialize back lift
- params.motor = MOTOR3;
- params.encoder = ENCODER3;
- mc_new_controller(¶ms, tc_get_position_generator(LC_TC_BACK), CONTROLLER_MODE_NORMAL);
+ mc_new_controller(¶ms, tc_get_position_generator(LC_TC_RIGHT), CONTROLLER_MODE_NORMAL);
}
void lc_end_stop_reached(uint8_t end_stops) {
// Set the end stop indicators and stop the lift if needed
- if ((end_stops & LC_FRONT_UP) || (end_stops & LC_FRONT_BOTTOM)) {
- if ((end_stops & LC_FRONT_UP) && (lc_state[LC_FRONT_LIFT].direction == 1))
- tc_stop(LC_TC_FRONT);
- if ((end_stops & LC_FRONT_BOTTOM) && (lc_state[LC_FRONT_LIFT].direction == -1))
- tc_s...
[truncated message content] |
