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