projects/microb2010/tests/test_board2008/actuator.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.3 2009-05-02 10:08:09 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 <uart.h>
  28 #include <encoders_microb.h>
  29 #include <pwm_ng.h>
  30 #include <timer.h>
  31 #include <scheduler.h>
  32 #include <clock_time.h>
  33
  34 #include <pid.h>
  35 #include <quadramp.h>
  36 #include <control_system_manager.h>
  37 #include <trajectory_manager.h>
  38 #include <blocking_detection_manager.h>
  39 #include <robot_system.h>
  40 #include <position_manager.h>
  41
  42 #include <rdline.h>
  43
  44 #include "sensor.h"
  45 #include "main.h"
  46
  47 #define OFF 0
  48 #define WAIT_SENSOR 1
  49 #define SENSOR_OK 2
  50 #define WAIT_DOWN 3
  51
  52 static volatile uint8_t fessor_state = OFF;
  53 static volatile uint32_t fessor_pos_up =  35000;
  54 static volatile uint32_t fessor_pos_down = 0;
  55 static volatile uint32_t fessor_delay_up = 500;
  56 static volatile uint32_t fessor_delay_down = 2000;
  57 static volatile uint32_t delay = 0;
  58 static volatile int32_t fessor_k1 = 500, fessor_k2 = 20;
  59 static volatile int32_t fessor_cmd = 0;
  60
  61
  62 static volatile uint8_t elevator_state = OFF;
  63 static volatile uint32_t elevator_pos_up =  -8000;
  64 static volatile uint32_t elevator_pos_down = 0;
  65 static volatile uint32_t elevator_delay_up = 500;
  66 static volatile uint32_t elevator_delay_down = 2000;
  67 //static volatile uint32_t delay = 0;
  68 static volatile int32_t elevator_k1 = 500, elevator_k2 = 20;
  69 static volatile int32_t elevator_cmd = 0;
  70
  71 void pwm_set_and_save(void *pwm, int32_t val)
  72 {
  73         /* we need to do the saturation here, before saving the
  74          * value */
  75         if (val > 4095)
  76                 val = 4095;
  77         if (val < -4095)
  78                 val = -4095;
  79
  80         if (pwm == LEFT_PWM)
  81                 mainboard.pwm_l = val;
  82         else if (pwm == RIGHT_PWM)
  83                 mainboard.pwm_r = val;
  84         pwm_ng_set(pwm, val);
  85 }
  86
  87 void pickup_wheels_on(void)
  88 {
  89         mainboard.enable_pickup_wheels = 1;
  90 }
  91
  92 void pickup_wheels_off(void)
  93 {
  94         mainboard.enable_pickup_wheels = 0;
  95 }
  96
  97 /* init fessor position at beginning */
  98 void fessor_autopos(void)
  99 {
 100         pwm_ng_set(FESSOR_PWM, -500);
 101         wait_ms(3000);
 102         pwm_ng_set(FESSOR_PWM, 0);
 103 }
 104
 105 /* set CS command for fessor */
 106 void fessor_set(void *dummy, int32_t cmd)
 107 {
 108         fessor_cmd = cmd;
 109 }
 110
 111 void fessor_set_coefs(uint32_t k1, uint32_t k2)
 112 {
 113         fessor_k1 = k1;
 114         fessor_k2 = k2;
 115 }
 116
 117 void fessor_set_delays(uint32_t delay_up, uint32_t delay_down)
 118 {
 119         fessor_delay_up = delay_up;
 120         fessor_delay_down = delay_down;
 121 }
 122
 123 void fessor_set_pos(uint32_t pos_up, uint32_t pos_down)
 124 {
 125         fessor_pos_up = pos_up;
 126         fessor_pos_down = pos_down;
 127 }
 128
 129 void fessor_dump_params(void)
 130 {
 131         printf_P(PSTR("coef %ld %ld\r\n"), fessor_k1, fessor_k2);
 132         printf_P(PSTR("pos %ld %ld\r\n"), fessor_pos_up, fessor_pos_down);
 133         printf_P(PSTR("delay %ld %ld\r\n"), fessor_delay_up, fessor_delay_down);
 134 }
 135
 136 /* called by CS perdiodically (current limit) */
 137 void fessor_manage(void)
 138 {
 139         static int32_t oldpos = 0;
 140         int32_t pos, maxcmd, speed, cmd;
 141
 142         cmd = fessor_cmd;
 143         pos = encoders_microb_get_value(FESSOR_ENC);
 144         speed = pos - oldpos;
 145         if (speed > 0 && cmd < 0)
 146                 maxcmd = fessor_k1;
 147         else if (speed < 0 && cmd > 0)
 148                 maxcmd = fessor_k1;
 149         else {
 150                 speed = ABS(speed);
 151                 maxcmd = fessor_k1 + fessor_k2 * speed;
 152         }
 153         if (cmd > maxcmd)
 154                 cmd = maxcmd;
 155         else if (cmd < -maxcmd)
 156                 cmd = -maxcmd;
 157
 158         pwm_ng_set(FESSOR_PWM, cmd);
 159
 160         oldpos = pos;
 161 }
 162
 163 void fessor_up(void)
 164 {
 165         fessor_state = 0;
 166         cs_set_consign(&mainboard.fessor.cs, fessor_pos_up);
 167 }
 168
 169 void fessor_down(void)
 170 {
 171         fessor_state = 0;
 172         cs_set_consign(&mainboard.fessor.cs, fessor_pos_down);
 173 }
 174
 175 void fessor_stop(void)
 176 {
 177         fessor_state = 0;
 178 }
 179
 180 void fessor_auto(void)
 181 {
 182         fessor_state = WAIT_SENSOR;
 183         cs_set_consign(&mainboard.fessor.cs, fessor_pos_up);
 184 }
 185
 186 /* for fessor auto mode */
 187 static void fessor_cb(void *dummy)
 188 {
 189         static uint8_t prev = 0;
 190         uint8_t val;
 191
 192         val = !sensor_get(0);
 193
 194         switch (fessor_state) {
 195         case OFF:
 196                 break;
 197         case WAIT_SENSOR:
 198                 if (val && !prev) {
 199                         delay = fessor_delay_up;
 200                         fessor_state = SENSOR_OK;
 201                 }
 202                 break;
 203         case SENSOR_OK:
 204                 if (delay-- == 0) {
 205                         cs_set_consign(&mainboard.fessor.cs, fessor_pos_down);
 206                         fessor_state = WAIT_DOWN;
 207                         delay = fessor_delay_down;
 208                 }
 209                 break;
 210         case WAIT_DOWN:
 211                 if (delay-- == 0) {
 212                         cs_set_consign(&mainboard.fessor.cs, fessor_pos_up);
 213                         fessor_state = WAIT_SENSOR;
 214                 }
 215                 break;
 216         default:
 217                 break;
 218         }
 219         prev = val;
 220 }
 221
 222 void fessor_init(void)
 223 {
 224         scheduler_add_periodical_event_priority(fessor_cb, NULL,
 225                                                 1000L / SCHEDULER_UNIT,
 226                                                 FESSOR_PRIO);
 227 }
 228
 229
 230
 231
 232
 233
 234
 235
 236 /* init elevator position at beginning */
 237 void elevator_autopos(void)
 238 {
 239         pwm_ng_set(ELEVATOR_PWM, 300);
 240         wait_ms(4000);
 241         pwm_ng_set(ELEVATOR_PWM, 0);
 242 }
 243
 244 /* set CS command for elevator */
 245 void elevator_set(void *dummy, int32_t cmd)
 246 {
 247         elevator_cmd = cmd;
 248 }
 249
 250 void elevator_set_coefs(uint32_t k1, uint32_t k2)
 251 {
 252         elevator_k1 = k1;
 253         elevator_k2 = k2;
 254 }
 255
 256 void elevator_set_delays(uint32_t delay_up, uint32_t delay_down)
 257 {
 258         elevator_delay_up = delay_up;
 259         elevator_delay_down = delay_down;
 260 }
 261
 262 void elevator_set_pos(uint32_t pos_up, uint32_t pos_down)
 263 {
 264         elevator_pos_up = pos_up;
 265         elevator_pos_down = pos_down;
 266 }
 267
 268 void elevator_dump_params(void)
 269 {
 270         printf_P(PSTR("coef %ld %ld\r\n"), elevator_k1, elevator_k2);
 271         printf_P(PSTR("pos %ld %ld\r\n"), elevator_pos_up, elevator_pos_down);
 272         printf_P(PSTR("delay %ld %ld\r\n"), elevator_delay_up, elevator_delay_down);
 273 }
 274
 275 /* called by CS perdiodically (current limit) */
 276 void elevator_manage(void)
 277 {
 278         static int32_t oldpos = 0;
 279         int32_t pos, maxcmd, speed, cmd;
 280
 281         cmd = elevator_cmd;
 282         pos = encoders_microb_get_value(ELEVATOR_ENC);
 283         speed = pos - oldpos;
 284         if (speed > 0 && cmd < 0)
 285                 maxcmd = elevator_k1;
 286         else if (speed < 0 && cmd > 0)
 287                 maxcmd = elevator_k1;
 288         else {
 289                 speed = ABS(speed);
 290                 maxcmd = elevator_k1 + elevator_k2 * speed;
 291         }
 292         if (cmd > maxcmd)
 293                 cmd = maxcmd;
 294         else if (cmd < -maxcmd)
 295                 cmd = -maxcmd;
 296
 297         pwm_ng_set(ELEVATOR_PWM, cmd);
 298
 299         oldpos = pos;
 300 }
 301
 302 void elevator_up(void)
 303 {
 304         elevator_state = 0;
 305         cs_set_consign(&mainboard.elevator.cs, elevator_pos_up);
 306 }
 307
 308 void elevator_down(void)
 309 {
 310         elevator_state = 0;
 311         cs_set_consign(&mainboard.elevator.cs, elevator_pos_down);
 312 }
 313
 314 void elevator_stop(void)
 315 {
 316         elevator_state = 0;
 317 }
 318
 319 void elevator_auto(void)
 320 {
 321         elevator_state = WAIT_SENSOR;
 322         cs_set_consign(&mainboard.elevator.cs, elevator_pos_down);
 323 }
 324
 325 /* for elevator auto mode */
 326 static void elevator_cb(void *dummy)
 327 {
 328         static uint8_t prev = 0;
 329         uint8_t val;
 330
 331         val = !sensor_get(0);
 332
 333         switch (elevator_state) {
 334         case OFF:
 335                 break;
 336         case WAIT_SENSOR:
 337                 if (val && !prev) {
 338                         delay = elevator_delay_up;
 339                         elevator_state = SENSOR_OK;
 340                 }
 341                 break;
 342         case SENSOR_OK:
 343                 if (delay-- == 0) {
 344                         cs_set_consign(&mainboard.elevator.cs, elevator_pos_up);
 345                         elevator_state = WAIT_DOWN;
 346                         delay = elevator_delay_down;
 347                 }
 348                 break;
 349         case WAIT_DOWN:
 350                 if (delay-- == 0) {
 351                         cs_set_consign(&mainboard.elevator.cs, elevator_pos_down);
 352                         elevator_state = WAIT_SENSOR;
 353                 }
 354                 break;
 355         default:
 356                 break;
 357         }
 358         prev = val;
 359 }
 360
 361 void elevator_init(void)
 362 {
 363         scheduler_add_periodical_event_priority(elevator_cb, NULL,
 364                                                 1000L / SCHEDULER_UNIT,
 365                                                 ELEVATOR_PRIO);
 366 }