src/eb_lift_11/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
  37 #define CAN_SPEED       1000000         //< CAN bus speed
  38 #define CAN_ISR         0
  39
  40 #define ADC_ISR         1
  41
  42 #define TIMER_IRQ_PRIORITY      5
  43
  44
  45 struct fsm fsm_lift;
  46
  47 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  48 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  49 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  50
  51 #define END_SWITCH_UP   9       //pin 4, exp. port on board
  52 #define END_SWITCH_DOWN 8       //pin 3, exp. port on board
  53 #define SWITCH_HOME 19       //pin 6, exp. port on board
  54
  55 void lift_switches_handler(void);
  56
  57 void lift_switches_handler()
  58 {
  59   if (IO0PIN & (1<<END_SWITCH_UP)){
  60     fsm_lift.flags |= CAN_LIFT_SWITCH_UP;
  61     deb_led_on(LEDR);
  62   }
  63   else{
  64     fsm_lift.flags &= ~CAN_LIFT_SWITCH_UP;
  65     deb_led_off(LEDR);
  66   }
  67
  68   if (IO0PIN & (1<<END_SWITCH_DOWN)){
  69     fsm_lift.flags |= CAN_LIFT_SWITCH_DOWN;
  70     deb_led_on(LEDR);
  71   }
  72   else{
  73     fsm_lift.flags &= ~CAN_LIFT_SWITCH_DOWN;
  74     deb_led_off(LEDR);
  75   }
  76
  77   if (IO0PIN & (1<<SWITCH_HOME)){
  78     fsm_lift.flags |= CAN_LIFT_SWITCH_HOME;
  79     deb_led_on(LEDR);
  80   }
  81   else{
  82     fsm_lift.flags &= ~CAN_LIFT_SWITCH_HOME;
  83     deb_led_off(LEDR);
  84   }
  85 }
  86
  87 #define LIFT_IRC_VAL_MAX 0xA9B0
  88 #define LIFT_IRC_VAL_MIN 0x0
  89
  90 #define IRC_A  2        //pin 1, exp. port on board
  91 #define IRC_B  3        //pin 2, exp. port on board
  92
  93 #define IRC_A_MASK      0x04 //(1<<IRC_A)
  94 #define IRC_B_MASK      0x08 //(1<<IRC_B)
  95 #define IRC_AB_MASK     0x0C //((1<<IRC_A)&(1<<IRC_B))
  96
  97 int irc_read_tick(uint16_t* last_irc);
  98 void irc_init(void);
  99
 100 void irc_init(void){
 101   SET_PIN(PINSEL0, IRC_A, PINSEL_0);
 102   SET_PIN(PINSEL0, IRC_B, PINSEL_0);
 103
 104   SET_PIN(PINSEL0, END_SWITCH_UP, PINSEL_0);
 105   SET_PIN(PINSEL0, END_SWITCH_DOWN, PINSEL_0);
 106
 107 }
 108
 109 //source code from http://www.vosrk.cz/robotika/Stavba/Enkoder.pdf
 110 int irc_read_tick(uint16_t* last_irc){
 111
 112   uint16_t irc, temp;
 113
 114   irc = IO0PIN & IRC_AB_MASK;
 115   if ((irc & IRC_B_MASK) != 0){
 116     irc ^= IRC_A_MASK;
 117   }
 118
 119 //   send_rs_str("IRC\t");
 120 //   send_rs_int(irc);
 121 //   send_rs_str("\n");
 122 //
 123 //   send_rs_str("LAST_IRC\t");
 124 //   send_rs_int(*last_irc);
 125 //   send_rs_str("\n");
 126
 127   temp = (irc - *last_irc) & IRC_AB_MASK;
 128
 129   *last_irc= irc;
 130
 131 //   send_rs_str("TEMP\t");
 132 //   send_rs_int(temp);
 133 //   send_rs_str("\n");
 134
 135   if (temp == IRC_A_MASK){ // nebo zaporny (-1)
 136     return (-1);
 137   }
 138   else if (temp == IRC_AB_MASK){
 139     return (1);
 140   }
 141   return 0;
 142 }
 143
 144 void init_motors(void){
 145
 146         init_engine_A();                        // initialization of PWM unit
 147         engine_A_en(ENGINE_EN_ON);              //enable motor A
 148         engine_A_dir(ENGINE_DIR_FW);            //set direction
 149         engine_A_pwm(0);                        // STOP pwm is in percent, range 0~100~200
 150
 151 }
 152
 153 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 154 void set_irq_handler(uint8_t source, uint8_t irq_vect, void (*handler)()) {
 155         /* set interrupt vector */
 156         ((uint32_t*)&VICVectAddr0)[irq_vect] = (uint32_t)handler;
 157         ((uint32_t*)&VICVectCntl0)[irq_vect] = 0x20 | source;
 158         /* enable interrupt */
 159         VICIntEnable = 1<<source;
 160 }
 161
 162 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 163 /** timer0 & ISR **/
 164
 165 void timer0_irq() __attribute__((interrupt));
 166 volatile uint32_t timer_msec = 0, timer_usec = 0;
 167
 168 void init_timer0(uint32_t prescale, uint32_t period) {
 169         T0TCR = 0x2; /* reset */
 170         T0PR = prescale - 1;
 171         T0MR0 = period;
 172         T0MCR = 0x3; /* match0: reset & irq */
 173         T0EMR = 0; /* no ext. match */
 174         T0CCR = 0x0; /* no capture */
 175         T0TCR = 0x1; /* go! */
 176 }
 177
 178 void timer0_irq() {
 179         static unsigned cnt1khz = 0;
 180
 181         /* reset timer irq */
 182         T0IR = -1;
 183
 184         /* increment timer_usec */
 185         timer_usec += 10;
 186         /* increment msec @1kHz */
 187         if (++cnt1khz == 100) {
 188                 cnt1khz = 0;
 189                 ++timer_msec;
 190         }
 191
 192         /* int acknowledge */
 193         VICVectAddr = 0;
 194 }
 195
 196
 197 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 198
 199 #define START_PIN       15              //pin 7, exp. port on board
 200 #define START_SEND_PRIOD_FAST   50      /* [miliseconds] */
 201 #define START_SEND_PRIOD_SLOW   300     /* [miliseconds] */
 202 #define START_SEND_FAST_COUNT   10      /* How many times to send start with small period (after a change) */
 203
 204
 205 void start_button(void)
 206 {
 207         can_msg_t msg;
 208         bool start_condition;
 209         static bool last_start_condition = 0;
 210
 211         static int count = 0;
 212         static uint32_t next_send = 0;
 213
 214
 215         start_condition = (IO0PIN & (1<<START_PIN)) == 0;
 216
 217         if (start_condition != last_start_condition) {
 218
 219
 220                 last_start_condition = start_condition;
 221                 count = 0;
 222                 next_send = timer_msec; /* Send now */
 223
 224                 fsm_lift.flags |= CAN_LIFT_START;
 225         }
 226
 227         if (timer_msec >= next_send) {
 228                 msg.id = CAN_ROBOT_CMD;
 229                 msg.flags = 0;
 230                 msg.dlc = 1;
 231                 msg.data[0] = start_condition;
 232
 233 //              send_rs_str("start\n");
 234
 235                 /*while*/ (can_tx_msg(&msg));
 236
 237                 if (count < START_SEND_FAST_COUNT) {
 238                         count++;
 239                         next_send = timer_msec + START_SEND_PRIOD_FAST;
 240                 } else
 241                         next_send = timer_msec + START_SEND_PRIOD_SLOW;
 242         }
 243
 244
 245 }
 246
 247
 248 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 249 void CAN_rx(can_msg_t *msg) {
 250         uint32_t spd;
 251         can_msg_t rx_msg;
 252         uint32_t req =0;
 253         memcpy(&rx_msg, msg, sizeof(can_msg_t));//make copy of message
 254
 255
 256         deb_led_on(LEDB);
 257
 258         switch (rx_msg.id)
 259         {
 260                 case CAN_LIFT_CMD:
 261                         deb_led_on(LEDB);
 262                         req = ((rx_msg.data[0]<<8) | (rx_msg.data[1]));
 263                         spd = rx_msg.data[2];
 264                         fsm_lift.can_req_homing=rx_msg.data[3];
 265                         // range 0 - A9C5
 266                         if (req >= LIFT_IRC_VAL_MIN && req <= LIFT_IRC_VAL_MAX) {
 267                                 fsm_lift.flags &= ~CAN_LIFT_OUT_OF_BOUNDS;
 268                                 fsm_lift.can_req_position = req;// save new req position of lift
 269                                 fsm_lift.can_req_spd = spd;// save new req spd of lift
 270                         } else
 271                                 fsm_lift.flags |= CAN_LIFT_OUT_OF_BOUNDS;
 272                 break;
 273                 default:break;
 274         }
 275
 276         deb_led_off(LEDB);
 277 }
 278
 279
 280 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 281 void init_periphery(void){
 282
 283         can_init_baudrate(CAN_SPEED, CAN_ISR, CAN_rx);//initialization of CAN bus
 284         init_motors();
 285
 286         /* init timer0 */
 287         init_timer0(1, CPU_APB_HZ/100000);
 288         set_irq_handler(4 /*timer0*/, TIMER_IRQ_PRIORITY, timer0_irq);
 289
 290         init_uart();
 291 //      init_adc(ADC_ISR);
 292         irc_init();
 293
 294
 295 }
 296 /*********************************************************/
 297 void can_send_status(void)
 298 {
 299         can_msg_t tx_msg;
 300         tx_msg.id = CAN_LIFT_STATUS;
 301         tx_msg.dlc = 5;
 302         tx_msg.flags = 0;
 303         tx_msg.data[0] = (fsm_lift.act_pos  >> 8) & 0xFF;
 304         tx_msg.data[1] = fsm_lift.act_pos & 0xFF;
 305         tx_msg.data[2] = (fsm_lift.can_response  >> 8) & 0xFF;
 306         tx_msg.data[3] = fsm_lift.can_response & 0xFF;
 307         tx_msg.data[4] = fsm_lift.flags;
 308         /*while*/(can_tx_msg(&tx_msg)); /* CAN erratum workaround */
 309 }
 310
 311 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 312 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 313
 314 void dbg_print_time()
 315 {
 316         char str[10];
 317         unsigned t = timer_usec, i;
 318         memset(str, ' ', sizeof(str));
 319         str[9] = 0;
 320         str[8] = '\n';
 321         for (i=7; t > 0; i--) {
 322                 str[i] = t%10 + '0';
 323                 t /= 10;
 324         }
 325         send_rs_str(str);
 326 }
 327
 328 void fsm_lift_init(struct fsm *fsm, enum event event);
 329
 330
 331 void blink_led()
 332 {
 333         static uint32_t led_time = 0;
 334
 335         if(timer_msec >= led_time + 500)
 336         {
 337                 led_time = timer_msec;
 338 //                static int up;
 339 //                  if (up == 0)
 340 //                  fsm_lift.can_req_position = 0x380;
 341 //                  if (up == 6)
 342 //                  fsm_lift.can_req_position = 0x1e0;
 343 //                  up = (up+1)%12;
 344
 345                 deb_led_change(LEDG);
 346
 347                 send_rs_str("ACT_POS\t");
 348                 send_rs_int(fsm_lift.act_pos);
 349                 send_rs_str("\t");
 350                 send_rs_str("CAN_FLAGS\t");
 351                 send_rs_int(fsm_lift.flags);
 352                 send_rs_str("\n");
 353         }
 354 }
 355
 356
 357
 358 #define COLOR_PIN 18            //pin 5, exp. port on board
 359
 360 void color_handler()
 361 {
 362         static uint32_t color_time = 0;
 363         char sw = 0;
 364
 365         if (timer_msec >= color_time + 100)
 366         {
 367                 can_msg_t tx_msg;
 368
 369                 color_time = timer_msec;
 370
 371                 if (IO0PIN & (1<<COLOR_PIN))
 372                         sw |= CAN_SWITCH_COLOR;
 373                 else
 374                         sw &= ~CAN_SWITCH_COLOR;
 375
 376
 377                 if (sw & CAN_SWITCH_COLOR){
 378                         deb_led_off(LEDY);
 379
 380                         send_rs_str("barva\n");}
 381                 else
 382                         deb_led_on(LEDY);
 383
 384 //              send_rs_int(IO1PIN);
 385 //              send_rs_int(sw);
 386 //              send_rs_str("\n");
 387 //
 388                 /* TODO: Put color to the second bit */
 389
 390                 tx_msg.id = CAN_ROBOT_SWITCHES;
 391                 tx_msg.dlc = 1;
 392                 tx_msg.flags = 0;
 393                 tx_msg.data[0] = sw;
 394
 395                 can_tx_msg(&tx_msg);
 396         }
 397 }
 398
 399 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 400 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 401 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 402 int main(void)
 403 {
 404         uint32_t main_time = timer_usec;
 405         uint32_t status_time = timer_usec;
 406
 407         static uint16_t last_irc=0;
 408
 409         //lift motor is motor A, MOTA connctor on board
 410         init_periphery();
 411
 412         SET_PIN(PINSEL0, START_PIN, PINSEL_0);          //init of start pin
 413         SET_PIN(PINSEL0, 3, PINSEL_0);  //init of color pin
 414         SET_PIN(PINSEL0, 4, PINSEL_0);  //init of home pin
 415
 416         send_rs_str("Lift started\n");
 417
 418         fsm_lift.act_pos = 0;
 419         init_fsm(&fsm_lift, &fsm_lift_init);
 420
 421 /*      return; */
 422
 423         while(1){
 424                 if(timer_usec >= main_time + 1000)
 425                 {
 426                         main_time = timer_usec;
 427
 428                         //dbg_print_time();
 429
 430 //                      fsm_lift.act_pos = adc_val[0];
 431
 432                         run_fsm(&fsm_lift);
 433                 }
 434
 435                 if (timer_msec >= status_time + 100 || //repeat sending message every 100 ms
 436                     fsm_lift.trigger_can_send) {   //or when something important happen
 437                         fsm_lift.trigger_can_send = false;
 438                         status_time = timer_msec; //save new time, when message was sent
 439                         can_send_status();
 440
 441                 }
 442
 443                 fsm_lift.act_pos=fsm_lift.act_pos+irc_read_tick(&last_irc);
 444
 445                 start_button();
 446
 447                 color_handler();
 448                 lift_switches_handler();
 449                 blink_led();
 450         }
 451 }
 452
 453 /** @} */