src/eb_lift_09/main.c

   1
   2 /**
   3  * @file main.c
   4  *
   5  *
   6  * @author Bc. Jiri Kubias, jiri.kubias@gmail.com
   7  *
   8  * @addtogroup lift
   9  */
  10
  11
  12 /**
  13  * @defgroup lift Lift application
  14  *
  15  * Lift control application
  16  * - controlls the lift and pusher
  17  * - program recieves CAN commands about wanted lift/pusher position
  18  * - also controls Holded and right chelae
  19  */
  20 /**
  21  * @ingroup lift
  22  * @{
  23  */
  24
  25
  26 ////////////////////////////////////////////////////////////////////////////////
  27 //
  28 //                 Eurobot BLINK TEST  (with LPC2129)
  29 //
  30 // Description
  31 // -----------
  32 // This software control mechanisms in eurobot. Use it with lpceurobot board
  33 // Author : Jarda Sach DCE CVUT
  34 //
  35 //
  36 ////////////////////////////////////////////////////////////////////////////////
  37
  38 #include <lpc21xx.h>                            /* LPC21xx definitions */
  39 #include <types.h>
  40 #include <deb_led.h>
  41 #include <system_def.h>
  42 #include <can_ids.h>
  43 #include <periph/can.h>
  44 #include <string.h>
  45 #include <deb_led.h>
  46 #include "servo.h"
  47 #include "engine.h"
  48 #include "lift.h"
  49 #include "uar.h"
  50 #include "def.h"
  51 #include "irc.h"
  52 #include "vhn.h"
  53 #include <can_msg_def.h>
  54
  55
  56
  57
  58
  59 struct fsm fsm_lift;
  60 struct fsm fsm_pusher;
  61
  62
  63 void CAN_rx(can_msg_t *msg);
  64
  65
  66
  67 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  68 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  69 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  70
  71
  72 void init_motors(void){
  73
  74         init_engine_A();                        // initialization of PWM unit
  75         init_engine_B();
  76         engine_A_en(ENGINE_EN_ON);              //enable motor A
  77         engine_B_en(ENGINE_EN_ON);              // ----//----- B
  78         engine_A_dir(ENGINE_DIR_FW);            //set direction
  79         engine_B_dir(ENGINE_DIR_FW);
  80         engine_A_pwm(0);                        // STOP pwm is in percent, range 0~100~200
  81         engine_B_pwm(0);                        // STOP pwm is in percent, range 0~100~200
  82         vhn_init();
  83 }
  84
  85 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  86 void set_irq_handler(uint8_t source, uint8_t irq_vect, void (*handler)()) {
  87         /* set interrupt vector */
  88         ((uint32_t*)&VICVectAddr0)[irq_vect] = (uint32_t)handler;
  89         ((uint32_t*)&VICVectCntl0)[irq_vect] = 0x20 | source;
  90         /* enable interrupt */
  91         VICIntEnable = 1<<source;
  92 }
  93
  94 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  95 /** timer0 & ISR **/
  96
  97 void timer0_irq() __attribute__((interrupt));
  98 volatile uint32_t timer_msec = 0, timer_usec = 0;
  99
 100 void init_timer0(uint32_t prescale, uint32_t period) {
 101         T0TCR = 0x2; /* reset */
 102         T0PR = prescale - 1;
 103         T0MR0 = period;
 104         T0MCR = 0x3; /* match0: reset & irq */
 105         T0EMR = 0; /* no ext. match */
 106         T0CCR = 0x0; /* no capture */
 107         T0TCR = 0x1; /* go! */
 108 }
 109
 110 void timer0_irq() {
 111         static unsigned cnt1khz = 0;
 112
 113         /* reset timer irq */
 114         T0IR = -1;
 115
 116         /* read IRCs @100kHz */
 117         irc_read();
 118         /* increment timer_usec */
 119         timer_usec += 10;
 120         /* increment msec @1kHz */
 121         if (++cnt1khz == 100) {
 122                 cnt1khz = 0;
 123                 ++timer_msec;
 124         }
 125
 126         /* int acknowledge */
 127         VICVectAddr = 0;
 128 }
 129
 130
 131
 132 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 133 void init_periphery(void){
 134
 135         can_init_baudrate(CAN_SPEED, CAN_ISR, CAN_rx);//initialization of CAN bus
 136         init_motors();
 137         init_servo(SERVO_PRIORITY);//SERVO_PRIORITY is level of intrrupt, is defined in lift header file
 138         set_servo(SERVO_DOOR, SERVO_DOOR_MAX);
 139         set_servo(CHELAE_R, CHELAE_R_MAX);
 140         set_servo(2, 0x80);
 141                 /* init timer0 */
 142         init_timer0(1, CPU_APB_HZ/100000);
 143         set_irq_handler(4 /*timer0*/, IRC_PRIORITY, timer0_irq);
 144
 145         /* init IRCs */
 146         irc_init();
 147         init_uart();
 148         vhn_init();
 149 }
 150 /*********************************************************/
 151 void can_send_status(void)
 152 {
 153     can_msg_t tx_msg;
 154     if (fsm_lift.ans_can == 0x118)
 155             send_rs_str("WRONG!!!\n");
 156     tx_msg.id = CAN_LP_STATUS;
 157     tx_msg.dlc = 5;
 158     tx_msg.flags = 0;
 159     tx_msg.data[0] = (fsm_pusher.ans_can  >> 8) & 0xFF;
 160     tx_msg.data[1] = fsm_pusher.ans_can & 0xFF;
 161     tx_msg.data[2] = (fsm_lift.ans_can  >> 8) & 0xFF;
 162     tx_msg.data[3] = fsm_lift.ans_can & 0xFF;
 163     tx_msg.data[4] = can_flags;
 164     while(can_tx_msg(&tx_msg)); /* CAN erratum workaround */
 165 }
 166
 167 void can_send_len(void)
 168 {
 169     can_msg_t tx_msg;
 170     tx_msg.id = CAN_LP_STATUS;
 171     tx_msg.dlc = 4;
 172     tx_msg.flags = 0;
 173     tx_msg.data[0] = (fsm_pusher.lenght  >> 8) & 0xFF;
 174     tx_msg.data[1] = fsm_pusher.lenght & 0xFF;
 175     tx_msg.data[2] = (fsm_lift.lenght  >> 8) & 0xFF;
 176     tx_msg.data[3] = fsm_lift.lenght & 0xFF;
 177     while(can_tx_msg(&tx_msg));
 178
 179 }
 180
 181
 182 void can_send_db(void)
 183 {
 184     can_msg_t tx_msg;
 185     tx_msg.id = CAN_DB_LP;
 186     tx_msg.dlc = 4;
 187     tx_msg.flags = 0;
 188     tx_msg.data[0] = (fsm_pusher.act_pos  >> 8) & 0xFF;
 189     tx_msg.data[1] = fsm_pusher.act_pos & 0xFF;
 190     tx_msg.data[2] = (fsm_lift.act_pos  >> 8) & 0xFF;
 191     tx_msg.data[3] = fsm_lift.act_pos & 0xFF;
 192     while(can_tx_msg(&tx_msg));
 193
 194 }
 195
 196 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 197 void CAN_rx(can_msg_t *msg) {
 198         can_msg_t rx_msg;
 199         uint32_t req =0;
 200         memcpy(&rx_msg, msg, sizeof(can_msg_t));//make copy of message
 201
 202
 203         deb_led_on(LEDB);
 204         switch (rx_msg.id)
 205         {
 206                 case CAN_LIFT:
 207                         deb_led_on(LEDB);
 208                         req = ((rx_msg.data[0]<<8) | (rx_msg.data[1]));
 209
 210                         fsm_lift.can_req_position = req;// save new req position of lift
 211                 break;
 212                 case CAN_PUSHER:
 213                         deb_led_on(LEDB);
 214                         req = ((rx_msg.data[0]<<8) | (rx_msg.data[1]));
 215                         fsm_pusher.can_req_position = req;// save new req position of lift
 216                 break;
 217
 218                 case CAN_HOLDER:
 219                         deb_led_on(LEDB);
 220                         set_servo(SERVO_DOOR, SERVO_DOOR_MIN + (0xFF -rx_msg.data[0]) * (SERVO_DOOR_MAX - SERVO_DOOR_MIN)/255 );        //set servo position TODO: limits
 221                 break;
 222
 223                 case CAN_CHELAE:
 224                         deb_led_on(LEDB);
 225                         set_servo(CHELAE_R , CHELAE_R_MIN + (0xFF - rx_msg.data[1]) * (CHELAE_R_MAX - CHELAE_R_MIN)/255 );      //set servo position TODO: limits
 226
 227                 default:break;
 228         }
 229         deb_led_off(LEDB);
 230 }
 231
 232
 233 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 234 void run_fsm(struct fsm *fsm){
 235         fsm_irc(fsm);
 236         fsm->last_state = fsm->current_state;           // set actual state
 237         fsm->current_state(fsm, EVENT_DO);              // change parameter
 238
 239         if(fsm->last_state != fsm->current_state){      // if state was changed
 240                 fsm->last_state(fsm, EVENT_EXIT);       // finish the old state
 241                 fsm->current_state(fsm, EVENT_ENTRY);   // initialize the new state
 242         }
 243 }
 244
 245
 246 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 247 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 248 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 249 int main(void){
 250
 251 // ~~~~~ initialization of periphery ~~~~~
 252
 253         can_flags = 0;
 254         init_periphery();
 255
 256         fsm_pusher.type = FSM_PUSHER;
 257         fsm_lift.type = FSM_LIFT;
 258
 259         fsm_lift.current_state = &fsm_init;
 260         fsm_pusher.current_state = &fsm_init;
 261         fsm_lift.current_state(&fsm_lift, EVENT_ENTRY);
 262         fsm_pusher.current_state(&fsm_pusher, EVENT_ENTRY);
 263
 264         uint32_t main_time = timer_usec;
 265         uint32_t led_time = timer_msec;
 266         uint32_t CAN_time = timer_msec;
 267
 268         SET_PIN(PINSEL1, 1, PINSEL_0);
 269         SET_PIN(PINSEL1, 3, PINSEL_0);
 270
 271
 272         IO1DIR &= ~(3<<20);
 273
 274 //      while(1)
 275 //      {
 276 //        if(IO1PIN & (1<<20))
 277 //          deb_led_on(LEDY);
 278 //        else
 279 //          deb_led_off(LEDY);
 280 //
 281 //
 282 //        if(IO1PIN & (1<<21))
 283 //          deb_led_on(LEDR);
 284 //        else
 285 //          deb_led_off(LEDR);
 286 //      }
 287
 288
 289         send_rs_str("list started\n");
 290
 291
 292         while(1){
 293
 294                 if(timer_usec >= main_time + 1000)
 295                 {
 296                     main_time = timer_usec;
 297                     run_fsm(&fsm_lift);
 298                     run_fsm(&fsm_pusher);
 299
 300                     fsm_lift.switch_front = (0 != (IO1PIN & (1<<LIFT_FRONT_SWITCH)));
 301                     fsm_lift.switch_back  = (0 != (IO1PIN & (1<<LIFT_BACK_SWITCH)));
 302                     {
 303                             static int counter = 0;
 304                             /* Histereze koncaku */
 305                             if (0 != (IO1PIN & (1<<PUSHER_FRONT_SWITCH))) counter++;
 306                             else counter--;
 307                             if (counter > 10) {
 308                                     counter=10;
 309                                     fsm_pusher.switch_front = true;
 310                             }
 311                             if (counter < 0) {
 312                                     fsm_pusher.switch_front = false;
 313                                     counter = 0;
 314                             }
 315                     }
 316                     fsm_pusher.switch_back  = (0 != (IO1PIN & (1<<PUSHER_BACK_SWITCH)));
 317
 318                     if (fsm_lift.switch_front) can_flags |= LIFT_LIFT_ENDSW_UP;
 319                     else can_flags &= ~LIFT_LIFT_ENDSW_UP;
 320                     if (fsm_lift.switch_back) can_flags |= LIFT_LIFT_ENDSW_DOWN;
 321                     else can_flags &= ~LIFT_LIFT_ENDSW_DOWN;
 322
 323                     if (fsm_pusher.switch_front) can_flags |= LIFT_PUSHER_ENDSW_FW;
 324                     else can_flags &= ~LIFT_PUSHER_ENDSW_FW;
 325                     if (fsm_pusher.switch_back) can_flags |= LIFT_PUSHER_ENDSW_BK;
 326                     else can_flags &= ~LIFT_PUSHER_ENDSW_BK;
 327
 328                 //    can_send_db();
 329                 }
 330
 331                 if (timer_msec >= CAN_time + 100){      //repeat sending message every 100 ms
 332                         CAN_time = timer_msec;          //save new time, when message was sent
 333                         can_send_status();
 334                 }
 335
 336                 if(timer_msec >= led_time + 500)
 337                 {
 338                     led_time = timer_msec;
 339                     deb_led_change(LEDG);
 340                 }
 341         }
 342 }
 343
 344 /** @} */