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 "actuator.h"
  46 #include "main.h"
  47
  48
  49 #define OFF 0
  50 #define WAIT_SENSOR 1
  51 #define SENSOR_OK 2
  52 #define WAIT_DOWN 3
  53
  54 static volatile uint8_t spickle_state = OFF;
  55 static volatile uint32_t spickle_pos_up =  35000;
  56 static volatile uint32_t spickle_pos_down = 0;
  57 static volatile uint32_t spickle_delay_up = 500;
  58 static volatile uint32_t spickle_delay_down = 2000;
  59 static volatile uint32_t delay = 0;
  60 static volatile int32_t spickle_k1 = 500, spickle_k2 = 20;
  61 static volatile int32_t spickle_cmd = 0;
  62
  63 /* init spickle position at beginning */
  64 static void spickle_autopos(void)
  65 {
  66         pwm_ng_set(LEFT_SPICKLE_PWM, -500);
  67         wait_ms(3000);
  68         pwm_ng_set(LEFT_SPICKLE_PWM, 0);
  69         encoders_spi_set_value(LEFT_SPICKLE_ENCODER, 0);
  70 }
  71
  72 /* set CS command for spickle */
  73 void spickle_set(void *mot, int32_t cmd)
  74 {
  75         static int32_t oldpos_left, oldpos_right;
  76         int32_t oldpos, pos, maxcmd, speed;
  77
  78         if (mot == LEFT_SPICKLE_PWM) {
  79                 pos = encoders_spi_get_value(LEFT_SPICKLE_ENCODER);
  80                 oldpos = oldpos_left;
  81         }
  82         else {
  83                 pos = encoders_spi_get_value(RIGHT_SPICKLE_ENCODER);
  84                 oldpos = oldpos_right;
  85         }
  86
  87         speed = pos - oldpos;
  88         if (speed > 0 && cmd < 0)
  89                 maxcmd = spickle_k1;
  90         else if (speed < 0 && cmd > 0)
  91                 maxcmd = spickle_k1;
  92         else {
  93                 speed = ABS(speed);
  94                 maxcmd = spickle_k1 + spickle_k2 * speed;
  95         }
  96         if (cmd > maxcmd)
  97                 cmd = maxcmd;
  98         else if (cmd < -maxcmd)
  99                 cmd = -maxcmd;
 100
 101         pwm_ng_set(mot, cmd);
 102
 103         if (mot == LEFT_SPICKLE_PWM)
 104                 oldpos_left = pos;
 105         else
 106                 oldpos_right = pos;
 107 }
 108
 109 void spickle_set_coefs(uint32_t k1, uint32_t k2)
 110 {
 111         spickle_k1 = k1;
 112         spickle_k2 = k2;
 113 }
 114
 115 void spickle_set_delays(uint32_t delay_up, uint32_t delay_down)
 116 {
 117         spickle_delay_up = delay_up;
 118         spickle_delay_down = delay_down;
 119 }
 120
 121 void spickle_set_pos(uint32_t pos_up, uint32_t pos_down)
 122 {
 123         spickle_pos_up = pos_up;
 124         spickle_pos_down = pos_down;
 125 }
 126
 127 void spickle_dump_params(void)
 128 {
 129         printf_P(PSTR("coef %ld %ld\r\n"), spickle_k1, spickle_k2);
 130         printf_P(PSTR("pos %ld %ld\r\n"), spickle_pos_up, spickle_pos_down);
 131         printf_P(PSTR("delay %ld %ld\r\n"), spickle_delay_up, spickle_delay_down);
 132 }
 133
 134 void spickle_up(void)
 135 {
 136         spickle_state = 0;
 137         cs_set_consign(&cobboard.left_spickle.cs, spickle_pos_up);
 138 }
 139
 140 void spickle_down(void)
 141 {
 142         spickle_state = 0;
 143         cs_set_consign(&cobboard.left_spickle.cs, spickle_pos_down);
 144 }
 145
 146 void spickle_stop(void)
 147 {
 148         spickle_state = 0;
 149 }
 150
 151 void spickle_auto(void)
 152 {
 153         spickle_state = WAIT_SENSOR;
 154         cs_set_consign(&cobboard.left_spickle.cs, spickle_pos_up);
 155 }
 156
 157 /* for spickle auto mode */
 158 static void spickle_cb(__attribute__((unused)) void *dummy)
 159 {
 160         static uint8_t prev = 0;
 161         uint8_t val;
 162
 163         val = sensor_get(S_LCOB);
 164
 165         switch (spickle_state) {
 166         case OFF:
 167                 break;
 168         case WAIT_SENSOR:
 169                 if (val && !prev) {
 170                         delay = spickle_delay_up;
 171                         spickle_state = SENSOR_OK;
 172                 }
 173                 break;
 174         case SENSOR_OK:
 175                 if (delay-- == 0) {
 176                         cs_set_consign(&cobboard.left_spickle.cs, spickle_pos_down);
 177                         spickle_state = WAIT_DOWN;
 178                         delay = spickle_delay_down;
 179                 }
 180                 break;
 181         case WAIT_DOWN:
 182                 if (delay-- == 0) {
 183                         cs_set_consign(&cobboard.left_spickle.cs, spickle_pos_up);
 184                         spickle_state = WAIT_SENSOR;
 185                 }
 186                 break;
 187         default:
 188                 break;
 189         }
 190         prev = val;
 191 }
 192
 193 void spickle_init(void)
 194 {
 195         spickle_autopos();
 196
 197         scheduler_add_periodical_event_priority(spickle_cb, NULL,
 198                                                 1000L / SCHEDULER_UNIT,
 199                                                 SPICKLE_PRIO);
 200 }