Menu
▾
▴
[Krobot-commits] [SCM] krobot branch, master, updated. 40a60d3a36a22d90795f4a64353fd34cedc4a79a
|
From: Xavier L. <Ba...@us...> - 2012-01-27 14:36:13
|
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 40a60d3a36a22d90795f4a64353fd34cedc4a79a (commit)
from 5403b5850fb9dd5aadbb3350764821280e44de0e (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 40a60d3a36a22d90795f4a64353fd34cedc4a79a
Author: Xavier Lagorce <Xav...@cr...>
Date: Fri Jan 27 15:38:17 2012 +0100
[Controller_Motor_STM32] Reorganized code into a common 'lib' directory and specific
code for Effectors board and Propulsion_Drive board
-----------------------------------------------------------------------
Changes:
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/bezier_utils.c b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/bezier_utils.c
deleted file mode 100644
index 83fe983..0000000
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/bezier_utils.c
+++ /dev/null
@@ -1,134 +0,0 @@
-/*
- * bezier_utils.c
- * --------------
- * Copyright : (c) 2011, Xavier Lagorce <Xav...@cr...>
- * Licence : BSD3
- *
- * This file is a part of [kro]bot.
- */
-
-#include "bezier_utils.h"
-
-#ifdef BEZIER_UTILS_USE_BERTOS
-#include <cpu/power.h>
-#endif
-
-float bezier_apply(float params[4], float u) {
- return (params[0] + params[1]*u + params[2]*u*u + params[3]*u*u*u);
-}
-
-void bezier_generate(float params[2][4],
- float p_x, float p_y,
- float p1_x, float p1_y,
- float p2_x, float p2_y,
- float s_x, float s_y) {
-
- params[0][0] = p_x;
- params[1][0] = p_y;
- params[0][1] = -3*p_x+3*p1_x;
- params[1][1] = -3*p_y+3*p1_y;
- params[0][2] = 3*p_x-6*p1_x+3*p2_x;
- params[1][2] = 3*p_y-6*p1_y+3*p2_y;
- params[0][3] = -p_x+3*p1_x-3*p2_x+s_x;
- params[1][3] = -p_y+3*p1_y-3*p2_y+s_y;
-}
-
-void bezier_velocity_profile(float dparams[2][4], float ddparams[2][4],
- float v_max, float at_max, float ar_max,
- float v_ini, float v_end,
- float du, float* v_tab) {
- float vmins[4], cr, cr_p, cr_pp, u, vm, dt, nv, dx, dy, dsu;
- int ind = 0, j, nb_pts, mins[4], im, i;
-
- for (i=0, u=0.0; u <= 1.0; i++, u+=du) {
- v_tab[i] = v_max;
- }
- nb_pts = i;
-
- // Looking for curvature radius minima to limit speed at this positions
- mins[ind] = 0;
- vmins[ind] = v_ini;
- ind++;
-
- cr_pp = fabsf(bezier_cr(0, dparams, ddparams));
- cr_p = fabsf(bezier_cr(du, dparams, ddparams));
- for (i=2, u=2*du; u <= 1.0; i++, u+=du) {
- cr = fabsf(bezier_cr(u, dparams, ddparams));
- if ((cr >= cr_p) && (cr_p < cr_pp)) {
- mins[ind] = i;
- vmins[ind] = sqrtf(ar_max*cr);
- ind++;
- }
- cr_pp = cr_p;
- cr_p = cr;
- }
- mins[ind] = nb_pts-1;
- vmins[ind] = v_end;
- ind++;
-
-#ifdef BEZIER_UTILS_USE_BERTOS
- cpu_relax();
-#endif
-
- // Compute speed limitations
- for (j=0; j < ind; j++) {
- im = mins[j];
- vm = vmins[j];
- if (vm < v_max) {
- v_tab[im] = vm;
-
- // Profile for preceding velocity
- for (i = im-1; i >= 0; i--) {
- dx = bezier_apply(dparams[0], du*(i+1));
- dy = bezier_apply(dparams[1], du*(i+1));
- dsu = sqrtf(dx*dx + dy*dy);
- dt = (-v_tab[i+1]+sqrtf(v_tab[i+1]*v_tab[i+1]+2*at_max*du*dsu))/at_max;
- nv = v_tab[i+1]+at_max*dt;
- if (nv < v_tab[i]) {
- v_tab[i] = nv;
- } else {
- break;
- }
- }
- // Profile for following sector
- for (i = im+1; i < nb_pts; i++) {
- dx = bezier_apply(dparams[0], du*(i-1));
- dy = bezier_apply(dparams[1], du*(i-1));
- dsu = sqrtf(dx*dx + dy*dy);
- dt = (-v_tab[i-1]+sqrtf(v_tab[i-1]*v_tab[i-1]+2*at_max*du*dsu))/at_max;
- nv = v_tab[i-1]+at_max*dt;
- if (nv < v_tab[i]) {
- v_tab[i] = nv;
- } else {
- break;
- }
- }
-
-#ifdef BEZIER_UTILS_USE_BERTOS
- cpu_relax();
-#endif
- } // end if (vm < v_max)
- } // for mins
-}
-
-float bezier_cr(float u, float dparams[2][4], float ddparams[2][4]) {
-
- float dx, dy, ddx, ddy;
-
- dx = bezier_apply(dparams[0], u);
- dy = bezier_apply(dparams[1], u);
- ddx = bezier_apply(ddparams[0], u);
- ddy = bezier_apply(ddparams[1], u);
-
- return powf(dx*dx+dy*dy, 1.5) / (dx*ddy - dy*ddx);
-}
-
-void bezier_diff(float params[2][4], float dparams[2][4]) {
- int i, j;
- for (i=0; i<=1; i++) {
- for (j=0; j<=2; j++) {
- dparams[i][j] = (j+1)*params[i][j+1];
- }
- dparams[i][3] = 0.;
- }
-}
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/bezier_utils.h b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/bezier_utils.h
deleted file mode 100644
index 2aa45fd..0000000
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/bezier_utils.h
+++ /dev/null
@@ -1,52 +0,0 @@
-/*
- * bezier_utils.h
- * --------------
- * Copyright : (c) 2011, Xavier Lagorce <Xav...@cr...>
- * Licence : BSD3
- *
- * This file is a part of [kro]bot.
- */
-
-#ifndef __BEZIER_UTILS_H
-#define __BEZIER_UTILS_H
-
-#define BEZIER_UTILS_USE_BERTOS
-
-#include <math.h>
-#include <stdint.h>
-
-/*
- * Apply a polynomial form of a Bezier Spline to a value of the parameter
- */
-float bezier_apply(float params[4], float u);
-
-/*
- * Computes the parameters of the polynomial from of a Bezier Spline
- */
-void bezier_generate(float params[2][4],
- float p_x, float p_y,
- float p1_x, float p1_y,
- float p2_x, float p2_y,
- float s_x, float s_y);
-
-/*
- * Computes the velocity profile along a Bezier Spline and according to physical
- * contraints
- */
-void bezier_velocity_profile(float dparams[2][4], float ddparams[2][4],
- float v_max, float at_max, float ar_max,
- float v_ini, float v_end,
- float du, float* v_tab);
-
-/*
- * Computes the curvature radius of a Bezier Spline for a given value of the
- * parameter
- */
-float bezier_cr(float u, float dparams[2][4], float ddparams[2][4]);
-
-/*
- * Differentiates the polynomial form of a Bezier Spline
- */
-void bezier_diff(float params[2][4], float dparams[2][4]);
-
-#endif /* __BEZIER_UTILS_H */
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/command_generator.c b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/command_generator.c
deleted file mode 100644
index 0d1145a..0000000
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/command_generator.c
+++ /dev/null
@@ -1,254 +0,0 @@
-/*
- * command_generator.c
- * -------------------
- * Copyright : (c) 2011, Xavier Lagorce <Xav...@cr...>
- * Licence : BSD3
- *
- * This file is a part of [kro]bot.
- */
-
-#include "command_generator.h"
-#include <stdlib.h>
-
-command_generator_t* new_constant_generator(command_generator_t *generator, float value) {
- generator->type.t = GEN_CONSTANT;
- generator->type.callback.type = GEN_CALLBACK_NONE;
- generator->constant.gen.last_output = value;
- generator->constant.gen.state = GEN_STATE_PAUSE;
-
- return generator;
-}
-
-command_generator_t* new_ramp_generator(command_generator_t *generator, float starting_value, float speed) {
- generator->type.t = GEN_RAMP;
- generator->type.callback.type = GEN_CALLBACK_NONE;
- generator->ramp.gen.state = GEN_STATE_PAUSE;
- generator->ramp.gen.last_output = starting_value;
- generator->ramp.last_time = 0;
- generator->ramp.speed = speed;
-
- return generator;
-}
-
-command_generator_t* new_ramp2_generator(command_generator_t *generator, float starting_value, command_generator_t *speed) {
- generator->type.t = GEN_RAMP2;
- generator->type.callback.type = GEN_CALLBACK_NONE;
- generator->ramp2.gen.state = GEN_STATE_PAUSE;
- generator->ramp2.gen.last_output = starting_value;
- generator->ramp2.last_time = 0;
- generator->ramp2.speed = speed;
-
- return generator;
-}
-
-command_generator_t* new_dd_generator(command_generator_t *generator,
- command_generator_t *linear_pos,
- command_generator_t *linear_speed,
- command_generator_t *rotational_pos,
- command_generator_t *rotational_speed,
- float wheel_radius, float shaft_width, float max_speed,
- uint8_t type) {
- generator->type.t = (type == 1) ? GEN_DD_RIGHT : GEN_DD_LEFT;
- generator->type.callback.type = GEN_CALLBACK_NONE;
- generator->dd.linear_pos = linear_pos;
- generator->dd.linear_speed = linear_speed;
- generator->dd.rotational_pos = rotational_pos;
- generator->dd.rotational_speed = rotational_speed;
- generator->dd.wheel_radius = wheel_radius;
- generator->dd.shaft_width = shaft_width;
- generator->dd.max_speed = max_speed;
-
- return generator;
-}
-
-command_generator_t* new_hd_generator(command_generator_t *generator,
- command_generator_t *linear_pos_x,
- command_generator_t *linear_speed_x,
- command_generator_t *linear_pos_y,
- command_generator_t *linear_speed_y,
- command_generator_t *rotational_pos,
- command_generator_t *rotational_speed,
- float wheel_radius, float struct_radius, float max_speed,
- uint8_t type) {
- switch (type) {
- case 1: // Back wheel
- generator->type.t = GEN_HD_B;
- break;
- case 2: // Right-front wheel
- generator->type.t = GEN_HD_RF;
- break;
- case 3: // Left-front wheel
- generator->type.t = GEN_HD_LF;
- break;
- default:
- return NULL;
- break;
- }
- generator->type.callback.type = GEN_CALLBACK_NONE;
- generator->hd.linear_pos_x = linear_pos_x;
- generator->hd.linear_speed_x = linear_speed_x;
- generator->hd.linear_pos_x = linear_pos_y;
- generator->hd.linear_speed_x = linear_speed_y;
- generator->hd.rotational_pos = rotational_pos;
- generator->hd.rotational_speed = rotational_speed;
- generator->hd.wheel_radius = wheel_radius;
- generator->hd.struct_radius = struct_radius;
- generator->hd.max_speed = max_speed;
-
- return generator;
-}
-
-
-command_generator_t* adjust_value(command_generator_t *generator, float value) {
- uint8_t type = generator->type.t;
-
- if (type != GEN_DD_RIGHT && type != GEN_DD_LEFT
- && type != GEN_HD_B && type != GEN_HD_RF && type != GEN_HD_LF) {
- generator->type.last_output = value;
- return generator;
- } else {
- return NULL;
- }
-}
-
-command_generator_t* adjust_speed(command_generator_t *generator, float speed) {
- switch (generator->type.t) {
- case GEN_RAMP:
- generator->ramp.speed = speed;
- return generator;
- default:
- return NULL;
- }
-}
-
-command_generator_t* start_generator(command_generator_t *generator) {
- switch (generator->type.t) {
- case GEN_RAMP:
- generator->ramp.last_time = ticks_to_us(timer_clock());
- break;
- case GEN_RAMP2:
- generator->ramp2.last_time = ticks_to_us(timer_clock());
- }
- generator->type.state = GEN_STATE_RUNNING;
-
- return generator;
-}
-
-command_generator_t* pause_generator(command_generator_t *generator) {
- // update generator->type.last_output
- get_output_value(generator);
- // pause the generator
- generator->type.state = GEN_STATE_PAUSE;
-
- return generator;
-}
-
-command_generator_t* add_callback(command_generator_t *generator, uint8_t type, float threshold, void (*callback_function)(command_generator_t*)) {
- generator->type.callback.callback_function = callback_function;
- generator->type.callback.threshold = threshold;
- generator->type.callback.type = type;
-
- return generator;
-}
-
-command_generator_t* remove_callback(command_generator_t *generator) {
- generator->type.callback.type = GEN_CALLBACK_NONE;
-
- return generator;
-}
-
-
-float get_output_value(command_generator_t *generator) {
- int32_t cur_time;
- float speed, dt, u1, u2, u_x, u_y, w;
-
- if (generator->type.state != GEN_STATE_RUNNING)
- return generator->type.last_output;
-
- switch (generator->type.t) {
- case GEN_CONSTANT:
- // Constant generator, no update needed
- break;
- case GEN_RAMP:
- cur_time = ticks_to_us(timer_clock());
- dt = (cur_time - generator->ramp.last_time)*1e-6;
- generator->type.last_output += dt*generator->ramp.speed;
- generator->ramp.last_time = cur_time;
- break;
- case GEN_RAMP2:
- cur_time = ticks_to_us(timer_clock());
- speed = get_output_value(generator->ramp2.speed);
- dt = (cur_time - generator->ramp2.last_time)*1e-6;
- generator->type.last_output += dt*speed;
- generator->ramp2.last_time = cur_time;
- break;
- case GEN_DD_RIGHT:
- case GEN_DD_LEFT:
- // Update position generators to allow callbacks
- get_output_value(generator->dd.linear_pos);
- get_output_value(generator->dd.rotational_pos);
- // Compute output
- u1 = get_output_value(generator->dd.linear_speed);
- u2 = get_output_value(generator->dd.rotational_speed);
- switch (generator->type.t) {
- case GEN_DD_RIGHT:
- generator->type.last_output = (2.0*u1+u2*generator->dd.shaft_width) / (2.0 * generator->dd.wheel_radius);
- break;
- case GEN_DD_LEFT:
- generator->type.last_output = (2.0*u1-u2*generator->dd.shaft_width) / (2.0 * generator->dd.wheel_radius);
- break;
- }
- if (generator->type.last_output >= 0) {
- generator->type.last_output = MIN(generator->type.last_output, generator->dd.max_speed);
- } else {
- generator->type.last_output = MAX(generator->type.last_output, -generator->dd.max_speed);
- }
- break;
- case GEN_HD_B:
- case GEN_HD_RF:
- case GEN_HD_LF:
- // Update position generators to allow callbacks
- get_output_value(generator->hd.linear_pos_x);
- get_output_value(generator->hd.linear_pos_y);
- get_output_value(generator->hd.rotational_pos);
- // Compute output
- u_x = get_output_value(generator->hd.linear_speed_x);
- u_y = get_output_value(generator->hd.linear_speed_y);
- w = get_output_value(generator->hd.rotational_speed);
- switch (generator->type.t) {
- case GEN_HD_B:
- generator->type.last_output =
- (u_x + w*generator->hd.struct_radius) / generator->hd.wheel_radius;
- break;
- case GEN_HD_RF:
- generator->type.last_output =
- (-u_x/2.0 + u_y*sqrt(3.0)/2.0 + w*generator->hd.struct_radius) / generator->hd.wheel_radius;
- break;
- case GEN_HD_LF:
- generator->type.last_output =
- (-u_x/2.0 - u_y*sqrt(3.0)/2.0 + w*generator->hd.struct_radius) / generator->hd.wheel_radius;
- break;
- }
- if (generator->type.last_output >= 0) {
- generator->type.last_output = MIN(generator->type.last_output, generator->hd.max_speed);
- } else {
- generator->type.last_output = MAX(generator->type.last_output, -generator->hd.max_speed);
- }
- break;
- }
-
- switch (generator->type.callback.type) {
- case GEN_CALLBACK_NONE :
- break;
- case GEN_CALLBACK_SUP :
- if (generator->type.last_output > generator->type.callback.threshold)
- generator->type.callback.callback_function(generator);
- break;
- case GEN_CALLBACK_INF :
- if (generator->type.last_output < generator->type.callback.threshold)
- generator->type.callback.callback_function(generator);
- break;
- }
-
- return generator->type.last_output;
-}
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/command_generator.h b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/command_generator.h
deleted file mode 100644
index 3fa2b6a..0000000
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/command_generator.h
+++ /dev/null
@@ -1,224 +0,0 @@
-/*
- * command_generator.h
- * -------------------
- * Copyright : (c) 2011, Xavier Lagorce <Xav...@cr...>
- * Licence : BSD3
- *
- * This module aims at providing a simple way to generate some command signal
- * for control processes (like steps, ramps, ...)
- *
- * This file is a part of [kro]bot.
- */
-
-#ifndef __COMMAND_GENERATOR_H
-#define __COMMAND_GENERATOR_H
-
-#include <drv/timer.h>
-#include <math.h>
-
-// Generator types
-#define GEN_NONE 0 // No type, the generator is not initialized.
-#define GEN_CONSTANT 1 // Outputs a constant.
-#define GEN_RAMP 2 // Outputs a ramp with the given slope.
-#define GEN_RAMP2 3 // Same as GEN_RAMP, but the slope is given by another
- // generator.
-#define GEN_DD_LEFT 4 // Outputs the left wheel speed for a differential
- // drive.
-#define GEN_DD_RIGHT 5 // Outputs the right wheel speed for a differential
- // drive.
-#define GEN_HD_B 6 // Outpus the back wheel speed for an holonomic drive.
-#define GEN_HD_RF 7 // Outpus the right-front wheel speed for an holonomic drive.
-#define GEN_HD_LF 8 // Outpus the left-front wheel speed for an holonomic drive.
-
-// Generator states
-#define GEN_STATE_PAUSE 0 // The output is freezed.
-#define GEN_STATE_RUNNING 1 // The generator is running.
-
-// Threshold comparison for callback trigger
-#define GEN_CALLBACK_NONE 0 // There is no callback.
-#define GEN_CALLBACK_SUP 1 // The callback is triggered if output > threshold.
-#define GEN_CALLBACK_INF 2 // The callback is triggered if output < threshold.
-
-typedef union _command_generator_t command_generator_t;
-
-// Callback description
-typedef struct {
- uint8_t type;
- float threshold;
- void (*callback_function)(command_generator_t*);
-} generator_callback_t;
-
-// Generator descriptions
-typedef struct {
- uint8_t t;
- float last_output;
- uint8_t state;
- generator_callback_t callback;
-} placeholder_generator_t;
-
-typedef struct {
- placeholder_generator_t gen;
-} constant_generator_t;
-
-typedef struct {
- placeholder_generator_t gen;
- int32_t last_time;
- float speed;
-} ramp_generator_t;
-
-typedef struct {
- placeholder_generator_t gen;
- int32_t last_time;
- command_generator_t *speed;
-} ramp2_generator_t;
-
-typedef struct {
- placeholder_generator_t gen;
- command_generator_t *linear_pos;
- command_generator_t *linear_speed;
- command_generator_t *rotational_pos;
- command_generator_t *rotational_speed;
- float wheel_radius;
- float shaft_width;
- float max_speed;
-} dd_generator_t;
-
-typedef struct {
- placeholder_generator_t gen;
- command_generator_t *linear_pos_x;
- command_generator_t *linear_speed_x;
- command_generator_t *linear_pos_y;
- command_generator_t *linear_speed_y;
- command_generator_t *rotational_pos;
- command_generator_t *rotational_speed;
- float wheel_radius;
- float struct_radius;
- float max_speed;
-} hd_generator_t;
-
-// Usable generator meta-type
-union _command_generator_t {
- placeholder_generator_t type;
- constant_generator_t constant;
- ramp_generator_t ramp;
- ramp2_generator_t ramp2;
- dd_generator_t dd;
- hd_generator_t hd;
-};
-
-/* Initializes a new Constant Generator.
- * - generator : pointer to the generator to initialize
- * - value : output value of the generator
- */
-command_generator_t* new_constant_generator(command_generator_t *generator,
- float value);
-
-/* Initializes a new Ramp Generator.
- * - generator : pointer to the generator to initialize
- * - starting_value : initial output value
- * - speed : slope
- */
-command_generator_t* new_ramp_generator(command_generator_t *generator,
- float starting_value, float speed);
-
-/* Initializes a new Ramp2 Generator.
- * - generator : pointer to the generator to initialize
- * - starting_value : initial output value
- * - speed : pointer to the generator which output is used as this generator's slope
- */
-command_generator_t* new_ramp2_generator(command_generator_t *generator,
- float starting_value,
- command_generator_t *speed);
-
-/* Initializes a new Differential Drive generator.
- * - generator : pointer to the generator to initialize
- * - linear_pos : pointer to the generator giving the integrates of linear_speed. This
- * generator will be called at each computation to allow update in parallel
- * with linear_speed.
- * - linear_speed : pointer to the generator giving the linear speed of the drive
- * - rotational_pos : pointer to the generator giving the integrates of rotational_speed. This
- * generator will be called at each computation to allow update in parallel
- * with rotational_speed.
- * - rotational_speed : pointer to the generator giving the rotational speed of the drive
- * - wheel_radius : radius of the wheels
- * - shaft_width : width of the propulsion shaft
- * - max_speed : maximum wheel speed (in rad/s)
- * - type : 1 for the right_wheel, -1 for the left_wheel
- */
-command_generator_t* new_dd_generator(command_generator_t *generator,
- command_generator_t *linear_pos,
- command_generator_t *linear_speed,
- command_generator_t *rotational_pos,
- command_generator_t *rotational_speed,
- float wheel_radius, float shaft_width, float max_speed,
- uint8_t type);
-
-/* Initializes a new Holonomic Drive generator.
- * - generator : pointer to the generator to initialize
- * - linear_pos_x : pointer to the generator giving the integrates of linear_speed_x. This
- * generator will be called at each computation to allow update in parallel
- * with linear_speed.
- * - linear_speed_x : pointer to the generator giving the linear speed along the x axis of
- * the drive.
- * - linear_pos_y : pointer to the generator giving the integrates of linear_speed_y. This
- * generator will be called at each computation to allow update in parallel
- * with linear_speed.
- * - linear_speed_y : pointer to the generator giving the linear speed along the y axis of
- * the drive.
- * - rotational_pos : pointer to the generator giving the integrates of rotational_speed. This
- * generator will be called at each computation to allow update in parallel
- * with rotational_speed.
- * - rotational_speed : pointer to the generator giving the rotational speed of the drive.
- * - wheel_radius : radius of the wheels.
- * - struct_radius : radius of the holonomic drive.
- * - max_speed : maximum wheel speed (in rad/s).
- * - type :
- * o 1 is the back wheel
- * o 2 is the right-front wheel
- * o 3 is the left-front wheel
- */
-command_generator_t* new_hd_generator(command_generator_t *generator,
- command_generator_t *linear_pos_x,
- command_generator_t *linear_speed_x,
- command_generator_t *linear_pos_y,
- command_generator_t *linear_speed_y,
- command_generator_t *rotational_pos,
- command_generator_t *rotational_speed,
- float wheel_radius, float struct_radius, float max_speed,
- uint8_t type);
-
-/*
- * Adjusts the current output value of 'generator' to 'value'.
- */
-command_generator_t* adjust_value(command_generator_t *generator, float value);
-
-/*
- * Adjusts the current slope of the ramp generator 'generator' to 'speed'.
- */
-command_generator_t* adjust_speed(command_generator_t *generator, float speed);
-
-/*
- * Starts or pauses 'generator'.
- */
-command_generator_t* start_generator(command_generator_t *generator);
-command_generator_t* pause_generator(command_generator_t *generator);
-
-/* Adds a callback to a generator
- * - generator : pointer to the generator to add a callback to
- * - type : threshold comparison type
- * - threshold : comparison threshold
- * - callback_function : callback to be called when the event is triggered
- */
-command_generator_t* add_callback(command_generator_t *generator, uint8_t type, float threshold, void (*callback_function)(command_generator_t*));
-
-/*
- * Removes the callback from 'generator'
- */
-command_generator_t* remove_callback(command_generator_t *generator);
-
-/*
- * Gets (and computes) the current output value of 'generator'
- */
-float get_output_value(command_generator_t *generator);
-
-#endif /* __COMMAND_GENERATOR_H */
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/controller_motor_stm32_user.mk b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/controller_motor_stm32_user.mk
index 56ea826..523bdbb 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/controller_motor_stm32_user.mk
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/controller_motor_stm32_user.mk
@@ -7,20 +7,24 @@
controller_motor_stm32_PROGRAMMER_TYPE = none
controller_motor_stm32_PROGRAMMER_PORT = none
+# Support board library
+controller_motor_stm32_LIB_PATH = $(controller_motor_stm32_SRC_PATH)/../../lib
+CFLAGS += -I$(controller_motor_stm32_LIB_PATH)
+
# Files included by the user.
controller_motor_stm32_USER_CSRC = \
- $(controller_motor_stm32_SRC_PATH)/stm32lib/stm32f10x_rcc.c \
- $(controller_motor_stm32_SRC_PATH)/stm32lib/stm32f10x_tim.c \
- $(controller_motor_stm32_SRC_PATH)/motor.c \
- $(controller_motor_stm32_SRC_PATH)/encoder.c \
- $(controller_motor_stm32_SRC_PATH)/odometry.c \
- $(controller_motor_stm32_SRC_PATH)/motor_controller.c \
- $(controller_motor_stm32_SRC_PATH)/command_generator.c \
- $(controller_motor_stm32_SRC_PATH)/trajectory_controller.c \
- $(controller_motor_stm32_SRC_PATH)/bezier_utils.c \
- $(controller_motor_stm32_SRC_PATH)/differential_drive.c \
+ $(controller_motor_stm32_LIB_PATH)/stm32lib/stm32f10x_rcc.c \
+ $(controller_motor_stm32_LIB_PATH)/stm32lib/stm32f10x_tim.c \
+ $(controller_motor_stm32_LIB_PATH)/motor.c \
+ $(controller_motor_stm32_LIB_PATH)/encoder.c \
+ $(controller_motor_stm32_LIB_PATH)/odometry.c \
+ $(controller_motor_stm32_LIB_PATH)/motor_controller.c \
+ $(controller_motor_stm32_LIB_PATH)/command_generator.c \
+ $(controller_motor_stm32_LIB_PATH)/trajectory_controller.c \
+ $(controller_motor_stm32_LIB_PATH)/bezier_utils.c \
+ $(controller_motor_stm32_LIB_PATH)/differential_drive.c \
+ $(controller_motor_stm32_LIB_PATH)/lift_controller.c \
$(controller_motor_stm32_SRC_PATH)/can_monitor.c \
- $(controller_motor_stm32_SRC_PATH)/lift_controller.c \
$(controller_motor_stm32_SRC_PATH)/main.c \
#
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/differential_drive.c b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/differential_drive.c
deleted file mode 100644
index b566a84..0000000
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/differential_drive.c
+++ /dev/null
@@ -1,373 +0,0 @@
-/*
- * differential_drive.c
- * --------------------
- * Copyright : (c) 2011, Xavier Lagorce <Xav...@cr...>
- * Licence : BSD3
- *
- * This file is a part of [kro]bot.
- */
-
-#include "differential_drive.h"
-
-#ifndef MIN
-#define MIN(a,b) ((a) < (b) ? (a) : (b))
-#endif
-
-PROC_DEFINE_STACK(stack_traj_following, KERN_MINSTACKSIZE * 32);
-
-typedef struct {
- uint8_t initialized, enabled;
- float params[2][4], dparams[2][4], ddparams[2][4];
- float du, v_tab[101];
- float goal[2], v_end, theta_end, v_lin;
- float start[2], theta_ini;
-} dd_bezier_traj_t;
-
-typedef struct {
- uint8_t initialized, enabled, running, working;
- float wheel_radius, shaft_width;
- float last_lin_acceleration, last_rot_acceleration;
- float u, v_max, at_max, ar_max;
- command_generator_t left_wheel_speed, right_wheel_speed;
- command_generator_t left_wheel, right_wheel;
- uint8_t current_traj;
- dd_bezier_traj_t trajs[2];
- float Ts, k1, k2, k3;
- robot_state_t ghost_state;
-} dd_params_t;
-
-static dd_params_t params;
-
-static void NORETURN traj_following_process(void);
-
-static void NORETURN traj_following_process(void) {
- Timer timer;
- uint8_t next_traj, ui;
- robot_state_t rs;
- float cr, v_ratio, v_max, v_rot, dxu, dyu;
- float z1, z2, z3, w1, w2, u1, u2, dt;
- dd_bezier_traj_t *traj;
- int32_t last_time, cur_time;
-
- // configure timer
- timer_setDelay(&timer, ms_to_ticks(params.Ts * 1000));
- timer_setEvent(&timer);
-
- // Indicate we are running
- params.running = 1;
-
- // Init
- params.working = 0;
- next_traj = (params.current_traj + 1) % 2;
-
- while (1) {
- if (params.enabled == 0) {
- params.running = 0;
- proc_exit();
- } else {
- if (!params.working && params.trajs[next_traj].initialized) {
- params.working = 1;
- params.u = 0;
- ui = 0;
- params.current_traj = next_traj;
- traj = ¶ms.trajs[params.current_traj];
- next_traj = (params.current_traj + 1) % 2;
- params.ghost_state.x = traj->start[0];
- params.ghost_state.y = traj->start[1];
- params.ghost_state.theta = traj->theta_ini;
- traj->enabled = 1;
- last_time = ticks_to_us(timer_clock());
- }
- timer_add(&timer);
- if (params.working) {
- odo_getState(&rs);
-
- // Stop following the trajectory if we are close enough to our goal
- if (params.u >= 1.0 || ((rs.x-traj->goal[0]) * (rs.x-traj->goal[0]) + (rs.y-traj->goal[1]) * (rs.y-traj->goal[1])) <= (0.01*0.01)) {
- params.working = 0;
- traj->initialized = 0;
- traj->enabled = 0;
- if (!params.trajs[next_traj].initialized) {
- // We have no other trajectory to follow, let's brake
- dd_set_rotational_speed(0., params.at_max);
- dd_set_linear_speed(0., params.at_max);
- }
- } else {
- // We are following a trajectory, let's do it
- // Compute ghost vehicule parameters
- cr = bezier_cr(params.u, traj->dparams, traj->ddparams);
- for (; ui*traj->du <= params.u; ui++);
- if (ui*traj->du > 1.0)
- ui--;
- if (ui > 0) {
- traj->v_lin = traj->v_tab[ui-1] +
- (traj->v_tab[ui]-traj->v_tab[ui-1]) * (params.u - traj->du*(ui-1))/traj->du;
- } else {
- traj->v_lin = traj->v_tab[0];
- }
- if (isnan(cr) || isinf(cr)) {
- v_ratio = 1.0;
- } else {
- v_ratio = (cr + params.shaft_width/2.0) / (cr - params.shaft_width/2.0);
- }
- v_max = 2/(1+MIN(fabsf(v_ratio), fabsf(1.0/v_ratio)))*traj->v_lin;
- if (cr >= 0) {
- v_rot = v_max * (1 - 1/v_ratio) / params.shaft_width;
- } else {
- v_rot = v_max * (v_ratio - 1) / params.shaft_width;
- }
-
- // Evolution of the ghost vehicule state
- cur_time = ticks_to_us(timer_clock());
- dt = (cur_time - last_time) * 1e-6;
- dxu = bezier_apply(traj->dparams[0], params.u);
- dyu = bezier_apply(traj->dparams[1], params.u);
- params.u += traj->v_lin/sqrtf(dxu*dxu+dyu*dyu)*dt;
- params.ghost_state.x += traj->v_lin*cosf(params.ghost_state.theta)*dt;
- params.ghost_state.y += traj->v_lin*sinf(params.ghost_state.theta)*dt;
- params.ghost_state.theta = fmodf(params.ghost_state.theta + v_rot*dt, 2*M_PI);
- if (params.ghost_state.theta > M_PI) {
- params.ghost_state.theta -= 2*M_PI;
- } else if (params.ghost_state.theta <= M_PI) {
- params.ghost_state.theta += 2*M_PI;
- }
-
- // Compute command
- z1=(rs.x-params.ghost_state.x)*cosf(params.ghost_state.theta)+(rs.y-params.ghost_state.y)*sinf(params.ghost_state.theta);
- z2=-(rs.x-params.ghost_state.x)*sinf(params.ghost_state.theta)+(rs.y-params.ghost_state.y)*cosf(params.ghost_state.theta);
- z3=tanf(rs.theta-params.ghost_state.theta);
-
- w1=-params.k1*fabsf(traj->v_lin)*(z1+z2*z3);
- w2=-params.k2*traj->v_lin*z2-params.k3*fabsf(traj->v_lin)*z3;
-
- u1=(w1+traj->v_lin)/cosf(rs.theta-params.ghost_state.theta);
- u2=w2*powf(cosf(rs.theta-params.ghost_state.theta),2)+v_rot;
-
- // Apply command
- dd_set_linear_speed(u1, 0.);
- dd_set_rotational_speed(u2, 0.);
-
- // Keep current time
- last_time = cur_time;
- }
- }
- }
- timer_waitEvent(&timer); // Wait for the remaining of the sample period
- }
-}
-
-void dd_start(float wheel_radius, float shaft_width, float max_wheel_speed,
- float v_max, float at_max, float ar_max,
- float k1, float k2, float k3, float Ts) {
- params.wheel_radius = wheel_radius;
- params.shaft_width = shaft_width;
- params.last_lin_acceleration = 0.0;
- params.last_rot_acceleration = 0.0;
-
- params.ghost_state.x = 0;
- params.ghost_state.y = 0;
- params.ghost_state.theta = 0;
-
- params.running = 0;
- params.working = 0;
- params.current_traj = 0;
- params.trajs[0].initialized = 0;
- params.trajs[1].initialized = 0;
- params.trajs[0].enabled = 0;
- params.trajs[1].enabled = 0;
- params.v_max = v_max;
- params.at_max = at_max;
- params.ar_max = ar_max;
-
- params.k1 = k1;
- params.k2 = k2;
- params.k3 = k3;
- params.Ts = Ts;
-
- tc_new_controller(DD_LINEAR_SPEED_TC);
- tc_new_controller(DD_ROTATIONAL_SPEED_TC);
- new_dd_generator(¶ms.left_wheel_speed,
- tc_get_position_generator(DD_LINEAR_SPEED_TC),
- tc_get_speed_generator(DD_LINEAR_SPEED_TC),
- tc_get_position_generator(DD_ROTATIONAL_SPEED_TC),
- tc_get_speed_generator(DD_ROTATIONAL_SPEED_TC),
- wheel_radius, shaft_width, max_wheel_speed,
- -1);
- new_dd_generator(¶ms.right_wheel_speed,
- tc_get_position_generator(DD_LINEAR_SPEED_TC),
- tc_get_speed_generator(DD_LINEAR_SPEED_TC),
- tc_get_position_generator(DD_ROTATIONAL_SPEED_TC),
- tc_get_speed_generator(DD_ROTATIONAL_SPEED_TC),
- wheel_radius, shaft_width, max_wheel_speed,
- 1);
- new_ramp2_generator(¶ms.left_wheel, 0.0, ¶ms.left_wheel_speed);
- new_ramp2_generator(¶ms.right_wheel, 0.0, ¶ms.right_wheel_speed);
-
- start_generator(¶ms.left_wheel_speed);
- start_generator(¶ms.right_wheel_speed);
- start_generator(¶ms.left_wheel);
- start_generator(¶ms.right_wheel);
-
- params.initialized = 1;
- params.enabled = 1;
-
- proc_new(traj_following_process, NULL, sizeof(stack_traj_following), stack_traj_following);
-}
-
-void dd_pause(void) {
- if (params.initialized) {
- dd_set_linear_speed(0.0, params.last_lin_acceleration);
- dd_set_rotational_speed(0.0, params.last_rot_acceleration);
- params.enabled = 0;
- }
-}
-
-void dd_resume(void) {
- if (params.initialized && params.enabled) {
- params.enabled = 1;
- }
-}
-
-void dd_stop(void) {
- if (params.initialized) {
- pause_generator(¶ms.left_wheel);
- pause_generator(¶ms.right_wheel);
- pause_generator(¶ms.left_wheel_speed);
- pause_generator(¶ms.right_wheel_speed);
- tc_delete_controller(DD_LINEAR_SPEED_TC);
- tc_delete_controller(DD_ROTATIONAL_SPEED_TC);
- params.enabled = 0;
- params.initialized = 0;
- }
-}
-
-command_generator_t* dd_get_left_wheel_generator(void) {
- return ¶ms.left_wheel;
-}
-
-command_generator_t* dd_get_right_wheel_generator(void) {
- return ¶ms.right_wheel;
-}
-
-void dd_move(float distance, float speed, float acceleration) {
- if (params.enabled) {
- params.last_lin_acceleration = acceleration;
- tc_move(DD_LINEAR_SPEED_TC, distance, speed, params.last_lin_acceleration);
- }
-}
-
-void dd_turn(float angle, float speed, float acceleration) {
- if (params.enabled) {
- params.last_rot_acceleration = acceleration;
- tc_move(DD_ROTATIONAL_SPEED_TC, angle, speed, params.last_rot_acceleration);
- }
-}
-
-void dd_set_linear_speed(float speed, float acceleration) {
- if (params.enabled) {
- if (acceleration != 0.) {
- params.last_lin_acceleration = acceleration;
- tc_goto_speed(DD_LINEAR_SPEED_TC, speed, params.last_lin_acceleration);
- } else {
- tc_set_speed(DD_LINEAR_SPEED_TC, speed);
- }
- }
-}
-
-void dd_set_rotational_speed(float speed, float acceleration) {
- if (params.enabled) {
- if (acceleration != 0.) {
- params.last_rot_acceleration = acceleration;
- tc_goto_speed(DD_ROTATIONAL_SPEED_TC, speed, params.last_rot_acceleration);
- } else {
- tc_set_speed(DD_ROTATIONAL_SPEED_TC, speed);
- }
- }
-}
-
-uint8_t dd_add_bezier(float x_end, float y_end, float d1, float d2, float end_angle, float end_speed) {
- uint8_t t_ind;
- robot_state_t rs;
- dd_bezier_traj_t *traj;
- float v_ini, x_ini, y_ini, theta_ini;
-
- t_ind = (params.current_traj + 1) % 2;
-
- if (params.trajs[t_ind].initialized == 1) {
- return DD_TRAJECTORY_ALREADY_USED;
- } else {
- traj = ¶ms.trajs[t_ind];
- // New trajectory is not enabled
- traj->enabled = 0;
-
- // Get starting parameters
- if (params.trajs[params.current_traj].enabled) {
- v_ini = params.trajs[params.current_traj].v_end;
- x_ini = params.trajs[params.current_traj].goal[0];
- y_ini = params.trajs[params.current_traj].goal[1];
- theta_ini = params.trajs[params.current_traj].theta_end;
- } else {
- odo_getState(&rs);
- x_ini = rs.x;
- y_ini = rs.y;
- theta_ini = rs.theta;
- v_ini = 0.01; // non null low velocity to allow the system to start
- }
-
- // Compute Bezier Spline parameters
- bezier_generate(traj->params,
- x_ini, y_ini,
- x_ini + d1*cosf(theta_ini), y_ini + d1*sinf(theta_ini),
- x_end - d2*cosf(end_angle), y_end - d2*sinf(end_angle),
- x_end, y_end);
- traj->goal[0] = x_end;
- traj->goal[1] = y_end;
- traj->v_end = end_speed;
- traj->theta_end = end_angle;
- traj->start[0] = x_ini;
- traj->start[1] = y_ini;
- traj->theta_ini = (d1 >= 0.0) ? theta_ini : (theta_ini + M_PI);
- // Differentiate parameters
- bezier_diff(traj->params, traj->dparams);
- bezier_diff(traj->dparams, traj->ddparams);
- // Compute velocity profile
- bezier_velocity_profile(traj->dparams, traj->ddparams,
- params.v_max, params.at_max, params.ar_max,
- v_ini, end_speed,
- 0.01, traj->v_tab);
- traj->du = 0.01;
-
- params.trajs[t_ind].initialized = 1;
- return DD_NO_ERROR;
- }
-}
-
-void dd_interrupt_trajectory(float rot_acc, float lin_acc) {
-
- // Prevent the controller from following the current trajectory
- params.working = 0;
-
- // Disable all the trajectories
- params.trajs[0].initialized = 0;
- params.trajs[0].enabled = 0;
- params.trajs[1].initialized = 0;
- params.trajs[1].enabled = 0;
- // Brake !
- dd_set_rotational_speed(0., rot_acc);
- dd_set_linear_speed(0., lin_acc);
-}
-
-uint8_t dd_get_ghost_state(robot_state_t *state, float *u) {
- if (state != NULL) {
- state->x = params.ghost_state.x;
- state->y = params.ghost_state.y;
- state->theta = params.ghost_state.theta;
- }
- if (u != NULL)
- *u = params.u;
-
- if (params.working == 1)
- return DD_GHOST_MOVING;
- else
- return DD_GHOST_STOPPED;
-}
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/differential_drive.h b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/differential_drive.h
deleted file mode 100644
index 0a0a0f9..0000000
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/differential_drive.h
+++ /dev/null
@@ -1,120 +0,0 @@
-/*
- * differential_drive.h
- * --------------------
- * Copyright : (c) 2011, Xavier Lagorce <Xav...@cr...>
- * Licence : BSD3
- *
- * This file is a part of [kro]bot.
- */
-
-#ifndef __DIFFERENTIAL_DRIVE_H
-#define __DIFFERENTIAL_DRIVE_H
-
-#include <math.h>
-#include <drv/timer.h>
-#include "trajectory_controller.h"
-#include "command_generator.h"
-#include "odometry.h"
-#include "bezier_utils.h"
-
-#ifndef DD_LINEAR_SPEED_TC
- #define DD_LINEAR_SPEED_TC 0
-#endif
-#ifndef DD_ROTATIONAL_SPEED_TC
- #define DD_ROTATIONAL_SPEED_TC 1
-#endif
-
-#define DD_NO_ERROR 0
-#define DD_TRAJECTORY_ALREADY_USED 1
-
-#define DD_GHOST_STOPPED 0
-#define DD_GHOST_MOVING 1
-
-/* Initializes the differential drive
- * - wheel_radius : radius of the wheels (in meters)
- * - shaft_width : propulsion shaft width (in meters)
- * - max_wheel_speed : maximum wheel speed (in rad/s)
- * - v_max : maximum linear speed (in m/s)
- * - at_max : maximum tangential acceleration (in m/s/s)
- * - ar_max : maximum radial acceleration (in m/s/s)
- * - k1, k2, k3 : control loop gain for trajectory following
- * - Ts : sample time for control loop in seconds
- *
- * Note : the differential drive system will use Trajectory controllers
- * DD_LINEAR_SPEED_TC and DD_ROTATIONAL_SPEED_TC
- */
-void dd_start(float wheel_radius, float shaft_width, float max_wheel_speed,
- float v_max, float at_max, float ar_max,
- float k1, float k2, float k3, float Ts);
-
-/* Pauses or Resumes the differential drive system.
- * In pause mode, the drive will accept no further command and actions will be
- * stopped.
- * If the robot is moving, wheels will be slowed down with the last used
- * acceleration (note that this won't necessarily give a straight line) when
- * pausing.
- */
-void dd_pause(void);
-void dd_resume(void);
-
-/*
- * Stops the differential drive system.
- */
-void dd_stop(void);
-
-/* Gets the generator correspondig to the wheels positions
- * to use them in motor_control for instance
- */
-command_generator_t* dd_get_left_wheel_generator(void);
-command_generator_t* dd_get_right_wheel_generator(void);
-
-/* Moves along a line
- * - distance : distance to move of (in meters), can be positive (forward)
- * or negative (backward).
- * - speed : moving speed (in meters per second) should be positive.
- * - acceleration : in meters per second per second, should be positive.
- */
-void dd_move(float distance, float speed, float acceleration);
-
-/* Turns around the propulsion shaft center
- * - angle : angle to turn of (in degrees), can be positive (CCW)
- * or negative (CW).
- * - speed : turning speed (in degrees per second) should be positive.
- * - acceleration : in degrees per second per second, should be positive.
- */
-void dd_turn(float angle, float speed, float acceleration);
-
-/*
- * Modify a given speed of the robot using the specified acceleration
- */
-void dd_set_linear_speed(float speed, float acceleration);
-void dd_set_rotational_speed(float speed, float acceleration);
-
-/*
- * Add a Bezier Spline to the trajectory follower
- */
-uint8_t dd_add_bezier(float x_end, float y_end, float d1, float d2, float end_angle, float end_speed);
-
-/*
- * Stops the current trajectory and removes the queued ones.
- * - rot_acc : acceleration to stop the rotational motion of the drive.
- * - lin_acc : acceleration to stop the linear motion of the drive.
- *
- * If one of the accelerations if zero, the corresponding movement will be stopped abruptly
- */
-void dd_interrupt_trajectory(float rot_acc, float lin_acc);
-
-/*
- * Return the current state of the followed ghost robot
- * - state : pointer to a robot_state_t structure where the ghost state will be written
- * - u : pointer to a float in which to write the current value of the parameter on the spline
- *
- * If one of these pointers is NULL, the associated value won't be written.
- *
- * return value :
- * - DD_GHOST_MOVING : if a trajectory is currently followed
- * - DD_GHOST_STOPPED : if the ghost robot is stopped
- */
-uint8_t dd_get_ghost_state(robot_state_t *state, float *u);
-
-#endif /* __DIFFERENTIAL_DRIVE_H */
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/encoder.c b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/encoder.c
deleted file mode 100644
index 6552dbb..0000000
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/encoder.c
+++ /dev/null
@@ -1,173 +0,0 @@
-/*
- * Wrapper to use the Encoder interface
- * Xavier Lagorce
- */
-
-#include "encoder.h"
-
-#include <cfg/macros.h>
-#include <drv/gpio_stm32.h>
-
-#include "stm32lib/stm32f10x.h"
-#include "stm32lib/stm32f10x_rcc.h"
-#include "stm32lib/stm32f10x_tim.h"
-
-
-/*
- * Function to initialise one encoder interface
- */
-void encodersInit(void) {
-
- TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
- TIM_ICInitTypeDef TIM_ICInitStructure;
-
- //Enable GPIOA and GPIOB clock
- RCC->APB2ENR |= RCC_APB2_GPIOA | RCC_APB2_GPIOB | RCC_APB2_GPIOC;
-
- //Enable timer clock
- RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);
- RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8, ENABLE);
- RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
- RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
-
- //Setup timer for quadrature encoder interface
- //Encoder1 A channel at PA6 (Ch1)
- // B channel at PA7 (Ch2) -> TIM3
- //Encoder2 A channel at PC6 (Ch1)
- // B channel at PC7 (Ch2) -> TIM8
- //Encoder3 A channel at PA8 (Ch1)
- // B channel at PA9 (Ch2) -> TIM1
- //Encoder4 A channel at PB6 (Ch1)
- // B channel at PB7 (Ch2) -> TIM4
- stm32_gpioPinConfig(((struct stm32_gpio *)GPIOA_BASE),
- BV(6) | BV(7) | BV(8) | BV(9),
- GPIO_MODE_IN_FLOATING, GPIO_SPEED_50MHZ);
- stm32_gpioPinConfig(((struct stm32_gpio *)GPIOB_BASE),
- BV(6) | BV(7),
- GPIO_MODE_IN_FLOATING, GPIO_SPEED_50MHZ);
- stm32_gpioPinConfig(((struct stm32_gpio *)GPIOC_BASE),
- BV(6) | BV(7),
- GPIO_MODE_IN_FLOATING, GPIO_SPEED_50MHZ);
-
- // TimeBase configuration
- TIM_TimeBaseStructure.TIM_Prescaler = 0;
- TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
- TIM_TimeBaseStructure.TIM_Period = 0xFFFF;
- TIM_TimeBaseStructure.TIM_ClockDivision = 0;
- TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
- TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);
- TIM_TimeBaseInit(TIM8, &TIM_TimeBaseStructure);
- TIM_TimeBaseInit(TIM1, &TIM_TimeBaseStructure);
- TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);
-
- //Initialize input capture structure: Ch1
- TIM_ICStructInit(&TIM_ICInitStructure);
- TIM_ICInitStructure.TIM_Channel = TIM_Channel_1;
- TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
- TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
- TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
- TIM_ICInitStructure.TIM_ICFilter = 0;
- TIM_ICInit(TIM3, &TIM_ICInitStructure);
- TIM_ICInit(TIM8, &TIM_ICInitStructure);
- TIM_ICInit(TIM1, &TIM_ICInitStructure);
- TIM_ICInit(TIM4, &TIM_ICInitStructure);
-
- //Initialize input capture structure: Ch2
- TIM_ICInitStructure.TIM_Channel = TIM_Channel_2;
- TIM_ICInit(TIM3, &TIM_ICInitStructure);
- TIM_ICInit(TIM8, &TIM_ICInitStructure);
- TIM_ICInit(TIM1, &TIM_ICInitStructure);
- TIM_ICInit(TIM4, &TIM_ICInitStructure);
-
- //Encoder Interface Configuration
- TIM_EncoderInterfaceConfig(TIM3,
- TIM_EncoderMode_TI12,
- TIM_ICPolarity_Rising,
- TIM_ICPolarity_Rising);
- TIM_EncoderInterfaceConfig(TIM8,
- TIM_EncoderMode_TI12,
- TIM_ICPolarity_Rising,
- TIM_ICPolarity_Rising);
- TIM_EncoderInterfaceConfig(TIM1,
- TIM_EncoderMode_TI12,
- TIM_ICPolarity_Rising,
- TIM_ICPolarity_Rising);
- TIM_EncoderInterfaceConfig(TIM4,
- TIM_EncoderMode_TI12,
- TIM_ICPolarity_Rising,
- TIM_ICPolarity_Rising);
- //Enable timer Peripherals
- TIM_Cmd(TIM3,ENABLE);
- TIM_Cmd(TIM8,ENABLE);
- TIM_Cmd(TIM1,ENABLE);
- TIM_Cmd(TIM4,ENABLE);
-}
-
-/*
- * Helper to get the current encoder position
- * encoder : get the value form this encoder
- */
-uint16_t getEncoderPosition(uint8_t encoder) {
- switch(encoder) {
- case ENCODER1:
- return TIM_GetCounter(TIM3);
- break;
- case ENCODER2:
- return TIM_GetCounter(TIM8);
- break;
- case ENCODER3:
- return TIM_GetCounter(TIM1);
- break;
- case ENCODER4:
- return TIM_GetCounter(TIM4);
- break;
- default:
- return 0;
- }
-}
-
-/*
- * Helper to get the current direction of evolution of the counter
- */
-uint8_t getEncoderDirection(uint8_t encoder) {
- switch(encoder) {
- case ENCODER1:
- return ((TIM3->CR1 & TIM_CR1_DIR) != 0 ? ENCODER_DIR_DOWN : ENCODER_DIR_UP);
- break;
- case ENCODER2:
- return ((TIM8->CR1 & TIM_CR1_DIR) != 0 ? ENCODER_DIR_DOWN : ENCODER_DIR_UP);
- break;
- case ENCODER3:
- return ((TIM1->CR1 & TIM_CR1_DIR) != 0 ? ENCODER_DIR_DOWN : ENCODER_DIR_UP);
- break;
- case ENCODER4:
- return ((TIM4->CR1 & TIM_CR1_DIR) != 0 ? ENCODER_DIR_DOWN : ENCODER_DIR_UP);
- break;
- default:
- return 0;
- }
-}
-
-/*
- * Helper to reset the current encoder position
- * encoder : reset the value from this encoder
- *
- */
-void resetEncoderPosition(uint8_t encoder) {
- switch(encoder) {
- case ENCODER1:
- TIM_SetCounter(TIM3, 0);
- break;
- case ENCODER2:
- TIM_SetCounter(TIM8, 0);
- break;
- case ENCODER3:
- TIM_SetCounter(TIM1, 0);
- break;
- case ENCODER4:
- TIM_SetCounter(TIM4, 0);
- break;
- default:
- break;
- }
-}
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/encoder.h b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/encoder.h
deleted file mode 100644
index e94ac88..0000000
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/encoder.h
+++ /dev/null
@@ -1,25 +0,0 @@
-/*
- * Wrapper to use the Encoder interface
- * Xavier Lagorce
- */
-
-#ifndef HEADER__ENCODER
-#define HEADER__ENCODER
-
-#define ENCODER1 1
-#define ENCODER2 2
-#define ENCODER3 3
-#define ENCODER4 4
-
-#define ENCODER_DIR_UP 0
-#define ENCODER_DIR_DOWN 1
-
-#include <drv/gpio_stm32.h>
-#include <drv/clock_stm32.h>
-
-void encodersInit(void);
-uint16_t getEncoderPosition(uint8_t encoder);
-void resetEncoderPosition(uint8_t encoder);
-uint8_t getEncoderDirection(uint8_t encoder);
-
-#endif
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/lift_controller.c b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/lift_controller.c
deleted file mode 100644
index 56cb686..0000000
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/lift_controller.c
+++ /dev/null
@@ -1,139 +0,0 @@
-/*
- * lift_controller.c
- * -----------------
- * Copyright : (c) 2011, Xavier Lagorce <Xav...@cr...>
- * Licence : BSD3
- *
- * This file is a part of [kro]bot.
- */
-
-#include "lift_controller.h"
-
-#define DIR_UP 0
-#define DIR_DOWN 1
-
-typedef struct {
- uint8_t enabled, tc_ind;
- int8_t direction;
- float bottom, extend;
- uint8_t end_stop_ind;
-} lc_state_t;
-
-lc_state_t lc_state[2];
-
-float pos2angle(uint8_t lift, float position);
-
-float pos2angle(uint8_t lift, float position) {
-
- //return lc_state[lift].offset - position / 23.474;
- return lc_state[lift].bottom + lc_state[lift].extend*position;
-}
-
-void lc_init(void) {
- motor_controller_params_t params;
- int i;
-
- // Init state
- for (i=0; i<2; i++) {
- lc_state[i].enabled = 1;
- lc_state[i].direction = -1;
- lc_state[i].bottom = 0;
- 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;
-
- // Init Trajectory controllers
- tc_new_controller(LC_TC_FRONT);
- tc_new_controller(LC_TC_BACK);
- // Limit PWM value
- motorSetMaxPWM(MOTOR3, 1600);
- motorSetMaxPWM(MOTOR4, 1600);
- // Common parameters
- params.encoder_gain = 2.0*M_PI/588.0;
- params.G0 = 0.0035;
- params.tau = 0.025;
- params.k[0] = -10216;
- params.k[1] = -255.39;
- params.l = -params.k[0];
- params.l0[0] = 0.0091;
- params.l0[1] = 1.6361;
- params.T = 0.005;
- // Initialize front 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);
-}
-
-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_stop(LC_TC_FRONT);
- lc_state[LC_FRONT_LIFT].end_stop_ind = end_stops & (LC_FRONT_UP|LC_FRONT_BOTTOM);
- }
- if ((end_stops & LC_BACK_UP) || (end_stops & LC_BACK_BOTTOM)) {
- if ((end_stops & LC_BACK_UP) && (lc_state[LC_BACK_LIFT].direction == 1))
- tc_stop(LC_TC_BACK);
- if ((end_stops & LC_BACK_BOTTOM) && (lc_state[LC_BACK_LIFT].direction == -1))
- tc_stop(LC_TC_BACK);
- lc_state[LC_BACK_LIFT].end_stop_ind = (end_stops & (LC_BACK_UP|LC_BACK_BOTTOM)) >> 2;
- }
-}
-
-void lc_homing(uint8_t lift) {
- // Go down
- lc_state[lift].direction = -1;
- tc_goto_speed(lc_state[lift].tc_ind, M_PI/4, 1);
- // Wait for the end stop to trigger
- while (!(lc_state[lift].end_stop_ind & LC_BOTTOM))
- cpu_relax();
- // Stop the lift
- tc_stop(lc_state[lift].tc_ind);
- // Assert end stop
- lc_state[lift].end_stop_ind &= ~LC_BOTTOM;
- // Wait and get current generator position as offset
- timer_delay(50);
- lc_state[lift].bottom = get_output_value(tc_get_position_generator(lc_state[lift].tc_ind));
-
- // Go up
- lc_state[lift].direction = 1;
- tc_goto_speed(lc_state[lift].tc_ind, -M_PI/4, 1);
- // Wait for the end stop to trigger
- while (!(lc_state[lift].end_stop_ind & LC_UP))
- cpu_relax();
- // Stop the lift
- tc_stop(lc_state[lift].tc_ind);
- // Assert end stop
- lc_state[lift].end_stop_ind &= ~LC_UP;
- // Wait and get current generator position as offset
- timer_delay(50);
- lc_state[lift].extend = get_output_value(tc_get_position_generator(lc_state[lift].tc_ind)) - lc_state[lift].bottom;
-}
-
-void lc_goto_position(uint8_t lift, float position) {
- float goal = pos2angle(lift, position);
-
- if (goal > get_output_value(tc_get_position_generator(lc_state[lift].tc_ind))) {
- lc_state[lift].direction = -1;
- } else {
- lc_state[lift].direction = 1;
- }
- tc_goto(lift, goal, M_PI, 5);
-}
-
-void lc_release(void) {
- mc_delete_controller(MOTOR3);
- mc_delete_controller(MOTOR4);
-
- lc_state[LC_FRONT_LIFT].enabled = 0;
- lc_state[LC_BACK_LIFT].enabled = 0;
-}
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/lift_controller.h b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/lift_controller.h
deleted file mode 100644
index 717c8cc..0000000
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/lift_controller.h
+++ /dev/null
@@ -1,41 +0,0 @@
-/*
- * lift_controller.h
- * -----------------
- * Copyright : (c) 2011, Xavier Lagorce <Xav...@cr...>
- * Licence : BSD3
- *
- * This file is a part of [kro]bot.
- */
-
-#ifndef __LIFT_CONTROLLER_H
-#define __LIFT_CONTROLLER_H
-
-#include "motor_controller.h"
-#include "trajectory_controller.h"
-#include "command_generator.h"
-
-#define LC_FRONT_LIFT 0
-#define LC_BACK_LIFT 1
-
-#define LC_FRONT_UP 1
-#define LC_FRONT_BOTTOM 2
-#define LC_BACK_UP 4
-#define LC_BACK_BOTTOM 8
-
-#define LC_UP 1
-#define LC_BOTTOM 2
-
-#define LC_POSITION_BOTTOM 1
-#define LC_POSITION_MIDDLE 2
-#define LC_POSITION_UP 4
-
-#define LC_TC_FRONT 0
-#define LC_TC_BACK 1
-
-void lc_init(void);
-void lc_end_stop_reached(uint8_t end_stops);
-void lc_homing(uint8_t lift);
-void lc_goto_position(uint8_t lift, float position);
-void lc_release(void);
-
-#endif /* __LIFT_CONTROLLER_H */
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/motor.c b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/motor.c
deleted file mode 100644
index f085184..0000000
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/motor.c
+++ /dev/null
@@ -1,381 +0,0 @@
-/*
- * Motor controller interface
- * This is a quick implementation based of the stm32lib from ST and from
- * previous implementations.
- *
- * This is supposed to be quick and dirty, waiting for BeRTOS proper
- * PWMs integration, to allow work on other systems using PWMs.
- *
- * Author : Xavier Lagorce
- */
-
-#include "motor.h"
-#include <cfg/macros.h>
-#include <drv/gpio_stm32.h>
-#include "stm32lib/stm32f10x_tim.h"
-
-uint8_t enabledMotors = 0, indMotors = 0;
-signed char currentSpeedSign[] = {0, 0, 0, 0};
-int32_t maxPWMs[] = {MAX_PWM, MAX_PWM, MAX_PWM, MAX_PWM};
-TIM_OCInitTypeDef TIM_OCInitStructure;
-
-static int32_t staSpeed(int32_t speed, int32_t maxSpeed, uint8_t *ind) {
- if (speed >= maxSpeed) {
- *ind = 1;
- return maxSpeed;
- } else if (speed <= -maxSpeed) {
- *ind = 1;
- return -maxSpeed;
- } else {
- *ind = 0;
- return speed;
- }
-}
-
-/*
- * Initialises TIM2 for PWM generation and associated GPIOs
- */
-void motorsInit(void) {
-
- TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
-
- //Enable GPIOA, GPIOB, GPIOC and GPIOD clock
- RCC->APB2ENR |= RCC_APB2_GPIOA | RCC_APB2_GPIOB | RCC_APB2_GPIOC | RCC_APB2_GPIOD;
-
- //Enable timer clock
- RCC->APB1ENR |= RCC_APB1_TIM2;
-
- //Setup timer for quadrature encoder interface
- //Motor1 : EN PA5
- // INA PC4
- // INB PC5
- // PWM PA0
- // IND LED1
- //Motor2 : EN PB15
- // INA PB1
- // INB PB14
- // PWM PA1
- // IND LED2
- //Motor3 : EN PA10
- // INA PC10
- // INB PC11
- // PWM PA2
- // IND LED3
- //Motor4 : EN PD2
- // INA PB5
- // INB PD9
- // PWM PA3
- // IND LED4
- stm32_gpioPinConfig(((struct stm32_gpio *)GPIOA_BASE),
- BV(5) | BV(10),
- GPIO_MODE_OUT_PP, GPIO_SPEED_50MHZ);
- stm32_gpioPinConfig(((struct stm32_gpio *)GPIOA_BASE),
- BV(0) | BV(1) | BV(2) | BV(3),
- GPIO_MODE_AF_...
[truncated message content] |
