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 int16_t delay_deployed[2];
58 int16_t delay_packed[2];
59 int32_t pos_deployed[2];
60 int32_t pos_packed[2];
63 static struct spickle_params spickle = {
67 &cobboard.left_spickle,
68 &cobboard.right_spickle,
88 /* init spickle position at beginning */
89 static void spickle_autopos(void)
91 pwm_ng_set(LEFT_SPICKLE_PWM, -500);
92 //pwm_ng_set(RIGHT_SPICKLE_PWM, -500);
94 pwm_ng_set(LEFT_SPICKLE_PWM, 0);
95 pwm_ng_set(RIGHT_SPICKLE_PWM, 0);
96 encoders_spi_set_value(LEFT_SPICKLE_ENCODER, 0);
97 encoders_spi_set_value(RIGHT_SPICKLE_ENCODER, 0);
100 /* Set CS command for spickle. Called by CS manager. */
101 void spickle_set(void *mot, int32_t cmd)
103 static int32_t oldpos_left, oldpos_right;
104 int32_t oldpos, pos, maxcmd, speed;
106 if (mot == LEFT_SPICKLE_PWM) {
107 pos = encoders_spi_get_value(LEFT_SPICKLE_ENCODER);
108 oldpos = oldpos_left;
111 pos = encoders_spi_get_value(RIGHT_SPICKLE_ENCODER);
112 oldpos = oldpos_right;
115 speed = pos - oldpos;
116 if (speed > 0 && cmd < 0)
118 else if (speed < 0 && cmd > 0)
122 maxcmd = spickle.k1 + spickle.k2 * speed;
126 else if (cmd < -maxcmd)
129 pwm_ng_set(mot, cmd);
131 if (mot == LEFT_SPICKLE_PWM)
137 void spickle_set_coefs(uint32_t k1, uint32_t k2)
143 void spickle_set_pos(uint8_t side, uint32_t pos_deployed, uint32_t pos_packed)
145 spickle.pos_deployed[side] = pos_deployed;
146 spickle.pos_packed[side] = pos_packed;
149 void spickle_set_delay(uint8_t side, uint32_t delay_deployed, uint32_t delay_packed)
151 spickle.delay_deployed[side] = delay_deployed;
152 spickle.delay_packed[side] = delay_packed;
155 void spickle_dump_params(void)
157 printf_P(PSTR("coef %ld %ld\r\n"), spickle.k1, spickle.k2);
158 printf_P(PSTR("left pos %ld %ld\r\n"),
159 spickle.pos_deployed[I2C_LEFT_SIDE],
160 spickle.pos_packed[I2C_LEFT_SIDE]);
161 printf_P(PSTR("left delay %ld %ld\r\n"),
162 spickle.delay_deployed[I2C_LEFT_SIDE],
163 spickle.delay_packed[I2C_LEFT_SIDE]);
164 printf_P(PSTR("right pos %ld %ld\r\n"),
165 spickle.pos_deployed[I2C_RIGHT_SIDE],
166 spickle.pos_packed[I2C_RIGHT_SIDE]);
167 printf_P(PSTR("right delay %ld %ld\r\n"),
168 spickle.delay_deployed[I2C_RIGHT_SIDE],
169 spickle.delay_packed[I2C_RIGHT_SIDE]);
172 void spickle_deploy(uint8_t side)
174 cs_set_consign(&spickle.csb[side]->cs, spickle.pos_deployed[side]);
177 void spickle_pack(uint8_t side)
179 cs_set_consign(&spickle.csb[side]->cs, spickle.pos_deployed[side]);
182 uint16_t spickle_get_deploy_delay(uint8_t side)
184 return spickle.delay_deployed[side];
187 uint16_t spickle_get_pack_delay(uint8_t side)
189 return spickle.delay_packed[side];
192 void spickle_init(void)
195 cobboard.left_spickle.on = 1;
196 //cobboard.right_spickle.on = 1;