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.3 2009-05-02 10:08:09 zer0 Exp $
23 #include <aversive/pgmspace.h>
24 #include <aversive/wait.h>
25 #include <aversive/error.h>
28 #include <encoders_microb.h>
31 #include <scheduler.h>
32 #include <clock_time.h>
36 #include <control_system_manager.h>
37 #include <trajectory_manager.h>
38 #include <blocking_detection_manager.h>
39 #include <robot_system.h>
40 #include <position_manager.h>
52 static volatile uint8_t fessor_state = OFF;
53 static volatile uint32_t fessor_pos_up = 35000;
54 static volatile uint32_t fessor_pos_down = 0;
55 static volatile uint32_t fessor_delay_up = 500;
56 static volatile uint32_t fessor_delay_down = 2000;
57 static volatile uint32_t delay = 0;
58 static volatile int32_t fessor_k1 = 500, fessor_k2 = 20;
59 static volatile int32_t fessor_cmd = 0;
62 static volatile uint8_t elevator_state = OFF;
63 static volatile uint32_t elevator_pos_up = -8000;
64 static volatile uint32_t elevator_pos_down = 0;
65 static volatile uint32_t elevator_delay_up = 500;
66 static volatile uint32_t elevator_delay_down = 2000;
67 //static volatile uint32_t delay = 0;
68 static volatile int32_t elevator_k1 = 500, elevator_k2 = 20;
69 static volatile int32_t elevator_cmd = 0;
71 void pwm_set_and_save(void *pwm, int32_t val)
73 /* we need to do the saturation here, before saving the
81 mainboard.pwm_l = val;
82 else if (pwm == RIGHT_PWM)
83 mainboard.pwm_r = val;
87 void pickup_wheels_on(void)
89 mainboard.enable_pickup_wheels = 1;
92 void pickup_wheels_off(void)
94 mainboard.enable_pickup_wheels = 0;
97 /* init fessor position at beginning */
98 void fessor_autopos(void)
100 pwm_ng_set(FESSOR_PWM, -500);
102 pwm_ng_set(FESSOR_PWM, 0);
105 /* set CS command for fessor */
106 void fessor_set(void *dummy, int32_t cmd)
111 void fessor_set_coefs(uint32_t k1, uint32_t k2)
117 void fessor_set_delays(uint32_t delay_up, uint32_t delay_down)
119 fessor_delay_up = delay_up;
120 fessor_delay_down = delay_down;
123 void fessor_set_pos(uint32_t pos_up, uint32_t pos_down)
125 fessor_pos_up = pos_up;
126 fessor_pos_down = pos_down;
129 void fessor_dump_params(void)
131 printf_P(PSTR("coef %ld %ld\r\n"), fessor_k1, fessor_k2);
132 printf_P(PSTR("pos %ld %ld\r\n"), fessor_pos_up, fessor_pos_down);
133 printf_P(PSTR("delay %ld %ld\r\n"), fessor_delay_up, fessor_delay_down);
136 /* called by CS perdiodically (current limit) */
137 void fessor_manage(void)
139 static int32_t oldpos = 0;
140 int32_t pos, maxcmd, speed, cmd;
143 pos = encoders_microb_get_value(FESSOR_ENC);
144 speed = pos - oldpos;
145 if (speed > 0 && cmd < 0)
147 else if (speed < 0 && cmd > 0)
151 maxcmd = fessor_k1 + fessor_k2 * speed;
155 else if (cmd < -maxcmd)
158 pwm_ng_set(FESSOR_PWM, cmd);
166 cs_set_consign(&mainboard.fessor.cs, fessor_pos_up);
169 void fessor_down(void)
172 cs_set_consign(&mainboard.fessor.cs, fessor_pos_down);
175 void fessor_stop(void)
180 void fessor_auto(void)
182 fessor_state = WAIT_SENSOR;
183 cs_set_consign(&mainboard.fessor.cs, fessor_pos_up);
186 /* for fessor auto mode */
187 static void fessor_cb(void *dummy)
189 static uint8_t prev = 0;
192 val = !sensor_get(0);
194 switch (fessor_state) {
199 delay = fessor_delay_up;
200 fessor_state = SENSOR_OK;
205 cs_set_consign(&mainboard.fessor.cs, fessor_pos_down);
206 fessor_state = WAIT_DOWN;
207 delay = fessor_delay_down;
212 cs_set_consign(&mainboard.fessor.cs, fessor_pos_up);
213 fessor_state = WAIT_SENSOR;
222 void fessor_init(void)
224 scheduler_add_periodical_event_priority(fessor_cb, NULL,
225 1000L / SCHEDULER_UNIT,
236 /* init elevator position at beginning */
237 void elevator_autopos(void)
239 pwm_ng_set(ELEVATOR_PWM, 300);
241 pwm_ng_set(ELEVATOR_PWM, 0);
244 /* set CS command for elevator */
245 void elevator_set(void *dummy, int32_t cmd)
250 void elevator_set_coefs(uint32_t k1, uint32_t k2)
256 void elevator_set_delays(uint32_t delay_up, uint32_t delay_down)
258 elevator_delay_up = delay_up;
259 elevator_delay_down = delay_down;
262 void elevator_set_pos(uint32_t pos_up, uint32_t pos_down)
264 elevator_pos_up = pos_up;
265 elevator_pos_down = pos_down;
268 void elevator_dump_params(void)
270 printf_P(PSTR("coef %ld %ld\r\n"), elevator_k1, elevator_k2);
271 printf_P(PSTR("pos %ld %ld\r\n"), elevator_pos_up, elevator_pos_down);
272 printf_P(PSTR("delay %ld %ld\r\n"), elevator_delay_up, elevator_delay_down);
275 /* called by CS perdiodically (current limit) */
276 void elevator_manage(void)
278 static int32_t oldpos = 0;
279 int32_t pos, maxcmd, speed, cmd;
282 pos = encoders_microb_get_value(ELEVATOR_ENC);
283 speed = pos - oldpos;
284 if (speed > 0 && cmd < 0)
285 maxcmd = elevator_k1;
286 else if (speed < 0 && cmd > 0)
287 maxcmd = elevator_k1;
290 maxcmd = elevator_k1 + elevator_k2 * speed;
294 else if (cmd < -maxcmd)
297 pwm_ng_set(ELEVATOR_PWM, cmd);
302 void elevator_up(void)
305 cs_set_consign(&mainboard.elevator.cs, elevator_pos_up);
308 void elevator_down(void)
311 cs_set_consign(&mainboard.elevator.cs, elevator_pos_down);
314 void elevator_stop(void)
319 void elevator_auto(void)
321 elevator_state = WAIT_SENSOR;
322 cs_set_consign(&mainboard.elevator.cs, elevator_pos_down);
325 /* for elevator auto mode */
326 static void elevator_cb(void *dummy)
328 static uint8_t prev = 0;
331 val = !sensor_get(0);
333 switch (elevator_state) {
338 delay = elevator_delay_up;
339 elevator_state = SENSOR_OK;
344 cs_set_consign(&mainboard.elevator.cs, elevator_pos_up);
345 elevator_state = WAIT_DOWN;
346 delay = elevator_delay_down;
351 cs_set_consign(&mainboard.elevator.cs, elevator_pos_down);
352 elevator_state = WAIT_SENSOR;
361 void elevator_init(void)
363 scheduler_add_periodical_event_priority(elevator_cb, NULL,
364 1000L / SCHEDULER_UNIT,