high level strat
[aversive.git] / projects / microb2010 / cobboard / spickle.c
1 /*
2  *  Copyright Droids Corporation (2009)
3  *
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.
8  *
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.
13  *
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
17  *
18  *  Revision : $Id: actuator.c,v 1.4 2009-04-24 19:30:41 zer0 Exp $
19  *
20  */
21
22 #include <aversive.h>
23 #include <aversive/pgmspace.h>
24 #include <aversive/wait.h>
25 #include <aversive/error.h>
26
27 #include <ax12.h>
28 #include <uart.h>
29 #include <spi.h>
30 #include <encoders_spi.h>
31 #include <pwm_ng.h>
32 #include <timer.h>
33 #include <scheduler.h>
34 #include <clock_time.h>
35
36 #include <pid.h>
37 #include <quadramp.h>
38 #include <control_system_manager.h>
39 #include <blocking_detection_manager.h>
40
41 #include <rdline.h>
42
43 #include "sensor.h"
44 #include "../common/i2c_commands.h"
45 #include "main.h"
46 #include "actuator.h"
47
48 struct spickle_params {
49         /* current limit (common to left and right) */
50         int32_t k1;
51         int32_t k2;
52
53         /* cs blocks */
54         struct cs_block * const csb[2];
55
56         /* params */
57         int32_t pos_deployed[2];
58         int32_t pos_mid[2];
59         int32_t pos_packed[2];
60 };
61
62 static struct spickle_params spickle = {
63         .k1 = 1000,
64         .k2 = 20,
65         .csb = {
66                 &cobboard.left_spickle,
67                 &cobboard.right_spickle,
68         },
69         .pos_deployed = {
70                 7000, // 200, /* left */
71                 7000, // 200, /* right */
72         },
73         .pos_mid = {
74                 25000, /* left */
75                 26000, /* right */
76         },
77         .pos_packed = {
78                 55800, /* left */
79                 55800, /* right */
80         },
81 };
82
83 /* init spickle position at beginning */
84 static void spickle_autopos(void)
85 {
86         printf_P(PSTR("spickle autopos..."));
87         pwm_ng_set(LEFT_SPICKLE_PWM, -700);
88         pwm_ng_set(RIGHT_SPICKLE_PWM, -700);
89         wait_ms(2500);
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"));
95 }
96
97 /* Set CS command for spickle. Called by CS manager. */
98 void spickle_set(void *mot, int32_t cmd)
99 {
100         static int32_t oldpos_left, oldpos_right;
101         int32_t oldpos, pos, maxcmd, speed;
102
103         if (mot == LEFT_SPICKLE_PWM) {
104                 pos = encoders_spi_get_value(LEFT_SPICKLE_ENCODER);
105                 oldpos = oldpos_left;
106         }
107         else {
108                 pos = encoders_spi_get_value(RIGHT_SPICKLE_ENCODER);
109                 oldpos = oldpos_right;
110         }
111
112         speed = pos - oldpos;
113         if (speed > 0 && cmd < 0)
114                 maxcmd = spickle.k1;
115         else if (speed < 0 && cmd > 0)
116                 maxcmd = spickle.k1;
117         else {
118                 speed = ABS(speed);
119                 maxcmd = spickle.k1 + spickle.k2 * speed;
120         }
121         if (cmd > maxcmd)
122                 cmd = maxcmd;
123         else if (cmd < -maxcmd)
124                 cmd = -maxcmd;
125
126         pwm_ng_set(mot, cmd);
127
128         if (mot == LEFT_SPICKLE_PWM)
129                 oldpos_left = pos;
130         else
131                 oldpos_right = pos;
132 }
133
134 void spickle_set_coefs(uint32_t k1, uint32_t k2)
135 {
136         spickle.k1 = k1;
137         spickle.k2 = k2;
138 }
139
140
141 void spickle_set_pos(uint8_t side, int32_t pos_packed,
142                      int32_t pos_mid, int32_t pos_deployed)
143 {
144         spickle.pos_deployed[side] = pos_deployed;
145         spickle.pos_mid[side] = pos_mid;
146         spickle.pos_packed[side] = pos_packed;
147 }
148
149 void spickle_dump_params(void)
150 {
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]);
160 }
161
162 void spickle_deploy(uint8_t side)
163 {
164         cs_set_consign(&spickle.csb[side]->cs, spickle.pos_deployed[side]);
165 }
166
167 void spickle_mid(uint8_t side)
168 {
169         cs_set_consign(&spickle.csb[side]->cs, spickle.pos_mid[side]);
170 }
171
172 void spickle_pack(uint8_t side)
173 {
174         cs_set_consign(&spickle.csb[side]->cs, spickle.pos_packed[side]);
175 }
176
177 void spickle_init(void)
178 {
179         spickle_autopos();
180         cobboard.left_spickle.on = 1;
181         cobboard.right_spickle.on = 1;
182 }