--- /dev/null
+#include <lpc21xx.h>
+#include <deb_led.h>
+#include <system_def.h>
+#include <string.h>
+#include <can_msg_def.h>
+#include "uar.h"
+#include "fsm.h"
+#include <engine.h>
+#include <stdbool.h>
+#include "def.h"
+#include <adc.h>
+
+#define DBG_ENTRY() do { \
+ send_rs_str(__func__); \
+ send_rs_str(": entry\n"); \
+ } while(0);
+
+static void wait_for_cmd(struct fsm *fsm, enum event event);
+static void move(struct fsm *fsm, enum event event);
+static void homing(struct fsm *fsm, enum event event);
+static void stop(void);
+
+
+static void homing(struct fsm *fsm, enum event event)
+{
+ static uint32_t time_start = 0;
+
+ switch (event) {
+ case EVENT_ENTRY:
+ DBG_ENTRY();
+ time_start = timer_msec;
+ break;
+ case EVENT_DO:
+ //homing with 8s timeout
+ engine_A_en(ENGINE_EN_ON);
+ engine_A_dir(ENGINE_DIR_FW);
+ engine_A_pwm(50);
+
+ if(fsm->flags & CAN_LIFT_SWITCH_DOWN){
+ engine_A_pwm(0);
+ fsm->act_pos = 0;
+ fsm->current_state = wait_for_cmd;
+ fsm->flags |= CAN_LIFT_HOMED;
+ fsm->trigger_can_send = 1;
+ fsm->can_req_homing = 0;
+ fsm->can_req_position = 0;
+ // fsm->can_req_position = 0x54E2;
+ } else if (timer_msec >= (time_start + 4000)) {
+ stop();
+ fsm->current_state = wait_for_cmd;
+ fsm->flags |= CAN_LIFT_TIMEOUT;
+ fsm->flags &= ~CAN_LIFT_HOMED;
+ fsm->trigger_can_send = 1;
+ fsm->can_req_homing = 0;
+ }
+ break;
+ case EVENT_EXIT:
+ break;
+ }
+}
+
+void fsm_lift_init(struct fsm *fsm, enum event event)
+{
+ switch (event) {
+ case EVENT_ENTRY:
+ DBG_ENTRY();
+ fsm->flags |= CAN_LIFT_INITIALIZING;
+ break;
+ case EVENT_DO:
+ fsm->flags &= ~CAN_LIFT_INITIALIZING;
+ fsm->current_state = wait_for_cmd;
+ break;
+ case EVENT_EXIT:
+ break;
+ }
+}
+
+static void stop()
+{
+ engine_A_pwm(0);
+ engine_A_en(ENGINE_EN_OFF);
+}
+
+
+#define DEAD_ZONE 10
+static bool do_control(struct fsm *fsm, int P)
+{
+ bool finished;
+ int e = fsm->req_pos - fsm->act_pos;
+ int action = (P*e) / 5; // ORIGINAL: int action = P*e;
+
+ engine_A_dir(action > 0);
+ action = action > 100 ? 100 : action;
+
+//#define BANG_BANG
+#ifdef BANG_BANG
+ action = action > 0 ? action : -action;
+ action = action > 40 ? 100 : 0;
+#else
+ action = action > 0 ? action : -action;
+#endif
+
+ engine_A_pwm(action);
+ engine_A_en(ENGINE_EN_ON);
+
+ if (fsm->req_target > fsm->start_pos)
+ finished = fsm->act_pos > fsm->req_target - DEAD_ZONE;
+ else
+ finished = fsm->act_pos < fsm->req_target + DEAD_ZONE;
+
+ return finished;
+}
+
+static void wait_for_cmd(struct fsm *fsm, enum event event)
+{
+ static int last_can_req_pos = 0;
+ switch (event) {
+ case EVENT_ENTRY:
+ DBG_ENTRY();
+ stop();
+ break;
+ case EVENT_DO:
+ //homing if push home button or homing can msg. received
+ if ((fsm->flags & CAN_LIFT_SWITCH_HOME) || (fsm->can_req_homing)) {
+ fsm->can_req_homing = 0;
+ fsm->current_state = homing;
+ break;
+ }
+ /* if lift is not homed, do not allow movement and regulation */
+ if (fsm->flags & CAN_LIFT_HOMED) {
+ do_control(fsm, 2);
+
+ if (fsm->can_req_position != last_can_req_pos &&
+ fsm->can_req_position != fsm->req_target) {
+ last_can_req_pos = fsm->can_req_position;
+ fsm->req_target = fsm->can_req_position;
+ fsm->req_spd = fsm->can_req_spd;
+ fsm->current_state = move;
+ }
+ }
+ break;
+ case EVENT_EXIT:
+ break;
+ }
+}
+
+#define XMIN(a,b) ((a) < (b) ? (a) : (b))
+#define XMAX(a,b) ((a) > (b) ? (a) : (b))
+static void move(struct fsm *fsm, enum event event)
+{
+ static int counter;
+ static bool lift_stopped_on_end = false;
+ bool finished;
+ switch (event) {
+ case EVENT_ENTRY:
+ counter = 0;
+ DBG_ENTRY();
+ fsm->time_start = timer_msec;
+ fsm->start_pos = fsm->act_pos;
+
+ /* check if movement starts on end switch */
+ if ((fsm->flags & CAN_LIFT_SWITCH_DOWN) || (fsm->flags & CAN_LIFT_SWITCH_UP))
+ lift_stopped_on_end = true;
+ else
+ lift_stopped_on_end = false;
+
+ if(fsm->req_spd == 0)
+ fsm->req_pos = fsm->req_target;
+ else
+ fsm->req_pos = fsm->start_pos;
+ break;
+ case EVENT_DO:
+ /* if movement starts on end switch, ignore this, else stop movement on act position */
+ if ((fsm->flags & CAN_LIFT_SWITCH_UP) && !lift_stopped_on_end){
+ fsm->can_response = fsm->req_target;
+ fsm->current_state = wait_for_cmd;
+ fsm->req_pos = fsm->act_pos;
+ lift_stopped_on_end = true;
+ }
+ /* if movement starts on end switch, ignore this, else stop movement on act position */
+ if ((fsm->flags & CAN_LIFT_SWITCH_DOWN) && !lift_stopped_on_end) {
+ fsm->can_response = fsm->req_target;
+ fsm->current_state = wait_for_cmd;
+ fsm->act_pos = 0;
+ fsm->req_pos = fsm->act_pos;
+ lift_stopped_on_end = true;
+ }
+
+ if (fsm->can_req_position != fsm->req_target) {
+ fsm->flags |= CAN_LIFT_TIMEOUT;
+ fsm->current_state = wait_for_cmd;
+ }
+
+ if(fsm->req_spd != 0 && counter++ >= 10)
+ {
+ counter = 0;
+ if(fsm->req_target > fsm->start_pos) {
+ fsm->req_pos = XMIN(fsm->req_pos + fsm->req_spd,fsm->req_target);
+ } else {
+ fsm->req_pos = XMAX(fsm->req_pos - fsm->req_spd,fsm->req_target);
+ }
+ }
+
+ if (timer_msec - fsm->time_start > (fsm->req_spd == 0 ? 2000 : 4000)) {
+ fsm->flags |= CAN_LIFT_TIMEOUT;
+ fsm->can_response = fsm->req_target;
+ fsm->current_state = wait_for_cmd;
+ fsm->req_pos = fsm->act_pos;
+ }
+
+ finished = do_control(fsm, fsm->req_spd ? 2 : 2);
+
+ if (finished && fsm->req_pos == fsm->req_target) {
+ fsm->can_response = fsm->req_target;
+ fsm->current_state = wait_for_cmd;
+ }
+ break;
+ case EVENT_EXIT:
+ stop();
+ fsm->trigger_can_send = true;;
+ break;
+ }
+}
--- /dev/null
+
+/**
+ * @file main.c
+ *
+ *
+ * @author Bc. Jiri Kubias, jiri.kubias@gmail.com
+ * @author Michal Sojka <sojkam1@fel.cvut.cz>
+ *
+ * @addtogroup fork
+ */
+
+
+/**
+ * @defgroup fork Vidle (fork) application
+ */
+/**
+ * @ingroup fork
+ * @{
+ */
+
+
+#include <lpc21xx.h> /* LPC21xx definitions */
+#include <types.h>
+#include <deb_led.h>
+#include <system_def.h>
+#include <can_ids.h>
+#include <periph/can.h>
+#include <string.h>
+#include <deb_led.h>
+#include "engine.h"
+#include "uar.h"
+#include <can_msg_def.h>
+#include "fsm.h"
+#include "def.h"
+#include <adc.h>
+
+#define CAN_SPEED 1000000 //< CAN bus speed
+#define CAN_ISR 0
+
+#define ADC_ISR 1
+
+#define TIMER_IRQ_PRIORITY 5
+
+
+struct fsm fsm_lift;
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+#define END_SWITCH_UP_PIN 9 //pin 4, exp. port on board
+#define END_SWITCH_DOWN_PIN 8 //pin 3, exp. port on board
+#define SWITCH_HOME_PIN 19 //pin 6, exp. port on board
+#define START_PIN 15 //pin 7, exp. port on board
+#define COLOR_PIN 18 //pin 5, exp. port on board
+#define SWITCH_STRATEGY_PIN 17 //pin 8, exp. port on board
+
+#define START_SEND_PRIOD_FAST 50 /* [miliseconds] */
+#define START_SEND_PRIOD_SLOW 300 /* [miliseconds] */
+#define START_SEND_FAST_COUNT 10 /* How many times to send start with small period (after a change) */
+
+#define LIFT_IRC_VAL_MAX 0x19C
+#define LIFT_IRC_VAL_MIN 0x0
+
+#define IRC_A_PIN 2 //pin 1, exp. port on board
+#define IRC_B_PIN 3 //pin 2, exp. port on board
+
+#define IRC_A_MASK 0x04 //(1<<IRC_A)
+#define IRC_B_MASK 0x08 //(1<<IRC_B)
+#define IRC_AB_MASK 0x0C //((1<<IRC_A)&(1<<IRC_B))
+
+void lift_switches_handler(void);
+
+void lift_switches_handler()
+{
+ if (IO0PIN & (1<<END_SWITCH_UP_PIN)){
+ fsm_lift.flags &= ~CAN_LIFT_SWITCH_UP;
+ deb_led_off(LEDR);
+ } else {
+ fsm_lift.flags |= CAN_LIFT_SWITCH_UP;
+ deb_led_on(LEDR);
+ }
+
+ if (IO0PIN & (1<<END_SWITCH_DOWN_PIN)){
+ fsm_lift.flags &= ~CAN_LIFT_SWITCH_DOWN;
+ deb_led_off(LEDR);
+ } else {
+ fsm_lift.flags |= CAN_LIFT_SWITCH_DOWN;
+ deb_led_on(LEDR);
+ }
+
+ if (IO0PIN & (1<<SWITCH_HOME_PIN)){
+ fsm_lift.flags &= ~CAN_LIFT_SWITCH_HOME;
+ deb_led_off(LEDR);
+ } else {
+ fsm_lift.flags |= CAN_LIFT_SWITCH_HOME;
+ deb_led_on(LEDR);
+ }
+// if (IO0PIN & (1<<IRC_A_PIN)){
+// deb_led_off(LEDR);
+// } else {
+// deb_led_on(LEDR);
+// }
+// if (IO0PIN & (1<<IRC_B_PIN)){
+// deb_led_off(LEDR);
+// } else {
+// deb_led_on(LEDR);
+// }
+}
+
+//source code from http://www.vosrk.cz/robotika/Stavba/Enkoder.pdf
+int32_t irc_read_tick(){
+
+ static uint16_t cnt_up = 0;
+ static uint16_t cnt_down = 0;
+ static uint16_t last_irc = 0;
+ static int32_t position = 0;
+ uint16_t irc, temp;
+
+ irc = IO0PIN & IRC_AB_MASK;
+ if ((irc & IRC_B_MASK) != 0){
+ irc ^= IRC_A_MASK;
+ }
+
+ temp = (irc - last_irc) & IRC_AB_MASK;
+
+ last_irc = irc;
+
+ if (temp == IRC_A_MASK){
+ /* count 100times slower - we do not need 250 ticks per milimeter*/
+ if (++cnt_down >= 100) {
+ cnt_down = 0;
+ cnt_up = 0;
+ deb_led_change(LEDB);
+ return --position;
+ }
+ } else if (temp == IRC_AB_MASK){
+ /* count 100times slower - we do not need 250 ticks per milimeter*/
+ if (++cnt_up >= 100) {
+ cnt_up = 0;
+ cnt_down = 0;
+ deb_led_change(LEDB);
+ return ++position;
+ }
+ }
+ return position;
+}
+
+void init_motors(void){
+
+ init_engine_A(); // initialization of PWM unit
+ engine_A_en(ENGINE_EN_ON); //enable motor A
+ engine_A_dir(ENGINE_DIR_FW); //set direction
+ engine_A_pwm(0); // STOP pwm is in percent, range 0~100~200
+
+}
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+void set_irq_handler(uint8_t source, uint8_t irq_vect, void (*handler)()) {
+ /* set interrupt vector */
+ ((uint32_t*)&VICVectAddr0)[irq_vect] = (uint32_t)handler;
+ ((uint32_t*)&VICVectCntl0)[irq_vect] = 0x20 | source;
+ /* enable interrupt */
+ VICIntEnable = 1<<source;
+}
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+/** timer0 & ISR **/
+
+void timer0_irq() __attribute__((interrupt));
+volatile uint32_t timer_msec = 0, timer_usec = 0;
+
+void init_timer0(uint32_t prescale, uint32_t period) {
+ T0TCR = 0x2; /* reset */
+ T0PR = prescale - 1;
+ T0MR0 = period;
+ T0MCR = 0x3; /* match0: reset & irq */
+ T0EMR = 0; /* no ext. match */
+ T0CCR = 0x0; /* no capture */
+ T0TCR = 0x1; /* go! */
+}
+
+void timer0_irq() {
+ static unsigned cnt1khz = 0;
+
+ /* reset timer irq */
+ T0IR = -1;
+
+ /* increment timer_usec */
+ timer_usec += 10;
+ /* increment msec @1kHz */
+ if (++cnt1khz == 100) {
+ cnt1khz = 0;
+ ++timer_msec;
+ }
+
+ /* int acknowledge */
+ VICVectAddr = 0;
+}
+
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+void start_button(void)
+{
+ can_msg_t msg;
+ bool start_condition;
+ static bool last_start_condition = 0;
+
+ static int count = 0;
+ static uint32_t next_send = 0;
+
+
+ start_condition = (IO0PIN & (1<<START_PIN)) == 0;
+
+ if (start_condition != last_start_condition) {
+
+
+ last_start_condition = start_condition;
+ count = 0;
+ next_send = timer_msec; /* Send now */
+
+ fsm_lift.flags |= CAN_LIFT_START;
+ }
+
+ if (timer_msec >= next_send) {
+ msg.id = CAN_ROBOT_CMD;
+ msg.flags = 0;
+ msg.dlc = 1;
+ msg.data[0] = start_condition;
+
+// send_rs_str("start\n");
+
+ /*while*/ (can_tx_msg(&msg));
+
+ if (count < START_SEND_FAST_COUNT) {
+ count++;
+ next_send = timer_msec + START_SEND_PRIOD_FAST;
+ } else
+ next_send = timer_msec + START_SEND_PRIOD_SLOW;
+ }
+
+
+}
+
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+void CAN_rx(can_msg_t *msg) {
+ uint32_t spd;
+ can_msg_t rx_msg;
+ uint32_t req =0;
+ memcpy(&rx_msg, msg, sizeof(can_msg_t));//make copy of message
+
+
+ deb_led_on(LEDB);
+
+ switch (rx_msg.id)
+ {
+ case CAN_LIFT_CMD:
+ deb_led_on(LEDB);
+ req = ((rx_msg.data[0]<<8) | (rx_msg.data[1]));
+ spd = rx_msg.data[2];
+ fsm_lift.can_req_homing=rx_msg.data[3];
+ // range 0 - A9C5
+ if (req >= LIFT_IRC_VAL_MIN && req <= LIFT_IRC_VAL_MAX) {
+ fsm_lift.flags &= ~CAN_LIFT_OUT_OF_BOUNDS;
+ fsm_lift.can_req_position = req;// save new req position of lift
+ fsm_lift.can_req_spd = spd;// save new req spd of lift
+ } else
+ fsm_lift.flags |= CAN_LIFT_OUT_OF_BOUNDS;
+ break;
+ default:break;
+ }
+
+ deb_led_off(LEDB);
+}
+
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+void init_periphery(void){
+
+ can_init_baudrate(CAN_SPEED, CAN_ISR, CAN_rx);//initialization of CAN bus
+ init_motors();
+
+ /* init timer0 */
+ init_timer0(1, CPU_APB_HZ/100000);
+ set_irq_handler(4 /*timer0*/, TIMER_IRQ_PRIORITY, timer0_irq);
+
+ init_uart();
+// init_adc(ADC_ISR);
+}
+/*********************************************************/
+void can_send_status(void)
+{
+ can_msg_t tx_msg;
+ tx_msg.id = CAN_LIFT_STATUS;
+ tx_msg.dlc = 5;
+ tx_msg.flags = 0;
+ tx_msg.data[0] = (fsm_lift.act_pos >> 8) & 0xFF;
+ tx_msg.data[1] = fsm_lift.act_pos & 0xFF;
+ tx_msg.data[2] = (fsm_lift.can_response >> 8) & 0xFF;
+ tx_msg.data[3] = fsm_lift.can_response & 0xFF;
+ tx_msg.data[4] = fsm_lift.flags;
+ /*while*/(can_tx_msg(&tx_msg)); /* CAN erratum workaround */
+}
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+void dbg_print_time()
+{
+ char str[10];
+ unsigned t = timer_usec, i;
+ memset(str, ' ', sizeof(str));
+ str[9] = 0;
+ str[8] = '\n';
+ for (i=7; t > 0; i--) {
+ str[i] = t%10 + '0';
+ t /= 10;
+ }
+ send_rs_str(str);
+}
+
+void fsm_lift_init(struct fsm *fsm, enum event event);
+
+
+void blink_led()
+{
+ static uint32_t led_time = 0;
+
+ if(timer_msec >= led_time + 500)
+ {
+ led_time = timer_msec;
+// static int up;
+// if (up == 0)
+// fsm_lift.can_req_position = 0x380;
+// if (up == 6)
+// fsm_lift.can_req_position = 0x1e0;
+// up = (up+1)%12;
+
+ deb_led_change(LEDG);
+
+ send_rs_str("ACT_POS\t");
+ send_rs_int(fsm_lift.act_pos);
+ send_rs_str("\t");
+ send_rs_str("CAN_FLAGS\t");
+ send_rs_int(fsm_lift.flags);
+ send_rs_str("\n");
+ }
+}
+
+void robot_switches_handler()
+{
+ static uint32_t color_time = 0;
+ char sw = 0;
+
+ if (timer_msec >= color_time + 100)
+ {
+ can_msg_t tx_msg;
+
+ color_time = timer_msec;
+
+ if (IO0PIN & (1<<COLOR_PIN))
+ sw |= CAN_SWITCH_COLOR;
+ else
+ sw &= ~CAN_SWITCH_COLOR;
+
+ if (IO0PIN & (1<<SWITCH_STRATEGY_PIN))
+ sw &= ~CAN_SWITCH_STRATEGY;
+ else
+ sw |= CAN_SWITCH_STRATEGY;
+
+ if (sw & CAN_SWITCH_COLOR){
+ deb_led_off(LEDY);
+
+ send_rs_str("color\n");}
+ else
+ deb_led_on(LEDY);
+
+ if (sw & CAN_SWITCH_STRATEGY){
+ deb_led_off(LEDY);
+
+ send_rs_str("strategy\n");
+ }
+ else
+ deb_led_on(LEDY);
+
+// send_rs_int(IO1PIN);
+// send_rs_int(sw);
+// send_rs_str("\n");
+//
+ /* TODO: Put color to the second bit */
+
+ tx_msg.id = CAN_ROBOT_SWITCHES;
+ tx_msg.dlc = 1;
+ tx_msg.flags = 0;
+ tx_msg.data[0] = sw;
+
+ can_tx_msg(&tx_msg);
+ }
+}
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+int main(void)
+{
+ uint32_t main_time = timer_usec;
+ uint32_t status_time = timer_usec;
+
+ //lift motor is motor A, MOTA connctor on board
+ init_periphery();
+
+ SET_PIN(PINSEL0, IRC_A_PIN, PINSEL_0);
+ SET_PIN(PINSEL0, IRC_B_PIN, PINSEL_0);
+
+ SET_PIN(PINSEL0, END_SWITCH_UP_PIN, PINSEL_0);
+ SET_PIN(PINSEL0, END_SWITCH_DOWN_PIN, PINSEL_0);
+
+ SET_PIN(PINSEL0, START_PIN, PINSEL_0); //init of start pin
+ SET_PIN(PINSEL1, (COLOR_PIN - 16), PINSEL_0); //init of color pin
+ SET_PIN(PINSEL1, (SWITCH_STRATEGY_PIN - 16), PINSEL_0); //init of strategy pin
+ SET_PIN(PINSEL1, (SWITCH_HOME_PIN - 16), PINSEL_0); //init of home pin
+
+ IO0DIR &= ~((1<<START_PIN) | (1<<SWITCH_HOME_PIN) | (1 << COLOR_PIN) | (1 << SWITCH_STRATEGY_PIN));
+ IO0DIR &= ~((1<<END_SWITCH_UP_PIN) | (1<<END_SWITCH_DOWN_PIN));
+ IO0DIR &= ~((1<<IRC_A_PIN) | (1<<IRC_B_PIN));
+ send_rs_str("Lift started\n");
+
+ fsm_lift.act_pos = 0;
+ init_fsm(&fsm_lift, &fsm_lift_init);
+
+/* return; */
+
+ while(1){
+ if(timer_usec >= main_time + 1000)
+ {
+ main_time = timer_usec;
+
+ //dbg_print_time();
+
+// fsm_lift.act_pos = adc_val[0];
+
+ run_fsm(&fsm_lift);
+ }
+
+ if (timer_msec >= status_time + 100 || //repeat sending message every 100 ms
+ fsm_lift.trigger_can_send) { //or when something important happen
+ fsm_lift.trigger_can_send = false;
+ status_time = timer_msec; //save new time, when message was sent
+ can_send_status();
+ }
+
+ fsm_lift.act_pos = irc_read_tick();
+
+ start_button();
+ robot_switches_handler();
+ lift_switches_handler();
+ blink_led();
+ }
+}
+
+/** @} */
projects / eurobot / public.git / commitdiff