2 * Copyright Droids Corporation (2009)
4 * This program is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation; either version 2 of the License, or
7 * (at your option) any later version.
9 * This program is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
14 * You should have received a copy of the GNU General Public License
15 * along with this program; if not, write to the Free Software
16 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
18 * Revision : $Id: actuator.c,v 1.4 2009-04-24 19:30:41 zer0 Exp $
23 #include <aversive/pgmspace.h>
24 #include <aversive/wait.h>
25 #include <aversive/error.h>
30 #include <encoders_spi.h>
33 #include <scheduler.h>
34 #include <clock_time.h>
38 #include <control_system_manager.h>
39 #include <blocking_detection_manager.h>
44 #include "../common/i2c_commands.h"
48 struct spickle_params {
49 /* current limit (common to left and right) */
54 struct cs_block * const csb[2];
57 int32_t pos_deployed[2];
59 int32_t pos_packed[2];
62 static struct spickle_params spickle = {
66 &cobboard.left_spickle,
67 &cobboard.right_spickle,
70 7000, // 200, /* left */
71 7000, // 200, /* right */
83 /* init spickle position at beginning */
84 static void spickle_autopos(void)
86 printf_P(PSTR("spickle autopos..."));
87 pwm_ng_set(LEFT_SPICKLE_PWM, -700);
88 pwm_ng_set(RIGHT_SPICKLE_PWM, -700);
90 pwm_ng_set(LEFT_SPICKLE_PWM, 0);
91 pwm_ng_set(RIGHT_SPICKLE_PWM, 0);
92 encoders_spi_set_value(LEFT_SPICKLE_ENCODER, 0);
93 encoders_spi_set_value(RIGHT_SPICKLE_ENCODER, 0);
94 printf_P(PSTR("ok\r\n"));
97 /* Set CS command for spickle. Called by CS manager. */
98 void spickle_set(void *mot, int32_t cmd)
100 static int32_t oldpos_left, oldpos_right;
101 int32_t oldpos, pos, maxcmd, speed;
103 if (mot == LEFT_SPICKLE_PWM) {
104 pos = encoders_spi_get_value(LEFT_SPICKLE_ENCODER);
105 oldpos = oldpos_left;
108 pos = encoders_spi_get_value(RIGHT_SPICKLE_ENCODER);
109 oldpos = oldpos_right;
112 speed = pos - oldpos;
113 if (speed > 0 && cmd < 0)
115 else if (speed < 0 && cmd > 0)
119 maxcmd = spickle.k1 + spickle.k2 * speed;
123 else if (cmd < -maxcmd)
126 pwm_ng_set(mot, cmd);
128 if (mot == LEFT_SPICKLE_PWM)
134 void spickle_set_coefs(uint32_t k1, uint32_t k2)
141 void spickle_set_pos(uint8_t side, int32_t pos_packed,
142 int32_t pos_mid, int32_t pos_deployed)
144 spickle.pos_deployed[side] = pos_deployed;
145 spickle.pos_mid[side] = pos_mid;
146 spickle.pos_packed[side] = pos_packed;
149 void spickle_dump_params(void)
151 printf_P(PSTR("coef %ld %ld\r\n"), spickle.k1, spickle.k2);
152 printf_P(PSTR("left pos %ld %ld\r\n"),
153 spickle.pos_packed[I2C_LEFT_SIDE],
154 spickle.pos_mid[I2C_LEFT_SIDE],
155 spickle.pos_deployed[I2C_LEFT_SIDE]);
156 printf_P(PSTR("right pos %ld %ld\r\n"),
157 spickle.pos_packed[I2C_RIGHT_SIDE],
158 spickle.pos_mid[I2C_RIGHT_SIDE],
159 spickle.pos_deployed[I2C_RIGHT_SIDE]);
162 void spickle_deploy(uint8_t side)
164 cs_set_consign(&spickle.csb[side]->cs, spickle.pos_deployed[side]);
167 void spickle_mid(uint8_t side)
169 cs_set_consign(&spickle.csb[side]->cs, spickle.pos_mid[side]);
172 void spickle_pack(uint8_t side)
174 cs_set_consign(&spickle.csb[side]->cs, spickle.pos_packed[side]);
177 void spickle_init(void)
180 cobboard.left_spickle.on = 1;
181 cobboard.right_spickle.on = 1;