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