add bumper can msg def, add bumper funcionality to jaws, not pin to bumper asigment...

diff --git a/src/eb_jaws_11/Makefile b/src/eb_jaws_11/Makefile
new file mode 100644 (file)
index 0000000..08cf5ff
--- /dev/null
+++ b/src/eb_jaws_11/Makefile
@@ -0,0 +1,14 @@
+# Generic directory or leaf node makefile for OCERA make framework
+
+ifndef MAKERULES_DIR
+MAKERULES_DIR := $(shell ( old_pwd="" ;  while [ ! -e Makefile.rules ] ; do if [ "$$old_pwd" = `pwd`  ] ; then exit 1 ; else old_pwd=`pwd` ; cd -L .. 2>/dev/null ; fi ; done ; pwd ) )
+endif
+
+ifeq ($(MAKERULES_DIR),)
+all : default
+.DEFAULT::
+       @echo -e "\nThe Makefile.rules has not been found in this or partent directory\n"
+else   
+include $(MAKERULES_DIR)/Makefile.rules
+endif
+

diff --git a/src/eb_jaws_11/Makefile.omk b/src/eb_jaws_11/Makefile.omk
new file mode 100644 (file)
index 0000000..180b038
--- /dev/null
+++ b/src/eb_jaws_11/Makefile.omk
@@ -0,0 +1,8 @@
+# -*- makefile -*-
+
+bin_PROGRAMS = eb_vidle
+
+eb_vidle_SOURCES = main.c fsm.c fsm_vidle.c uar.c vhn.c
+eb_vidle_LIBS = can ebb
+
+link_VARIANTS = flash

diff --git a/src/eb_jaws_11/def.h b/src/eb_jaws_11/def.h
new file mode 100644 (file)
index 0000000..9bd2d67
--- /dev/null
+++ b/src/eb_jaws_11/def.h
@@ -0,0 +1,7 @@
+#ifndef DEFH
+#define DEFH
+
+extern volatile uint32_t timer_msec;
+extern volatile uint32_t timer_usec;  /* incremented by 10 @100kHz */
+
+#endif

diff --git a/src/eb_jaws_11/fsm.c b/src/eb_jaws_11/fsm.c
new file mode 100644 (file)
index 0000000..4a306a5
--- /dev/null
+++ b/src/eb_jaws_11/fsm.c
@@ -0,0 +1,19 @@
+#include "fsm.h"
+
+void init_fsm(struct fsm *fsm, state_fcn initial_state)
+{
+       fsm->current_state = initial_state;
+       fsm->current_state(fsm, EVENT_ENTRY);
+}
+
+void run_fsm(struct fsm *fsm){
+       fsm->last_state = fsm->current_state;           // set actual state
+       fsm->current_state(fsm, EVENT_DO);              // change parameter
+       
+       if(fsm->last_state != fsm->current_state){      // if state was changed
+               fsm->last_state(fsm, EVENT_EXIT);       // finish the old state
+               fsm->current_state(fsm, EVENT_ENTRY);   // initialize the new state
+       }
+}
+
+

diff --git a/src/eb_jaws_11/fsm.h b/src/eb_jaws_11/fsm.h
new file mode 100644 (file)
index 0000000..56e59de
--- /dev/null
+++ b/src/eb_jaws_11/fsm.h
@@ -0,0 +1,37 @@
+#ifndef FSM_H
+#define FSM_H
+
+#include <types.h>
+#include <stdbool.h>
+
+// events of each state of state machine
+enum event {
+       EVENT_ENTRY,
+       EVENT_DO,
+       EVENT_EXIT
+}; 
+
+struct fsm;
+
+typedef void (*state_fcn)(struct fsm *fsm, enum event my_event);//pointer to function returning void and two input parametr
+
+struct fsm {
+       state_fcn current_state;                // current state
+       state_fcn last_state;                   // last state
+       int32_t act_pos;                        // actual position
+       int32_t req_pos;                        // requested position
+       int32_t req_spd;
+       int32_t req_target;
+       volatile int32_t can_req_spd;
+       volatile uint32_t can_req_position;     // next requested position
+       int32_t start_pos;
+       uint32_t can_response;                  // when the move is done, the value here equals to the req_pos
+       uint8_t flags;   //< CAN flags bits (defined in can_msg_def.h)
+       uint32_t time_start;    /* For timeout detection */
+       bool trigger_can_send;
+};
+
+void init_fsm(struct fsm *fsm, state_fcn initial_state);
+void run_fsm(struct fsm *fsm);
+
+#endif

diff --git a/src/eb_jaws_11/fsm_vidle.c b/src/eb_jaws_11/fsm_vidle.c
new file mode 100644 (file)
index 0000000..c9b5cd4
--- /dev/null
+++ b/src/eb_jaws_11/fsm_vidle.c
@@ -0,0 +1,145 @@
+#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);
+
+
+void fsm_vidle_init(struct fsm *fsm, enum event event)
+{
+       switch (event) {
+       case EVENT_ENTRY:
+               DBG_ENTRY();
+               fsm->can_req_position = fsm->act_pos;
+               fsm->flags |= CAN_VIDLE_INITIALIZING;
+               break;
+       case EVENT_DO:
+               /* TODO: Homing */
+               fsm->flags &= ~CAN_VIDLE_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) / 10;                // ORIGINAL: int action = P*e;
+
+       engine_A_dir(action < 0);
+//#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:
+               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;
+       bool finished;
+       switch (event) {
+       case EVENT_ENTRY:
+               counter = 0;
+               DBG_ENTRY();
+               fsm->time_start = timer_msec;
+               fsm->start_pos = fsm->act_pos;
+               if(fsm->req_spd == 0)
+                       fsm->req_pos = fsm->req_target;
+               else
+                       fsm->req_pos = fsm->start_pos;
+               break;
+       case EVENT_DO:
+               if (fsm->can_req_position != fsm->req_target) {
+                       fsm->flags |= CAN_VIDLE_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 ? 1000 : 3000)) {
+                       fsm->flags |= CAN_VIDLE_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;
+       }
+}

diff --git a/src/eb_jaws_11/main.c b/src/eb_jaws_11/main.c
new file mode 100644 (file)
index 0000000..2b89c76
--- /dev/null
+++ b/src/eb_jaws_11/main.c
@@ -0,0 +1,450 @@
+
+/**
+ * @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>
+#include "vhn.h"
+
+#define        CAN_SPEED       1000000         //< CAN bus speed
+#define CAN_ISR                0
+
+#define ADC_ISR                1
+
+#define TIMER_IRQ_PRIORITY     5
+
+
+struct fsm fsm_jaws;
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+
+
+
+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
+
+       init_engine_B();                        // initialization of PWM unit
+       engine_B_en(ENGINE_EN_ON);              //enable motor B
+       engine_B_dir(ENGINE_DIR_FW);            //set direction 
+       engine_B_pwm(0);                        // STOP pwm is in percent, range 0~100~200
+
+       /*      vhn_init(); */
+}
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+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;
+}
+
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+// #define START_PIN   15              // start pin
+// #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) */
+
+
+// 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 */
+//     }
+// 
+//     if (timer_msec >= next_send) {
+//             msg.id = CAN_ROBOT_CMD;
+//             msg.flags = 0;
+//             msg.dlc = 1;
+//             msg.data[0] = start_condition;
+//             /*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_VIDLE_CMD:
+                       deb_led_on(LEDB);
+                       req = ((rx_msg.data[0]<<8) | (rx_msg.data[1]));
+                       spd = rx_msg.data[2];
+                       
+                       if (req >= 0x150 && req <= 0x3e0) {
+                               fsm_jaws.flags &= ~CAN_JAWS_OUT_OF_BOUNDS;
+                               fsm_jaws.can_req_position = req;// save new req position of lift
+                               fsm_jaws.can_req_spd = spd;// save new req spd of lift
+                       } else
+                               fsm_jaws.flags |= CAN_JAWS_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_JAWS_STATUS;
+       tx_msg.dlc = 5;
+       tx_msg.flags = 0;
+       tx_msg.data[0] = (fsm_jaws.act_pos  >> 8) & 0xFF;
+       tx_msg.data[1] = fsm_jaws.act_pos & 0xFF;
+       tx_msg.data[2] = (fsm_jaws.can_response  >> 8) & 0xFF;
+       tx_msg.data[3] = fsm_jaws.can_response & 0xFF;
+       tx_msg.data[4] = fsm_jaws.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_jaws_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_vidle.can_req_position = 0x380;
+                   if (up == 6)
+                   fsm_vidle.can_req_position = 0x1e0;
+                   up = (up+1)%12;
+               */
+               deb_led_change(LEDG);
+               send_rs_int(fsm_jaws.act_pos);
+               send_rs_str("\n");
+       }
+}
+
+
+// #define BUMPER_PIN  17              // bumper pin  (SCK1/P0_17)
+// #define COLOR_PIN   3               // change color of dress pin  (SDA1/P0_3)
+// 
+// #define BUMPER_LEFT 19      // left bumper MOSI1/P0_19
+// #define BUMPER_RIGHT 9              // right bumper RXD1/P0_9       
+
+#define BUMPER_REAR    17              // rear bumper in the lift  (SCK1/P0_17)
+
+#define BUMPER_LEFT 19                         // left bumper MOSI1/P0_19
+#define BUMPER_RIGHT 9                 // right bumper RXD1/P0_9       
+
+#define BUMPER_LEFT_CORNER 16                  // left bumper in rear corner 
+#define BUMPER_RIGHT_CORNER 13                 // right bumper in rear corner
+
+void handle_bumper()
+{
+       static uint32_t bumper_time = 0;
+       char sw = 0;
+
+       if (timer_msec >= bumper_time + 100)    
+       {
+               can_msg_t tx_msg;
+
+               bumper_time = timer_msec;
+               
+                       
+                       
+               if (IO0PIN & (1<<BUMPER_REAR))
+                       sw &= ~CAN_SWITCH_BUMPER;
+               else
+                       sw |= CAN_SWITCH_BUMPER;
+
+               if (IO0PIN & (1<<BUMPER_LEFT_CORNER))
+                       sw |= CAN_SWITCH_CORNER_LEFT;
+               else
+                       sw &= ~CAN_SWITCH_CORNER_LEFT;
+               
+               if (IO0PIN & (1<<BUMPER_RIGHT_CORNER))
+                       sw |= CAN_SWITCH_CORNER_RIGHT;
+               else
+                       sw &= ~CAN_SWITCH_CORNER_RIGHT;
+
+               if (IO0PIN & (1<<BUMPER_LEFT))
+                       sw &= ~CAN_SWITCH_LEFT;
+               else
+                       sw |= CAN_SWITCH_LEFT;
+
+               if (IO0PIN & (1<<BUMPER_RIGHT))
+                       sw &= ~CAN_SWITCH_RIGHT;
+               else
+                       sw |= CAN_SWITCH_RIGHT;
+
+//             if (sw & CAN_SWITCH_COLOR)
+//                     deb_led_off(LEDY);
+//             else
+//                     deb_led_on(LEDY);
+               
+               if (sw & (CAN_SWITCH_BUMPER|CAN_SWITCH_LEFT|CAN_SWITCH_RIGHT|BUMPER_RIGHT_CORNER|BUMPER_LEFT_CORNER))
+                       deb_led_on(LEDR);
+               else
+                       deb_led_off(LEDR);
+               
+               /* 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);
+       }
+}
+
+// void test_vhn()
+// {
+//     vhn_init();
+//     
+//     char dir = ENGINE_DIR_BW;
+//     int spd = 0;
+//     while(1) {
+//             spd = (spd+1)%100;
+//             if (spd == 0) {
+//                     dir = !dir;
+//                     vhn_reset();
+//             }
+//             vhn_speed(spd, dir);
+//             send_rs_int(spd);
+//             uart_send_char(' ');
+//             send_rs_int(dir);
+//             send_rs_str(" Vidle started\n");
+// /*          char ch = uart_get_char(); */
+// /*          switch (ch) { */
+// /*          case '-': */
+// /*                  dir = !dir; */
+// /*                  break; */
+// /*          case '0': */
+// /*                  vhn_speed(0, dir); */
+// /*                  break; */
+// /*          case '1': */
+// /*                  vhn_speed(0x40, dir); */
+// /*                  break; */
+// /*          case '2': */
+// /*                  vhn_speed(0x80, dir); */
+// /*                  break; */
+// /*          case '3': */
+// /*                  vhn_speed(0xa0, dir); */
+// /*                  break; */
+// /*          default: */
+// /*                  ch='?'; */
+// /*          } */
+// /*          uart_send_char(ch); */
+// //          blink_led();
+//     }
+// 
+// }
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+int main(void)
+{
+       uint32_t main_time = timer_usec;
+       uint32_t status_time = timer_usec;
+       
+
+       init_periphery();
+       
+       /* TODO: Add comment FIXME: zkusit smazat, mam moct ze to melo neco spojeneho s chelae z eb09  */
+       //SET_PIN(PINSEL1, 1, PINSEL_0);
+       //SET_PIN(PINSEL1, 3, PINSEL_0);
+       
+       
+       
+       
+//     SET_PIN(PINSEL0, START_PIN, PINSEL_0);          // inicializace start pinu
+       SET_PIN(PINSEL0, COLOR_PIN, PINSEL_0);
+       SET_PIN(PINSEL1, 1, PINSEL_0);          // inicializace bumper pinu (FIXME SET_PIN je BLBA implemetace, musim ji nekdy opravit)
+
+       SET_PIN(PINSEL1, 3, PINSEL_0); 
+       SET_PIN(PINSEL0, BUMPER_RIGHT, PINSEL_0); 
+
+       
+//     IO0DIR &= ~((1<<START_PIN) | (1<<BUMPER_RIGHT) | (1 << COLOR_PIN));
+       IO0DIR &= ~((1<<BUMPER_PIN) | (1<<BUMPER_LEFT));
+       
+       //IO1DIR &= ~(3<<20);
+
+       send_rs_str("Jaws started\n");
+       // The above send_rs_str is importat - we wait for the first AD conversion to be finished
+       fsm_jaws
+.act_pos = adc_val[0];
+       init_fsm(&fsm_jaws
+, &fsm_jaws_init
+);
+
+/*     test_vhn(); */
+/*     return; */
+       
+       while(1){
+               if(timer_usec >= main_time + 1000)
+               {
+                       main_time = timer_usec;
+
+                       //dbg_print_time();
+
+                       fsm_jaws
+.act_pos = adc_val[0];
+                       
+                   
+                       run_fsm(&fsm_jaws
+);
+               }
+
+               if (timer_msec >= status_time + 100 || //repeat sending message every 100 ms
+                   fsm_jaws
+.trigger_can_send) {   //or when something important happen
+                       fsm_jaws
+.trigger_can_send = false;
+                       status_time = timer_msec; //save new time, when message was sent
+                       can_send_status();
+
+               }
+
+//             start_button();
+               handle_bumper();
+
+               blink_led();
+       }
+}
+
+/** @} */

diff --git a/src/eb_jaws_11/uar.c b/src/eb_jaws_11/uar.c
new file mode 100644 (file)
index 0000000..f30a022
--- /dev/null
+++ b/src/eb_jaws_11/uar.c
@@ -0,0 +1,82 @@
+#include <uart.h>
+
+
+
+
+/**
+ *     Send one char to uart.
+ */
+void uart_send_char(char val)
+{
+       uart0SendCh(val);
+}
+
+/**
+ *     Read one char from uart.
+ */
+char uart_get_char(void)
+{
+       return uart0GetCh();
+}
+
+/**
+ *     Initialize UART - platform dependent
+ */
+void init_uart(void)
+{
+       init_uart0((int)9600, UART_BITS_8, UART_STOP_BIT_1, UART_PARIT_OFF, 0 );
+}
+
+
+/**
+ *     Send string to serial output in ASCII code. .
+ *     @param data[]   string to print
+ */
+void send_rs_str(const char data[])
+{
+       
+       int i = 0;
+       int j = 0;
+       
+       for (j = 0; j < 255; j++)
+       {
+               if(data[j] == 0) break;
+       }
+               
+       for (i= 0 ; i < j; i++)
+       {
+               uart_send_char(data[i]);
+       }
+}
+
+/**
+ *     Send int value to serial output in ASCII code. Removes unused zeros.
+ *     @param val      value to print
+ */
+void send_rs_int(int val)
+{
+       char dat[8];
+       int i;
+       int pom = 0;
+       
+       for(i = 0; i < 8; i++)
+       {
+               dat[i] = (val & 0xF) + 0x30;
+               if(dat[i] > '9')
+                       dat[i] += 7;
+               val >>= 4;
+       }
+       
+       for(i = 0; i < 8; i++)
+       {
+               if((pom == 0) & (dat[7-i] == '0')) 
+               {
+                       if((i == 6) | (i == 7))
+                               uart_send_char('0');            
+                       continue;
+               }
+               pom = 1;
+               uart_send_char(dat[7-i]);
+       }
+       
+}

diff --git a/src/eb_jaws_11/uar.h b/src/eb_jaws_11/uar.h
new file mode 100644 (file)
index 0000000..3f9f41c
--- /dev/null
+++ b/src/eb_jaws_11/uar.h
@@ -0,0 +1,13 @@
+#ifndef _UAR_H
+#define _UAR_H
+
+
+void send_rs_int(int val);
+void send_rs_str(const char data[]);
+void init_uart(void);
+char uart_get_char(void);
+void uart_send_char(char val);
+
+
+#endif
+

diff --git a/src/eb_jaws_11/vhn.c b/src/eb_jaws_11/vhn.c
new file mode 100644 (file)
index 0000000..43f4837
--- /dev/null
+++ b/src/eb_jaws_11/vhn.c
@@ -0,0 +1,56 @@
+#include <lpc21xx.h>                            /* LPC21xx definitions */
+#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 "def.h"
+#include "vhn.h"
+
+
+void vhn_init(void)
+{
+       init_engine_B();
+       PINSEL2 &= ~(3<<2);
+       IO1DIR |= (VHN_EN_A | VHN_EN_B | VHN_IN_A | VHN_IN_B);
+       IO1SET = (VHN_EN_A | VHN_EN_B | VHN_IN_A | VHN_IN_B);
+}
+
+void vhn_speed(uint8_t speed, uint8_t dir) {
+  
+    if(speed == 0)
+    {
+      IO1CLR = (VHN_IN_A | VHN_IN_B);
+      return;
+    }
+  
+    if(dir == ENGINE_DIR_FW)
+    {
+       IO1SET = VHN_IN_A;
+       IO1CLR = VHN_IN_B;
+    }
+    else
+    {
+       IO1CLR = VHN_IN_A;
+       IO1SET = VHN_IN_B;
+    }
+    
+    //IO1CLR = (VHN_EN_A | VHN_EN_A);
+    IO1SET = (VHN_EN_A | VHN_EN_A);
+    engine_B_pwm(100-speed);
+      
+}
+
+void vhn_reset()
+{
+       volatile int i = 10000;
+       deb_led_on(LEDR);
+       IO1CLR = VHN_IN_A|VHN_IN_B;
+       while(i--);
+       //while (IO1PIN & (VHN_EN_A | VHN_EN_B) != (VHN_EN_A | VHN_EN_B));
+       IO1SET = VHN_IN_A|VHN_IN_B;
+       deb_led_off(LEDR);
+}

diff --git a/src/eb_jaws_11/vhn.h b/src/eb_jaws_11/vhn.h
new file mode 100644 (file)
index 0000000..2197284
--- /dev/null
+++ b/src/eb_jaws_11/vhn.h
@@ -0,0 +1,22 @@
+/* VHN2SP30 H-bridge control */
+
+#ifndef  __VHN_H
+#define  __VHN_H
+
+  #include "def.h"
+
+  #define VHN_EN_A             (1<<16) /* P1.16 = GPIO1 = GPIO port 2 */
+  #define VHN_EN_B             (1<<17) /* P1.17 = GPIO2 = GPIO port 1 */
+  #define VHN_IN_A             (1<<18) /* P1.18 = GPIO3 = GPIO port 4 */
+  #define VHN_IN_B             (1<<19) /* P1.19 = GPIO4 = GPIO port 3 */
+
+
+    void vhn_init(void);
+    void vhn_reset(void);
+    void vhn_speed(uint8_t speed, uint8_t dir);
+
+       
+       
+
+
+#endif /*__VHN_H*/