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 "vhn.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 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  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 START_SEND_PRIOD_FAST   50      /* [miliseconds] */
 112 #define START_SEND_PRIOD_SLOW   300     /* [miliseconds] */
 113 #define START_SEND_FAST_COUNT   10      /* How many times to send start with small period (after a change) */
 114
 115 #if 0
 116 void start_button(void)
 117 {
 118         can_msg_t msg;
 119         bool start_condition;
 120         static bool last_start_condition = 0;
 121
 122         static int count = 0;
 123         static uint32_t next_send = 0;
 124
 125
 126         start_condition = (IO0PIN & (1<<START_PIN)) == 0;
 127
 128         if (start_condition != last_start_condition) {
 129                 last_start_condition = start_condition;
 130                 count = 0;
 131                 next_send = timer_msec; /* Send now */
 132         }
 133
 134         if (timer_msec >= next_send) {
 135                 msg.id = CAN_ROBOT_CMD;
 136                 msg.flags = 0;
 137                 msg.dlc = 1;
 138                 msg.data[0] = start_condition;
 139                 /*while*/ (can_tx_msg(&msg));
 140
 141                 if (count < START_SEND_FAST_COUNT) {
 142                         count++;
 143                         next_send = timer_msec + START_SEND_PRIOD_FAST;
 144                 } else
 145                         next_send = timer_msec + START_SEND_PRIOD_SLOW;
 146         }
 147
 148
 149 }
 150
 151 #endif
 152 #if 0
 153 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 154 void CAN_rx(can_msg_t *msg) {
 155         uint32_t spd;
 156         can_msg_t rx_msg;
 157         uint32_t req =0;
 158         memcpy(&rx_msg, msg, sizeof(can_msg_t));//make copy of message
 159
 160
 161         deb_led_on(LEDB);
 162
 163         switch (rx_msg.id)
 164         {
 165                 case CAN_VIDLE_CMD:
 166                         deb_led_on(LEDB);
 167                         req = ((rx_msg.data[0]<<8) | (rx_msg.data[1]));
 168                         spd = rx_msg.data[2];
 169
 170                         if (req >= 0x150 && req <= 0x3e0) {
 171                                 fsm_vidle.flags &= ~CAN_VIDLE_OUT_OF_BOUNDS;
 172                                 fsm_vidle.can_req_position = req;// save new req position of lift
 173                                 fsm_vidle.can_req_spd = spd;// save new req spd of lift
 174                         } else
 175                                 fsm_vidle.flags |= CAN_VIDLE_OUT_OF_BOUNDS;
 176                 break;
 177                 default:break;
 178         }
 179
 180         deb_led_off(LEDB);
 181 }
 182
 183 #endif
 184 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 185 void init_periphery(void){
 186
 187 //      can_init_baudrate(CAN_SPEED, CAN_ISR, CAN_rx);//initialization of CAN bus
 188         init_motors();
 189
 190         /* init timer0 */
 191         init_timer0(1, CPU_APB_HZ/100000);
 192         set_irq_handler(4 /*timer0*/, TIMER_IRQ_PRIORITY, timer0_irq);
 193
 194         init_uart();
 195         init_adc(ADC_ISR);
 196
 197
 198 }
 199 /*********************************************************/
 200 #if 0
 201 void can_send_status(void)
 202 {
 203         can_msg_t tx_msg;
 204         tx_msg.id = CAN_VIDLE_STATUS;
 205         tx_msg.dlc = 5;
 206         tx_msg.flags = 0;
 207         tx_msg.data[0] = (fsm_vidle.act_pos  >> 8) & 0xFF;
 208         tx_msg.data[1] = fsm_vidle.act_pos & 0xFF;
 209         tx_msg.data[2] = (fsm_vidle.can_response  >> 8) & 0xFF;
 210         tx_msg.data[3] = fsm_vidle.can_response & 0xFF;
 211         tx_msg.data[4] = fsm_vidle.flags;
 212         /*while*/(can_tx_msg(&tx_msg)); /* CAN erratum workaround */
 213 }
 214 #endif
 215 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 216 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 217
 218 void dbg_print_time()
 219 {
 220         char str[10];
 221         unsigned t = timer_usec, i;
 222         memset(str, ' ', sizeof(str));
 223         str[9] = 0;
 224         str[8] = '\n';
 225         for (i=7; t > 0; i--) {
 226                 str[i] = t%10 + '0';
 227                 t /= 10;
 228         }
 229         send_rs_str(str);
 230 }
 231
 232 void fsm_vidle_init(struct fsm *fsm, enum event event);
 233
 234
 235 void blink_led()
 236 {
 237         static uint32_t led_time = 0;
 238
 239         if(timer_msec >= led_time + 500)
 240         {
 241                 led_time = timer_msec;
 242                 /*  static int up;
 243                     if (up == 0)
 244                     fsm_vidle.can_req_position = 0x380;
 245                     if (up == 6)
 246                     fsm_vidle.can_req_position = 0x1e0;
 247                     up = (up+1)%12;
 248                 */
 249                 deb_led_change(LEDG);
 250                 send_rs_int(fsm_vidle.act_pos);
 251                 send_rs_str("\n");
 252         }
 253 }
 254
 255
 256 #define BUMPER_PIN      17              // bumper pin  (SCK1/P0_17)
 257 #define COLOR_PIN       3               // change color of dress pin  (SDA1/P0_3)
 258
 259 #define BUMPER_LEFT 19  // left bumper MOSI1/P0_19
 260 #define BUMPER_RIGHT 9          // right bumper RXD1/P0_9
 261
 262 #if 0
 263 void handle_bumper()
 264 {
 265         static uint32_t bumper_time = 0;
 266         char sw = 0;
 267
 268         if (timer_msec >= bumper_time + 100)
 269         {
 270                 can_msg_t tx_msg;
 271
 272                 bumper_time = timer_msec;
 273
 274
 275
 276                 if (IO0PIN & (1<<BUMPER_PIN))
 277                         sw &= ~CAN_SWITCH_BUMPER;
 278                 else
 279                         sw |= CAN_SWITCH_BUMPER;
 280
 281                 if (IO0PIN & (1<<COLOR_PIN))
 282                         sw |= CAN_SWITCH_COLOR;
 283                 else
 284                         sw &= ~CAN_SWITCH_COLOR;
 285
 286                 if (IO0PIN & (1<<BUMPER_LEFT))
 287                         sw &= ~CAN_SWITCH_LEFT;
 288                 else
 289                         sw |= CAN_SWITCH_LEFT;
 290
 291                 if (IO0PIN & (1<<BUMPER_RIGHT))
 292                         sw &= ~CAN_SWITCH_RIGHT;
 293                 else
 294                         sw |= CAN_SWITCH_RIGHT;
 295
 296                 if (sw & CAN_SWITCH_COLOR)
 297                         deb_led_off(LEDY);
 298                 else
 299                         deb_led_on(LEDY);
 300
 301                 if (sw & (CAN_SWITCH_BUMPER|CAN_SWITCH_LEFT|CAN_SWITCH_RIGHT))
 302                         deb_led_on(LEDR);
 303                 else
 304                         deb_led_off(LEDR);
 305
 306                 /* TODO: Put color to the second bit */
 307
 308                 tx_msg.id = CAN_ROBOT_SWITCHES;
 309                 tx_msg.dlc = 1;
 310                 tx_msg.flags = 0;
 311                 tx_msg.data[0] = sw;
 312
 313                 can_tx_msg(&tx_msg);
 314         }
 315 }
 316 #endif
 317
 318 void dummy_wait()
 319 {
 320         volatile unsigned int wait = 5000000;
 321         while(--wait);
 322 }
 323 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 324 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 325 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 326 int main(void)
 327 {
 328         uint32_t main_time = timer_usec;
 329         uint32_t status_time = timer_usec;
 330         can_msg_t tx_msg;
 331         bool obrat=true;
 332         int i=0,delta=0,j=0,suma=0;
 333
 334
 335         init_periphery();
 336
 337         /* TODO: Add comment FIXME: zkusit smazat, mam moct ze to melo neco spojeneho s chelae z eb09  */
 338         //SET_PIN(PINSEL1, 1, PINSEL_0);
 339         //SET_PIN(PINSEL1, 3, PINSEL_0);
 340
 341
 342
 343
 344         SET_PIN(PINSEL0, START_PIN, PINSEL_0);          // inicializace start pinu
 345         SET_PIN(PINSEL0, COLOR_PIN, PINSEL_0);
 346         SET_PIN(PINSEL1, 1, PINSEL_0);          // inicializace bumper pinu (FIXME SET_PIN je BLBA implemetace, musim ji nekdy opravit)
 347
 348         SET_PIN(PINSEL1, 3, PINSEL_0);
 349         SET_PIN(PINSEL0, BUMPER_RIGHT, PINSEL_0);
 350
 351
 352         IO0DIR &= ~((1<<START_PIN) | (1<<BUMPER_RIGHT) | (1 << COLOR_PIN));
 353         IO0DIR &= ~((1<<BUMPER_PIN) | (1<<BUMPER_LEFT));
 354
 355         //IO1DIR &= ~(3<<20);
 356
 357         send_rs_str("Vidle started\n");
 358         // The above send_rs_str is importat - we wait for the first AD conversion to be finished
 359         fsm_vidle.act_pos = adc_val[0];
 360
 361
 362 /*      test_vhn(); */
 363 /*      return; */
 364
 365         while(1){
 366                 if(timer_usec >= main_time + 1000)
 367                 {
 368                         main_time = timer_usec;
 369
 370                         //dbg_print_time();
 371
 372                 //      fsm_vidle.act_pos = adc_val[0];
 373
 374
 375         //              run_fsm(&fsm_vidle);
 376                 }
 377
 378                 if (timer_msec >= status_time + 100 || //repeat sending message every 100 ms
 379                     fsm_vidle.trigger_can_send) {   //or when something important happen
 380                         fsm_vidle.trigger_can_send = false;
 381                         status_time = timer_msec; //save new time, when message was sent
 382                 //      can_send_status();
 383
 384                 }
 385
 386                 //start_button();
 387                 //handle_bumper();
 388
 389
 390                 delta=i;
 391                 i=adc_val[0];
 392                 delta=i-delta;
 393                 suma+=delta;
 394                 engine_A_pwm(20);
 395                 if (j++>10000) {
 396                   if (abs(suma)<1) {obrat=!obrat;}
 397                 send_rs_int(suma);
 398                 send_rs_str("\n");
 399                   j=0;
 400                   suma=0;
 401
 402                 }
 403                 if (obrat)
 404                   {
 405                     if (i>750)
 406                     engine_A_dir(ENGINE_DIR_BW);
 407                     else {engine_A_dir(ENGINE_DIR_FW);
 408                     obrat=false;
 409                     suma=20;
 410                     }
 411                   }
 412                    else
 413                    {
 414                      if(i<950)
 415                        engine_A_dir(ENGINE_DIR_FW);
 416                     else {engine_A_dir(ENGINE_DIR_BW);
 417                     obrat=true;
 418                     suma=20;
 419                     }
 420
 421                    }
 422
 423
 424
 425
 426
 427         }
 428 }
 429
 430 /** @} */