src/eb_test/main.c

   1
   2 /**
   3  * @file main.c
   4  *
   5  *
   6  * @author Bc. Jiri Kubias, jiri.kubias@gmail.com
   7  * @author Michal Sojka <sojkam1@fel.cvut.cz>
   8  *
   9  * @addtogroup fork
  10  */
  11
  12
  13 /**
  14  * @defgroup fork Vidle (fork) application
  15  */
  16 /**
  17  * @ingroup fork
  18  * @{
  19  */
  20
  21
  22 #include <lpc21xx.h>                            /* LPC21xx definitions */
  23 #include <types.h>
  24 #include <deb_led.h>
  25 #include <system_def.h>
  26 #include <can_ids.h>
  27 #include <periph/can.h>
  28 #include <string.h>
  29 #include <deb_led.h>
  30 #include <engine.h>
  31 #include "uar.h"
  32 #include <can_msg_def.h>
  33 #include "fsm.h"
  34 #include "def.h"
  35 #include <adc.h>
  36 #include <expansion.h>
  37
  38 #define CAN_SPEED       1000000         //< CAN bus speed
  39 #define CAN_ISR         0
  40
  41 #define ADC_ISR         1
  42
  43 #define TIMER_IRQ_PRIORITY      5
  44
  45
  46 struct fsm fsm_vidle;
  47
  48 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  49 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~l~~~~~~~~~~~~~~~~~
  50 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  51
  52
  53
  54
  55 void init_motors(void){
  56
  57         init_engine_A();                        // initialization of PWM unit
  58         engine_A_en(ENGINE_EN_ON);              //enable motor A
  59         engine_A_dir(ENGINE_DIR_FW);            //set direction
  60         engine_A_pwm(0);                        // STOP pwm is in percent, range 0~100~200
  61 /*      vhn_init(); */
  62 }
  63
  64 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  65 void set_irq_handler(uint8_t source, uint8_t irq_vect, void (*handler)()) {
  66         /* set interrupt vector */
  67         ((uint32_t*)&VICVectAddr0)[irq_vect] = (uint32_t)handler;
  68         ((uint32_t*)&VICVectCntl0)[irq_vect] = 0x20 | source;
  69         /* enable interrupt */
  70         VICIntEnable = 1<<source;
  71 }
  72
  73 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  74 /** timer0 & ISR **/
  75
  76 void timer0_irq() __attribute__((interrupt));
  77 volatile uint32_t timer_msec = 0, timer_usec = 0;
  78
  79 void init_timer0(uint32_t prescale, uint32_t period) {
  80         T0TCR = 0x2; /* reset */
  81         T0PR = prescale - 1;
  82         T0MR0 = period;
  83         T0MCR = 0x3; /* match0: reset & irq */
  84         T0EMR = 0; /* no ext. match */
  85         T0CCR = 0x0; /* no capture */
  86         T0TCR = 0x1; /* go! */
  87 }
  88
  89 void timer0_irq() {
  90         static unsigned cnt1khz = 0;
  91
  92         /* reset timer irq */
  93         T0IR = -1;
  94
  95         /* increment timer_usec */
  96         timer_usec += 10;
  97         /* increment msec @1kHz */
  98         if (++cnt1khz == 100) {
  99                 cnt1khz = 0;
 100                 ++timer_msec;
 101         }
 102
 103         /* int acknowledge */
 104         VICVectAddr = 0;
 105 }
 106
 107
 108 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 109
 110 #define START_PIN       15              // start pin
 111 #define BUTTON_1_PIN    EXPPORT_1
 112 #define BUTTON_2_PIN    EXPPORT_3
 113 #define START_SEND_PRIOD_FAST   50      /* [miliseconds] */
 114 #define START_SEND_PRIOD_SLOW   300     /* [miliseconds] */
 115 #define START_SEND_FAST_COUNT   10      /* How many times to send start with small period (after a change) */
 116
 117 #if 0
 118 void start_button(void)
 119 {
 120         can_msg_t msg;
 121         bool start_condition;
 122         static bool last_start_condition = 0;
 123
 124         static int count = 0;
 125         static uint32_t next_send = 0;
 126
 127
 128         start_condition = (IO0PIN & (1<<START_PIN)) == 0;
 129
 130         if (start_condition != last_start_condition) {
 131                 last_start_condition = start_condition;
 132                 count = 0;
 133                 next_send = timer_msec; /* Send now */
 134         }
 135
 136         if (timer_msec >= next_send) {
 137                 msg.id = CAN_ROBOT_CMD;
 138                 msg.flags = 0;
 139                 msg.dlc = 1;
 140                 msg.data[0] = start_condition;
 141                 /*while*/ (can_tx_msg(&msg));
 142
 143                 if (count < START_SEND_FAST_COUNT) {
 144                         count++;
 145                         next_send = timer_msec + START_SEND_PRIOD_FAST;
 146                 } else
 147                         next_send = timer_msec + START_SEND_PRIOD_SLOW;
 148         }
 149
 150
 151 }
 152
 153 #endif
 154 #if 0
 155 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 156 void CAN_rx(can_msg_t *msg) {
 157         uint32_t spd;
 158         can_msg_t rx_msg;
 159         uint32_t req =0;
 160         memcpy(&rx_msg, msg, sizeof(can_msg_t));//make copy of message
 161
 162
 163         deb_led_on(LEDB);
 164
 165         switch (rx_msg.id)
 166         {
 167                 case CAN_VIDLE_CMD:
 168                         deb_led_on(LEDB);
 169                         req = ((rx_msg.data[0]<<8) | (rx_msg.data[1]));
 170                         spd = rx_msg.data[2];
 171
 172                         if (req >= 0x150 && req <= 0x3e0) {
 173                                 fsm_vidle.flags &= ~CAN_VIDLE_OUT_OF_BOUNDS;
 174                                 fsm_vidle.can_req_position = req;// save new req position of lift
 175                                 fsm_vidle.can_req_spd = spd;// save new req spd of lift
 176                         } else
 177                                 fsm_vidle.flags |= CAN_VIDLE_OUT_OF_BOUNDS;
 178                 break;
 179                 default:break;
 180         }
 181
 182         deb_led_off(LEDB);
 183 }
 184
 185 #endif
 186 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 187 void init_periphery(void){
 188
 189 //      can_init_baudrate(CAN_SPEED, CAN_ISR, CAN_rx);//initialization of CAN bus
 190         init_motors();
 191
 192         /* init timer0 */
 193         init_timer0(1, CPU_APB_HZ/100000);
 194         set_irq_handler(4 /*timer0*/, TIMER_IRQ_PRIORITY, timer0_irq);
 195
 196         init_uart();
 197         init_adc(ADC_ISR);
 198
 199
 200 }
 201 /*********************************************************/
 202 #if 0
 203 void can_send_status(void)
 204 {
 205         can_msg_t tx_msg;
 206         tx_msg.id = CAN_VIDLE_STATUS;
 207         tx_msg.dlc = 5;
 208         tx_msg.flags = 0;
 209         tx_msg.data[0] = (fsm_vidle.act_pos  >> 8) & 0xFF;
 210         tx_msg.data[1] = fsm_vidle.act_pos & 0xFF;
 211         tx_msg.data[2] = (fsm_vidle.can_response  >> 8) & 0xFF;
 212         tx_msg.data[3] = fsm_vidle.can_response & 0xFF;
 213         tx_msg.data[4] = fsm_vidle.flags;
 214         /*while*/(can_tx_msg(&tx_msg)); /* CAN erratum workaround */
 215 }
 216 #endif
 217 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 218 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 219
 220 void dbg_print_time()
 221 {
 222         char str[10];
 223         unsigned t = timer_usec, i;
 224         memset(str, ' ', sizeof(str));
 225         str[9] = 0;
 226         str[8] = '\n';
 227         for (i=7; t > 0; i--) {
 228                 str[i] = t%10 + '0';
 229                 t /= 10;
 230         }
 231         send_rs_str(str);
 232 }
 233
 234 void fsm_vidle_init(struct fsm *fsm, enum event event);
 235
 236
 237 void blink_led()
 238 {
 239         static uint32_t led_time = 0;
 240
 241         if(timer_msec >= led_time + 500)
 242         {
 243                 led_time = timer_msec;
 244                 /*  static int up;
 245                     if (up == 0)
 246                     fsm_vidle.can_req_position = 0x380;
 247                     if (up == 6)
 248                     fsm_vidle.can_req_position = 0x1e0;
 249                     up = (up+1)%12;
 250                 */
 251                 deb_led_change(LEDG);
 252                 send_rs_int(fsm_vidle.act_pos);
 253                 send_rs_str("\n");
 254         }
 255 }
 256
 257
 258 #define BUMPER_PIN      17              // bumper pin  (SCK1/P0_17)
 259 #define COLOR_PIN       3               // change color of dress pin  (SDA1/P0_3)
 260
 261 #define BUMPER_LEFT 19  // left bumper MOSI1/P0_19
 262 #define BUMPER_RIGHT 9          // right bumper RXD1/P0_9
 263
 264 #if 0
 265 void handle_bumper()
 266 {
 267         static uint32_t bumper_time = 0;
 268         char sw = 0;
 269
 270         if (timer_msec >= bumper_time + 100)
 271         {
 272                 can_msg_t tx_msg;
 273
 274                 bumper_time = timer_msec;
 275
 276
 277
 278                 if (IO0PIN & (1<<BUMPER_PIN))
 279                         sw &= ~CAN_SWITCH_BUMPER;
 280                 else
 281                         sw |= CAN_SWITCH_BUMPER;
 282
 283                 if (IO0PIN & (1<<COLOR_PIN))
 284                         sw |= CAN_SWITCH_COLOR;
 285                 else
 286                         sw &= ~CAN_SWITCH_COLOR;
 287
 288                 if (IO0PIN & (1<<BUMPER_LEFT))
 289                         sw &= ~CAN_SWITCH_LEFT;
 290                 else
 291                         sw |= CAN_SWITCH_LEFT;
 292
 293                 if (IO0PIN & (1<<BUMPER_RIGHT))
 294                         sw &= ~CAN_SWITCH_RIGHT;
 295                 else
 296                         sw |= CAN_SWITCH_RIGHT;
 297
 298                 if (sw & CAN_SWITCH_COLOR)
 299                         deb_led_off(LEDY);
 300                 else
 301                         deb_led_on(LEDY);
 302
 303                 if (sw & (CAN_SWITCH_BUMPER|CAN_SWITCH_LEFT|CAN_SWITCH_RIGHT))
 304                         deb_led_on(LEDR);
 305                 else
 306                         deb_led_off(LEDR);
 307
 308                 /* TODO: Put color to the second bit */
 309
 310                 tx_msg.id = CAN_ROBOT_SWITCHES;
 311                 tx_msg.dlc = 1;
 312                 tx_msg.flags = 0;
 313                 tx_msg.data[0] = sw;
 314
 315                 can_tx_msg(&tx_msg);
 316         }
 317 }
 318 #endif
 319
 320 void dummy_wait()
 321 {
 322         volatile unsigned int wait = 5000000;
 323         while(--wait);
 324 }
 325 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 326 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 327 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 328 int main(void)
 329 {
 330         uint32_t main_time = timer_usec;
 331         uint32_t status_time = timer_usec;
 332         can_msg_t tx_msg;
 333         bool obrat=true;
 334           bool button_status[3];
 335         int i=0,delta=0,j=0,suma=0;
 336
 337
 338         init_periphery();
 339
 340         /* TODO: Add comment FIXME: zkusit smazat, mam moct ze to melo neco spojeneho s chelae z eb09  */
 341         //SET_PIN(PINSEL1, 1, PINSEL_0);
 342         //SET_PIN(PINSEL1, 3, PINSEL_0);
 343
 344
 345
 346
 347         SET_PIN(PINSEL0, START_PIN, PINSEL_0);          // inicializace start pinu
 348         SET_PIN(PINSEL0, COLOR_PIN, PINSEL_0);
 349         SET_PIN(PINSEL1, 1, PINSEL_0);          // inicializace bumper pinu (FIXME SET_PIN je BLBA implemetace, musim ji nekdy opravit)
 350         SET_PIN(PINSEL0,BUTTON_1_PIN,PINSEL_0);
 351         SET_PIN(PINSEL0,BUTTON_2_PIN,PINSEL_0);
 352         SET_PIN(PINSEL1, 3, PINSEL_0);
 353         SET_PIN(PINSEL0, BUMPER_RIGHT, PINSEL_0);
 354         SET_PIN(PINSEL0, BUMPER_RIGHT, PINSEL_0);
 355
 356
 357         IO0DIR &= ~((1<<START_PIN) | (1<<BUMPER_RIGHT) | (1 << COLOR_PIN));
 358
 359         IO0DIR &= ~((1<<BUMPER_PIN) | (1<<BUMPER_LEFT));  //first shift one on the right pin
 360         IO0DIR &= ~((1<<BUTTON_1_PIN) | (1<<BUTTON_2_PIN)); //then invert, cause 0 is desired input
 361
 362
 363
 364
 365
 366         send_rs_str("Vidle started\n");
 367
 368         // The above send_rs_str is importat - we wait for the first AD conversion to be finished
 369         fsm_vidle.act_pos = adc_val[0];
 370           button_status[1] = (IO0PIN & (1<<BUTTON_1_PIN)) == 0;  //reading value (inspiration from START_PIN)
 371           button_status[2] = (IO0PIN & (1<<BUTTON_2_PIN)) == 0;
 372           fsm_vidle.button1=button_status[1];
 373           fsm_vidle.button2=button_status[2];
 374         init_fsm(&fsm_vidle, &fsm_vidle_init);
 375         engine_A_dir(ENGINE_DIR_FW);
 376 /*      test_vhn(); */
 377 /*      return; */
 378
 379
 380         while(1){
 381
 382
 383
 384                 if(timer_usec >= main_time + 1000)
 385                 {
 386                         main_time = timer_usec;
 387
 388                         //dbg_print_time();
 389                         fsm_vidle.last_pos = fsm_vidle.act_pos;
 390                         fsm_vidle.act_pos = adc_val[0];
 391                         fsm_vidle.delta=fsm_vidle.act_pos-fsm_vidle.last_pos;
 392                         button_status[1] = (IO0PIN & (1<<BUTTON_1_PIN)) == 0;  //reading value (inspiration from START_PIN)
 393                         button_status[2] = (IO0PIN & (1<<BUTTON_2_PIN)) == 0;
 394                 //        if (button_status[1]) { send_rs_str("pozadavek na  uzavreni zavory"); send_rs_str("\n");}
 395                 //            else { send_rs_str("pozadavek na  otevreni zavory"); send_rs_str("\n");}
 396                   //      if (button_status[2]) { send_rs_str("pozadavek na ignoraci prekazek"); send_rs_str("\n");}
 397                         //      else { send_rs_str("pozadavek na uhybani prekazkam"); send_rs_str("\n");}
 398                         fsm_vidle.button1=button_status[1];
 399                         fsm_vidle.button2=button_status[2];
 400                         run_fsm(&fsm_vidle);
 401
 402                 }
 403
 404                 if (timer_msec >= status_time + 100 || //repeat sending message every 100 ms
 405                     fsm_vidle.trigger_can_send) {   //or when something important happen
 406                         fsm_vidle.trigger_can_send = false;
 407                         status_time = timer_msec; //save new time, when message was sent
 408                 //      can_send_status();
 409
 410                 }
 411
 412                 //start_button();
 413                 //handle_bumper();
 414
 415
 416
 417
 418
 419
 420
 421
 422
 423
 424
 425         }
 426 }
 427
 428 /** @} */