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