projects/microb2010/cobboard/state.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: state.c,v 1.5 2009-11-08 17:25:00 zer0 Exp $
  19  *
  20  */
  21
  22 #include <math.h>
  23 #include <string.h>
  24
  25 #include <aversive.h>
  26 #include <aversive/wait.h>
  27 #include <aversive/error.h>
  28
  29 #include <ax12.h>
  30 #include <uart.h>
  31 #include <spi.h>
  32 #include <encoders_spi.h>
  33 #include <pwm_ng.h>
  34 #include <timer.h>
  35 #include <scheduler.h>
  36 #include <clock_time.h>
  37
  38 #include <pid.h>
  39 #include <quadramp.h>
  40 #include <control_system_manager.h>
  41 #include <blocking_detection_manager.h>
  42
  43 #include <rdline.h>
  44 #include <vt100.h>
  45
  46 #include "../common/i2c_commands.h"
  47 #include "main.h"
  48 #include "cmdline.h"
  49 #include "sensor.h"
  50 #include "actuator.h"
  51 #include "state.h"
  52
  53 #define STMCH_DEBUG(args...) DEBUG(E_USER_ST_MACH, args)
  54 #define STMCH_NOTICE(args...) NOTICE(E_USER_ST_MACH, args)
  55 #define STMCH_ERROR(args...) ERROR(E_USER_ST_MACH, args)
  56
  57 /* shorter aliases for this file */
  58 #define INIT               I2C_COBBOARD_MODE_INIT
  59 #define MANUAL             I2C_COBBOARD_MODE_MANUAL
  60 #define HARVEST            I2C_COBBOARD_MODE_HARVEST
  61 #define EXIT               I2C_COBBOARD_MODE_EXIT
  62
  63 static struct i2c_cmd_cobboard_set_mode mainboard_command;
  64 static struct vt100 local_vt100;
  65 static volatile uint8_t prev_state;
  66 static volatile uint8_t changed = 0;
  67
  68 uint8_t state_debug = 0;
  69
  70 void state_dump_sensors(void)
  71 {
  72         STMCH_DEBUG("TODO\n");
  73 }
  74
  75 void state_debug_wait_key_pressed(void)
  76 {
  77         if (!state_debug)
  78                 return;
  79         printf_P(PSTR("press a key\r\n"));
  80         while(!cmdline_keypressed());
  81 }
  82
  83 /* set a new state, return 0 on success */
  84 int8_t state_set_mode(struct i2c_cmd_cobboard_set_mode *cmd)
  85 {
  86         changed = 1;
  87         prev_state = mainboard_command.mode;
  88         memcpy(&mainboard_command, cmd, sizeof(mainboard_command));
  89         STMCH_DEBUG("%s mode=%d", __FUNCTION__, mainboard_command.mode);
  90         return 0;
  91 }
  92
  93 /* check that state is the one in parameter and that state did not
  94  * changed */
  95 uint8_t state_check(uint8_t mode)
  96 {
  97         int16_t c;
  98         if (mode != mainboard_command.mode)
  99                 return 0;
 100
 101         if (changed)
 102                 return 0;
 103
 104         /* force quit when CTRL-C is typed */
 105         c = cmdline_getchar();
 106         if (c == -1)
 107                 return 1;
 108         if (vt100_parser(&local_vt100, c) == KEY_CTRL_C) {
 109                 mainboard_command.mode = EXIT;
 110                 return 0;
 111         }
 112         return 1;
 113 }
 114
 115 uint8_t state_get_mode(void)
 116 {
 117         return mainboard_command.mode;
 118 }
 119
 120 /* manual mode, arm position is sent from mainboard */
 121 static void state_do_manual(void)
 122 {
 123         if (!state_check(MANUAL))
 124                 return;
 125         STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
 126         while (state_check(MANUAL));
 127 }
 128
 129 /* init mode */
 130 static void state_do_init(void)
 131 {
 132         if (!state_check(INIT))
 133                 return;
 134         state_init();
 135         STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
 136         while (state_check(INIT));
 137 }
 138
 139 /* harvest columns elts from area */
 140 static void state_do_harvest(void)
 141 {
 142         if (!state_check(HARVEST))
 143                 return;
 144         STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
 145         while (state_check(HARVEST));
 146 }
 147 /* main state machine */
 148 void state_machine(void)
 149 {
 150         while (state_get_mode() != EXIT) {
 151                 changed = 0;
 152                 state_do_init();
 153                 state_do_manual();
 154                 state_do_harvest();
 155         }
 156 }
 157
 158 void state_init(void)
 159 {
 160         vt100_init(&local_vt100);
 161         mainboard_command.mode = HARVEST;
 162         cobboard.cob_count = 0;
 163 }