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"
49 struct spickle_params {
50 /* current limit (common to left and right) */
57 struct cs_block * const csb[2];
60 int32_t pos_deployed[2];
62 int32_t pos_packed[2];
65 static struct spickle_params spickle = {
71 &cobboard.left_spickle,
72 &cobboard.right_spickle,
75 7000, // 200, /* left */
76 7000, // 200, /* right */
88 /* init spickle position at beginning */
89 static void spickle_autopos(void)
91 printf_P(PSTR("spickle autopos..."));
92 pwm_ng_set(LEFT_SPICKLE_PWM, -700);
93 pwm_ng_set(RIGHT_SPICKLE_PWM, -700);
95 pwm_ng_set(LEFT_SPICKLE_PWM, 0);
96 pwm_ng_set(RIGHT_SPICKLE_PWM, 0);
97 encoders_spi_set_value(LEFT_SPICKLE_ENCODER, 0);
98 encoders_spi_set_value(RIGHT_SPICKLE_ENCODER, 0);
99 printf_P(PSTR("ok\r\n"));
102 /* Set CS command for spickle. Called by CS manager. */
103 void spickle_set(void *mot, int32_t cmd)
105 static int32_t oldpos_left, oldpos_right;
106 int32_t oldpos, pos, maxcmd, speed;
109 if (mot == LEFT_SPICKLE_PWM) {
110 pos = encoders_spi_get_value(LEFT_SPICKLE_ENCODER);
111 oldpos = oldpos_left;
112 if (state_spicklemode_weak(I2C_LEFT_SIDE)) {
122 pos = encoders_spi_get_value(RIGHT_SPICKLE_ENCODER);
123 oldpos = oldpos_right;
124 if (state_spicklemode_weak(I2C_RIGHT_SIDE)) {
134 speed = pos - oldpos;
135 if (speed > 0 && cmd < 0)
137 else if (speed < 0 && cmd > 0)
141 maxcmd = k1 + k2 * speed;
145 else if (cmd < -maxcmd)
148 pwm_ng_set(mot, cmd);
150 if (mot == LEFT_SPICKLE_PWM)
156 void spickle_set_wcoefs(uint32_t k1, uint32_t k2)
162 void spickle_set_scoefs(uint32_t k1, uint32_t k2)
168 void spickle_set_pos(uint8_t side, int32_t pos_packed,
169 int32_t pos_mid, int32_t pos_deployed)
171 spickle.pos_deployed[side] = pos_deployed;
172 spickle.pos_mid[side] = pos_mid;
173 spickle.pos_packed[side] = pos_packed;
176 void spickle_dump_params(void)
178 printf_P(PSTR("strong_coef %ld %ld\r\n"), spickle.sk1, spickle.sk2);
179 printf_P(PSTR("weak_coef %ld %ld\r\n"), spickle.wk1, spickle.wk2);
180 printf_P(PSTR("left pos %ld %ld\r\n"),
181 spickle.pos_packed[I2C_LEFT_SIDE],
182 spickle.pos_mid[I2C_LEFT_SIDE],
183 spickle.pos_deployed[I2C_LEFT_SIDE]);
184 printf_P(PSTR("right pos %ld %ld\r\n"),
185 spickle.pos_packed[I2C_RIGHT_SIDE],
186 spickle.pos_mid[I2C_RIGHT_SIDE],
187 spickle.pos_deployed[I2C_RIGHT_SIDE]);
190 static uint8_t spickle_is_at_pos(uint8_t side, int32_t pos)
194 if (side == I2C_LEFT_SIDE)
195 enc = encoders_spi_get_value(LEFT_SPICKLE_ENCODER);
197 enc = encoders_spi_get_value(RIGHT_SPICKLE_ENCODER);
206 static void spickle_set_qr(uint8_t side)
208 struct quadramp_filter *qr;
210 if (side == I2C_LEFT_SIDE)
211 qr = &cobboard.left_spickle.qr;
213 qr = &cobboard.right_spickle.qr;
215 if (state_spicklemode_weak(side))
216 quadramp_set_1st_order_vars(qr, 700, 700); /* set speed */
218 quadramp_set_1st_order_vars(qr, 3000, 3000); /* set speed */
221 uint8_t spickle_is_packed(uint8_t side)
223 return spickle_is_at_pos(side, spickle.pos_packed[side]);
226 uint8_t spickle_is_deployed(uint8_t side)
228 return spickle_is_at_pos(side, spickle.pos_deployed[side]);
231 void spickle_deploy(uint8_t side)
233 spickle_set_qr(side);
234 cs_set_consign(&spickle.csb[side]->cs, spickle.pos_deployed[side]);
237 void spickle_mid(uint8_t side)
239 spickle_set_qr(side);
240 cs_set_consign(&spickle.csb[side]->cs, spickle.pos_mid[side]);
243 void spickle_pack(uint8_t side)
245 spickle_set_qr(side);
246 cs_set_consign(&spickle.csb[side]->cs, spickle.pos_packed[side]);
249 void spickle_init(void)
252 cobboard.left_spickle.on = 1;
253 cobboard.right_spickle.on = 1;