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         int16_t delay_deployed[2];
  58         int16_t delay_packed[2];
  59         int32_t pos_deployed[2];
  60         int32_t pos_packed[2];
  61 };
  62
  63 static struct spickle_params spickle = {
  64         .k1 = 500,
  65         .k2 = 20,
  66         .csb = {
  67                 &cobboard.left_spickle,
  68                 &cobboard.right_spickle,
  69         },
  70         .delay_deployed = {
  71                 500, /* left */
  72                 500, /* right */
  73         },
  74         .delay_packed = {
  75                 500, /* left */
  76                 500, /* right */
  77         },
  78         .pos_deployed = {
  79                 35000, /* left */
  80                 35000, /* right */
  81         },
  82         .pos_packed = {
  83                 0, /* left */
  84                 0, /* right */
  85         },
  86 };
  87
  88 /* init spickle position at beginning */
  89 static void spickle_autopos(void)
  90 {
  91         pwm_ng_set(LEFT_SPICKLE_PWM, -500);
  92         //pwm_ng_set(RIGHT_SPICKLE_PWM, -500);
  93         wait_ms(1000);
  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);
  98 }
  99
 100 /* Set CS command for spickle. Called by CS manager. */
 101 void spickle_set(void *mot, int32_t cmd)
 102 {
 103         static int32_t oldpos_left, oldpos_right;
 104         int32_t oldpos, pos, maxcmd, speed;
 105
 106         if (mot == LEFT_SPICKLE_PWM) {
 107                 pos = encoders_spi_get_value(LEFT_SPICKLE_ENCODER);
 108                 oldpos = oldpos_left;
 109         }
 110         else {
 111                 pos = encoders_spi_get_value(RIGHT_SPICKLE_ENCODER);
 112                 oldpos = oldpos_right;
 113         }
 114
 115         speed = pos - oldpos;
 116         if (speed > 0 && cmd < 0)
 117                 maxcmd = spickle.k1;
 118         else if (speed < 0 && cmd > 0)
 119                 maxcmd = spickle.k1;
 120         else {
 121                 speed = ABS(speed);
 122                 maxcmd = spickle.k1 + spickle.k2 * speed;
 123         }
 124         if (cmd > maxcmd)
 125                 cmd = maxcmd;
 126         else if (cmd < -maxcmd)
 127                 cmd = -maxcmd;
 128
 129         pwm_ng_set(mot, cmd);
 130
 131         if (mot == LEFT_SPICKLE_PWM)
 132                 oldpos_left = pos;
 133         else
 134                 oldpos_right = pos;
 135 }
 136
 137 void spickle_set_coefs(uint32_t k1, uint32_t k2)
 138 {
 139         spickle.k1 = k1;
 140         spickle.k2 = k2;
 141 }
 142
 143 void spickle_set_pos(uint8_t side, uint32_t pos_deployed, uint32_t pos_packed)
 144 {
 145         spickle.pos_deployed[side] = pos_deployed;
 146         spickle.pos_packed[side] = pos_packed;
 147 }
 148
 149 void spickle_set_delay(uint8_t side, uint32_t delay_deployed, uint32_t delay_packed)
 150 {
 151         spickle.delay_deployed[side] = delay_deployed;
 152         spickle.delay_packed[side] = delay_packed;
 153 }
 154
 155 void spickle_dump_params(void)
 156 {
 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]);
 170 }
 171
 172 void spickle_deploy(uint8_t side)
 173 {
 174         cs_set_consign(&spickle.csb[side]->cs, spickle.pos_deployed[side]);
 175 }
 176
 177 void spickle_pack(uint8_t side)
 178 {
 179         cs_set_consign(&spickle.csb[side]->cs, spickle.pos_packed[side]);
 180 }
 181
 182 uint16_t spickle_get_deploy_delay(uint8_t side)
 183 {
 184         return spickle.delay_deployed[side];
 185 }
 186
 187 uint16_t spickle_get_pack_delay(uint8_t side)
 188 {
 189         return spickle.delay_packed[side];
 190 }
 191
 192 void spickle_init(void)
 193 {
 194         spickle_autopos();
 195         cobboard.left_spickle.on = 1;
 196         //cobboard.right_spickle.on = 1;
 197 }