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                 500,// 7000, // 200, /* left */
  71                 500,// 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 void spickle_set_pos(uint8_t side, int32_t pos_packed,
 141                      int32_t pos_mid, int32_t pos_deployed)
 142 {
 143         spickle.pos_deployed[side] = pos_deployed;
 144         spickle.pos_mid[side] = pos_mid;
 145         spickle.pos_packed[side] = pos_packed;
 146 }
 147
 148 void spickle_dump_params(void)
 149 {
 150         printf_P(PSTR("coef %ld %ld\r\n"), spickle.k1, spickle.k2);
 151         printf_P(PSTR("left pos %ld %ld\r\n"),
 152                  spickle.pos_packed[I2C_LEFT_SIDE],
 153                  spickle.pos_mid[I2C_LEFT_SIDE],
 154                  spickle.pos_deployed[I2C_LEFT_SIDE]);
 155         printf_P(PSTR("right pos %ld %ld\r\n"),
 156                  spickle.pos_packed[I2C_RIGHT_SIDE],
 157                  spickle.pos_mid[I2C_RIGHT_SIDE],
 158                  spickle.pos_deployed[I2C_RIGHT_SIDE]);
 159 }
 160
 161 static uint8_t spickle_is_at_pos(uint8_t side, int32_t pos)
 162 {
 163         int32_t diff;
 164         int32_t enc;
 165         if (side == I2C_LEFT_SIDE)
 166                 enc = encoders_spi_get_value(LEFT_SPICKLE_ENCODER);
 167         else
 168                 enc = encoders_spi_get_value(RIGHT_SPICKLE_ENCODER);
 169         diff = pos - enc;
 170         if (diff < 0)
 171                 diff = -diff;
 172         if (diff < 500)
 173                 return 1;
 174         return 0;
 175 }
 176
 177 uint8_t spickle_is_packed(uint8_t side)
 178 {
 179         return spickle_is_at_pos(side, spickle.pos_packed[side]);
 180 }
 181
 182 uint8_t spickle_is_deployed(uint8_t side)
 183 {
 184         return spickle_is_at_pos(side, spickle.pos_deployed[side]);
 185 }
 186
 187 void spickle_deploy(uint8_t side)
 188 {
 189         cs_set_consign(&spickle.csb[side]->cs, spickle.pos_deployed[side]);
 190 }
 191
 192 void spickle_mid(uint8_t side)
 193 {
 194         cs_set_consign(&spickle.csb[side]->cs, spickle.pos_mid[side]);
 195 }
 196
 197 void spickle_pack(uint8_t side)
 198 {
 199         cs_set_consign(&spickle.csb[side]->cs, spickle.pos_packed[side]);
 200 }
 201
 202 void spickle_init(void)
 203 {
 204         spickle_autopos();
 205         cobboard.left_spickle.on = 1;
 206         cobboard.right_spickle.on = 1;
 207 }