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 "state.h"
  46 #include "main.h"
  47 #include "actuator.h"
  48
  49 struct spickle_params {
  50         /* current limit (common to left and right) */
  51         int32_t sk1;
  52         int32_t sk2;
  53         int32_t wk1;
  54         int32_t wk2;
  55
  56         /* cs blocks */
  57         struct cs_block * const csb[2];
  58
  59         /* params */
  60         int32_t pos_deployed[2];
  61         int32_t pos_mid[2];
  62         int32_t pos_packed[2];
  63 };
  64
  65 static struct spickle_params spickle = {
  66         .sk1 = 1000,
  67         .sk2 = 20,
  68         .wk1 = 200,
  69         .wk2 = 5,
  70         .csb = {
  71                 &cobboard.left_spickle,
  72                 &cobboard.right_spickle,
  73         },
  74         .pos_deployed = {
  75                 500,// 7000, // 200, /* left */
  76                 500,// 7000, // 200, /* right */
  77         },
  78         .pos_mid = {
  79                 25000, /* left */
  80                 26000, /* right */
  81         },
  82         .pos_packed = {
  83                 55800, /* left */
  84                 55800, /* right */
  85         },
  86 };
  87
  88 /* init spickle position at beginning */
  89 static void spickle_autopos(void)
  90 {
  91         printf_P(PSTR("spickle autopos..."));
  92         pwm_ng_set(LEFT_SPICKLE_PWM, -700);
  93         pwm_ng_set(RIGHT_SPICKLE_PWM, -700);
  94         wait_ms(2500);
  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"));
 100 }
 101
 102 /* Set CS command for spickle. Called by CS manager. */
 103 void spickle_set(void *mot, int32_t cmd)
 104 {
 105         static int32_t oldpos_left, oldpos_right;
 106         int32_t oldpos, pos, maxcmd, speed;
 107         int32_t k1, k2;
 108
 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)) {
 113                         k1 = spickle.wk1;
 114                         k2 = spickle.wk2;
 115                 }
 116                 else {
 117                         k1 = spickle.sk1;
 118                         k2 = spickle.sk2;
 119                 }
 120         }
 121         else {
 122                 pos = encoders_spi_get_value(RIGHT_SPICKLE_ENCODER);
 123                 oldpos = oldpos_right;
 124                 if (state_spicklemode_weak(I2C_RIGHT_SIDE)) {
 125                         k1 = spickle.wk1;
 126                         k2 = spickle.wk2;
 127                 }
 128                 else {
 129                         k1 = spickle.sk1;
 130                         k2 = spickle.sk2;
 131                 }
 132         }
 133
 134         speed = pos - oldpos;
 135         if (speed > 0 && cmd < 0)
 136                 maxcmd = k1;
 137         else if (speed < 0 && cmd > 0)
 138                 maxcmd = k1;
 139         else {
 140                 speed = ABS(speed);
 141                 maxcmd = k1 + k2 * speed;
 142         }
 143         if (cmd > maxcmd)
 144                 cmd = maxcmd;
 145         else if (cmd < -maxcmd)
 146                 cmd = -maxcmd;
 147
 148         pwm_ng_set(mot, cmd);
 149
 150         if (mot == LEFT_SPICKLE_PWM)
 151                 oldpos_left = pos;
 152         else
 153                 oldpos_right = pos;
 154 }
 155
 156 void spickle_set_wcoefs(uint32_t k1, uint32_t k2)
 157 {
 158         spickle.wk1 = k1;
 159         spickle.wk2 = k2;
 160 }
 161
 162 void spickle_set_scoefs(uint32_t k1, uint32_t k2)
 163 {
 164         spickle.sk1 = k1;
 165         spickle.sk2 = k2;
 166 }
 167
 168 void spickle_set_pos(uint8_t side, int32_t pos_packed,
 169                      int32_t pos_mid, int32_t pos_deployed)
 170 {
 171         spickle.pos_deployed[side] = pos_deployed;
 172         spickle.pos_mid[side] = pos_mid;
 173         spickle.pos_packed[side] = pos_packed;
 174 }
 175
 176 void spickle_dump_params(void)
 177 {
 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]);
 188 }
 189
 190 static uint8_t spickle_is_at_pos(uint8_t side, int32_t pos)
 191 {
 192         int32_t diff;
 193         int32_t enc;
 194         if (side == I2C_LEFT_SIDE)
 195                 enc = encoders_spi_get_value(LEFT_SPICKLE_ENCODER);
 196         else
 197                 enc = encoders_spi_get_value(RIGHT_SPICKLE_ENCODER);
 198         diff = pos - enc;
 199         if (diff < 0)
 200                 diff = -diff;
 201         if (diff < 500)
 202                 return 1;
 203         return 0;
 204 }
 205
 206 static void spickle_set_qr(uint8_t side)
 207 {
 208         struct quadramp_filter *qr;
 209
 210         if (side == I2C_LEFT_SIDE)
 211                 qr = &cobboard.left_spickle.qr;
 212         else
 213                 qr = &cobboard.right_spickle.qr;
 214
 215         if (state_spicklemode_weak(side))
 216                 quadramp_set_1st_order_vars(qr, 700, 700); /* set speed */
 217         else
 218                 quadramp_set_1st_order_vars(qr, 3000, 3000); /* set speed */
 219 }
 220
 221 uint8_t spickle_is_packed(uint8_t side)
 222 {
 223         return spickle_is_at_pos(side, spickle.pos_packed[side]);
 224 }
 225
 226 uint8_t spickle_is_deployed(uint8_t side)
 227 {
 228         return spickle_is_at_pos(side, spickle.pos_deployed[side]);
 229 }
 230
 231 void spickle_deploy(uint8_t side)
 232 {
 233         spickle_set_qr(side);
 234         cs_set_consign(&spickle.csb[side]->cs, spickle.pos_deployed[side]);
 235 }
 236
 237 void spickle_mid(uint8_t side)
 238 {
 239         spickle_set_qr(side);
 240         cs_set_consign(&spickle.csb[side]->cs, spickle.pos_mid[side]);
 241 }
 242
 243 void spickle_pack(uint8_t side)
 244 {
 245         spickle_set_qr(side);
 246         cs_set_consign(&spickle.csb[side]->cs, spickle.pos_packed[side]);
 247 }
 248
 249 void spickle_init(void)
 250 {
 251         spickle_autopos();
 252         cobboard.left_spickle.on = 1;
 253         cobboard.right_spickle.on = 1;
 254 }