src/eb_jaws_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_jaw_right;
  46 struct fsm fsm_jaw_left;
  47
  48 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  49 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  50 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  51
  52 void init_motors(void){
  53
  54         init_engine_A();                        // initialization of PWM unit
  55         engine_A_en(ENGINE_EN_ON);              //enable motor A
  56         engine_A_dir(ENGINE_DIR_FW);            //set direction
  57         engine_A_pwm(0);                        // STOP pwm is in percent, range 0~100~200
  58
  59         init_engine_B();                        // initialization of PWM unit
  60         engine_B_en(ENGINE_EN_ON);              //enable motor A
  61         engine_B_dir(ENGINE_DIR_FW);            //set direction
  62         engine_B_pwm(0);                        // STOP pwm is in percent, range 0~100~200
  63
  64 /*      vhn_init(); */
  65 }
  66
  67 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  68 void set_irq_handler(uint8_t source, uint8_t irq_vect, void (*handler)()) {
  69         /* set interrupt vector */
  70         ((uint32_t*)&VICVectAddr0)[irq_vect] = (uint32_t)handler;
  71         ((uint32_t*)&VICVectCntl0)[irq_vect] = 0x20 | source;
  72         /* enable interrupt */
  73         VICIntEnable = 1<<source;
  74 }
  75
  76 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  77 /** timer0 & ISR **/
  78
  79 void timer0_irq() __attribute__((interrupt));
  80 volatile uint32_t timer_msec = 0, timer_usec = 0;
  81
  82
  83 void init_timer0(uint32_t prescale, uint32_t period) {
  84         T0TCR = 0x2; /* reset */
  85         T0PR = prescale - 1;
  86         T0MR0 = period;
  87         T0MCR = 0x3; /* match0: reset & irq */
  88         T0EMR = 0; /* no ext. match */
  89         T0CCR = 0x0; /* no capture */
  90         T0TCR = 0x1; /* go! */
  91 }
  92
  93
  94 void timer0_irq() {
  95         static unsigned cnt1khz = 0;
  96
  97         /* reset timer irq */
  98         T0IR = -1;
  99
 100         /* increment timer_usec */
 101         timer_usec += 10;
 102         /* increment msec @1kHz */
 103         if (++cnt1khz == 100) {
 104                 cnt1khz = 0;
 105                 ++timer_msec;
 106         }
 107
 108         /* int acknowledge */
 109         VICVectAddr = 0;
 110 }
 111
 112
 113 void CAN_rx(can_msg_t *msg) {
 114         uint32_t spd;
 115         can_msg_t rx_msg;
 116         uint32_t req =0;
 117         memcpy(&rx_msg, msg, sizeof(can_msg_t));//make copy of message
 118
 119
 120         deb_led_on(LEDB);
 121
 122         switch (rx_msg.id)
 123         {
 124                 case CAN_JAW_LEFT_CMD:
 125
 126                         deb_led_on(LEDB);
 127                         req = ((rx_msg.data[0]<<8) | (rx_msg.data[1]));
 128                         spd = rx_msg.data[2];
 129
 130                         if (req >= fsm_jaw_left.min_pos && req <= fsm_jaw_left.max_pos) {
 131                                 fsm_jaw_left.flags &= ~CAN_JAW_OUT_OF_BOUNDS;
 132                                 fsm_jaw_left.can_req_position = req;
 133                                 fsm_jaw_left.can_req_spd = spd;
 134                         } else
 135                                 fsm_jaw_left.flags |= CAN_JAW_OUT_OF_BOUNDS;
 136                 break;
 137
 138                 case CAN_JAW_RIGHT_CMD:
 139
 140                         deb_led_on(LEDB);
 141                         req = ((rx_msg.data[0]<<8) | (rx_msg.data[1]));
 142                         spd = rx_msg.data[2];
 143
 144                         if (req >= fsm_jaw_right.min_pos && req <= fsm_jaw_right.max_pos) {
 145                                 fsm_jaw_right.flags &= ~CAN_JAW_OUT_OF_BOUNDS;
 146                                 fsm_jaw_right.can_req_position = req;
 147                                 fsm_jaw_right.can_req_spd = spd;
 148                         } else
 149                                 fsm_jaw_right.flags |= CAN_JAW_OUT_OF_BOUNDS;
 150                 break;
 151                 default:break;
 152         }
 153
 154         deb_led_off(LEDB);
 155 }
 156
 157
 158 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 159 void init_periphery(void){
 160
 161         can_init_baudrate(CAN_SPEED, CAN_ISR, CAN_rx);//initialization of CAN bus
 162         init_motors();
 163
 164         /* init timer0 */
 165         init_timer0(1, CPU_APB_HZ/100000);
 166         set_irq_handler(4 /*timer0*/, TIMER_IRQ_PRIORITY, timer0_irq);
 167
 168         init_uart();
 169         init_adc(ADC_ISR);
 170
 171
 172 }
 173 /*********************************************************/
 174 void can_send_status(struct fsm *fsm){
 175         can_msg_t tx_msg;
 176         tx_msg.id = fsm->can_id;
 177         tx_msg.dlc = 5;
 178         tx_msg.flags = 0;
 179         tx_msg.data[0] = (fsm->act_pos  >> 8) & 0xFF;
 180         tx_msg.data[1] = fsm->act_pos & 0xFF;
 181         tx_msg.data[2] = (fsm->can_response  >> 8) & 0xFF;
 182         tx_msg.data[3] = fsm->can_response & 0xFF;
 183         tx_msg.data[4] = fsm->flags;
 184         /*while*/(can_tx_msg(&tx_msg)); /* CAN erratum workaround */
 185
 186 }
 187
 188 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 189 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 190
 191 void dbg_print_time()
 192 {
 193         char str[10];
 194         unsigned t = timer_usec, i;
 195         memset(str, ' ', sizeof(str));
 196         str[9] = 0;
 197         str[8] = '\n';
 198         for (i=7; t > 0; i--) {
 199                 str[i] = t%10 + '0';
 200                 t /= 10;
 201         }
 202         send_rs_str(str);
 203 }
 204
 205 void fsm_jaw_init(struct fsm *fsm, enum event event);
 206
 207 void blink_led()
 208 {
 209         static uint32_t led_time = 0;
 210
 211         if(timer_msec >= led_time + 500)
 212         {
 213                 led_time = timer_msec;
 214                 /*  static int up;
 215                     if (up == 0)
 216                     fsm_vidle.can_req_position = 0x380;
 217                     if (up == 6)
 218                     fsm_vidle.can_req_position = 0x1e0;
 219                     up = (up+1)%12;
 220                 */
 221                 deb_led_change(LEDG);
 222                 send_rs_int(fsm_jaw_left.act_pos);
 223                 send_rs_str("\n");
 224                 send_rs_int(fsm_jaw_right.act_pos);
 225                 send_rs_str("\n");
 226                 send_rs_str("\n");
 227
 228         }
 229 }
 230
 231 #define BUMPER_LEFT     18
 232 #define BUMPER_RIGHT    8
 233
 234 #define BUMPER_LEFT_ACROSS      19
 235 #define BUMPER_RIGHT_ACROSS     17
 236
 237 #define BUMPER_REAR     15
 238
 239 void handle_bumper()
 240 {
 241         static uint32_t bumper_time = 0;
 242         char sw = 0;
 243
 244         if (timer_msec >= bumper_time + 100)
 245         {
 246                 can_msg_t tx_msg;
 247
 248                 bumper_time = timer_msec;
 249
 250
 251
 252                 if (IO0PIN & (1<<BUMPER_REAR)){
 253                         sw &= ~CAN_BUMPER_REAR;
 254
 255 //                      send_rs_str("rear\n");
 256
 257 //                      deb_led_on(LEDY);
 258                 }
 259                 else{
 260                         sw |= CAN_BUMPER_REAR;
 261
 262 //                      deb_led_off(LEDY);
 263                 }
 264
 265                 if (IO0PIN & (1<<BUMPER_LEFT)){
 266
 267                         sw &= ~CAN_BUMPER_LEFT;
 268
 269 //                      send_rs_str("left\n");
 270
 271 //                      deb_led_on(LEDB);
 272                 }
 273                 else{
 274
 275                     sw |= CAN_BUMPER_LEFT;
 276
 277 //                  deb_led_off(LEDB);
 278                 }
 279                 if (IO0PIN & (1<<BUMPER_RIGHT)){
 280                         sw &= ~CAN_BUMPER_RIGHT;
 281 //                      send_rs_str("right\n");
 282 //                      deb_led_on(LEDG);
 283                 }
 284                 else{
 285                         sw |= CAN_BUMPER_RIGHT;
 286
 287 //                      deb_led_off(LEDG);
 288                 }
 289                 if (IO0PIN & (1<<BUMPER_LEFT_ACROSS)){
 290                         sw &= ~CAN_BUMPER_LEFT_ACROSS;
 291 //                      send_rs_str("left_across\n");
 292 //                      deb_led_on(LEDR);
 293                 }
 294                 else{
 295                         sw |= CAN_BUMPER_LEFT_ACROSS;
 296
 297 //                      deb_led_off(LEDR);
 298                 }
 299                 if (IO0PIN & (1<<BUMPER_RIGHT_ACROSS)){
 300                         sw &= ~CAN_BUMPER_RIGHT_ACROSS;
 301 //                      send_rs_str("right_across\n");
 302 //                      deb_led_on(LEDY);
 303                 }
 304                 else{
 305                         sw |= CAN_BUMPER_RIGHT_ACROSS;
 306
 307 //                      deb_led_off(LEDY);
 308                 }
 309
 310                 if (sw & (CAN_BUMPER_REAR | CAN_BUMPER_LEFT | CAN_BUMPER_RIGHT | CAN_BUMPER_LEFT_ACROSS | CAN_BUMPER_RIGHT_ACROSS))
 311                         deb_led_on(LEDR);
 312                 else
 313                         deb_led_off(LEDR);
 314
 315 //              send_rs_int(IO1PIN);
 316 //              send_rs_int(sw);
 317 //              send_rs_str("\n");
 318 //
 319
 320                 tx_msg.id = CAN_ROBOT_BUMPERS;
 321                 tx_msg.dlc = 1;
 322                 tx_msg.flags = 0;
 323                 tx_msg.data[0] = sw;
 324
 325                 can_tx_msg(&tx_msg);
 326         }
 327 }
 328
 329
 330 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 331 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 332 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 333 int main(void)
 334 {
 335         uint32_t main_time = timer_usec;
 336         uint32_t status_time_left = timer_usec;
 337         uint32_t status_time_right = timer_usec;
 338
 339
 340         init_periphery();
 341
 342         SET_PIN(PINSEL1, 1, PINSEL_0);
 343         SET_PIN(PINSEL1, 2, PINSEL_0);
 344         SET_PIN(PINSEL1, 3, PINSEL_0);
 345         SET_PIN(PINSEL0, BUMPER_REAR, PINSEL_0);
 346         SET_PIN(PINSEL0, BUMPER_RIGHT, PINSEL_0);
 347
 348         IO0DIR &= ~((1<<BUMPER_REAR) | (1<<BUMPER_RIGHT) | (1<<BUMPER_LEFT) | (1<<BUMPER_RIGHT_ACROSS) | (1<<BUMPER_LEFT_ACROSS));
 349
 350         send_rs_str("Jaws started\n");
 351
 352         //close jaw, max_pos, open jaw min_pos
 353         //letf jaw potenciometer on ADC 0, header J32 on board
 354         fsm_jaw_left.max_pos=0x320; //max 0x324
 355         fsm_jaw_left.min_pos=0xC0; //min 0xb7
 356         fsm_jaw_left.act_pos = adc_val[0];
 357
 358         //close jaw, max_pos, open jaw min_pos
 359         //letf jaw potenciometer on ADC 1, header J33 on board
 360         fsm_jaw_right.max_pos=0x380; //max 0x387
 361         fsm_jaw_right.min_pos=0x120; //min 0x117
 362         fsm_jaw_right.act_pos = adc_val[1];
 363
 364         //left jaw engine is engine A, conector MOTA on board
 365         fsm_jaw_left.engine_en = &engine_A_en;
 366         fsm_jaw_left.engine_dir = &engine_A_dir;
 367         fsm_jaw_left.engine_pwm = &engine_A_pwm;
 368
 369         fsm_jaw_left.can_id=CAN_JAW_LEFT_STATUS;
 370
 371         //right jaw engine is engine B, conector MOTB on board
 372         fsm_jaw_right.engine_en = &engine_B_en;
 373         fsm_jaw_right.engine_dir = &engine_B_dir;
 374         fsm_jaw_right.engine_pwm = &engine_B_pwm;
 375
 376         fsm_jaw_right.can_id=CAN_JAW_RIGHT_STATUS;
 377
 378         init_fsm(&fsm_jaw_right, &fsm_jaw_init);
 379         init_fsm(&fsm_jaw_left, &fsm_jaw_init);
 380
 381 /*      test_vhn(); */
 382 /*      return; */
 383
 384         while(1){
 385                 if(timer_usec >= main_time + 1000)
 386                 {
 387                         main_time = timer_usec;
 388
 389                         //dbg_print_time();
 390
 391                         fsm_jaw_left.act_pos = adc_val[0];
 392                         run_fsm(&fsm_jaw_left);
 393
 394                         fsm_jaw_right.act_pos = adc_val[1];
 395                         run_fsm(&fsm_jaw_right);
 396
 397                 }
 398
 399                 if (timer_msec >= status_time_left + 100 || //repeat sending message every 100 ms
 400                     fsm_jaw_left.trigger_can_send) {   //or when something important happen
 401                         fsm_jaw_left.trigger_can_send = false;
 402                         status_time_left = timer_msec; //save new time, when message was sent
 403                         can_send_status(&fsm_jaw_left);
 404
 405                 }
 406
 407                 if (timer_msec >= status_time_right + 100 || //repeat sending message every 100 ms
 408                     fsm_jaw_right.trigger_can_send) {   //or when something important happen
 409                         fsm_jaw_right.trigger_can_send = false;
 410                         status_time_right = timer_msec; //save new time, when message was sent
 411                         can_send_status(&fsm_jaw_right);
 412
 413                 }
 414
 415                 handle_bumper();
 416
 417                 blink_led();
 418         }
 419 }
 420
 421 /** @} */