nxtOSEK/lejos_nxj/src/nxtvm/platform/nxt/nxt_motors.c

   1 #include "nxt_motors.h"
   2
   3 #include "nxt_avr.h"
   4 #include "aic.h"
   5 #include "interrupts.h"
   6
   7 #include "AT91SAM7.h"
   8
   9 #define MA0 15
  10 #define MA1 1
  11 #define MB0 26
  12 #define MB1 9
  13 #define MC0 0
  14 #define MC1 8
  15
  16 #define MOTOR_PIN_MASK          ((1 << MA0) | (1<<MA1) | (1<<MB0) | (1<<MB1) | (1<<MC0) | (1<<MC1))
  17 #define MOTOR_INTERRUPT_PINS    ((1 << MA0) | (1<<MB0) | (1<<MC0))
  18
  19
  20 static struct motor_struct {
  21   int current_count;
  22   int target_count;
  23   int speed_percent;
  24   U32 last;
  25 } motor[NXT_N_MOTORS];
  26
  27 static U32 nxt_motor_initialised;
  28 static U32 interrupts_this_period;
  29
  30 int
  31 nxt_motor_get_count(U32 n)
  32 {
  33   if (n < NXT_N_MOTORS)
  34     return motor[n].current_count;
  35   else
  36     return 0;
  37 }
  38
  39 void
  40 nxt_motor_set_count(U32 n, int count)
  41 {
  42   if (n < NXT_N_MOTORS)
  43     motor[n].current_count = count;
  44 }
  45
  46 void
  47 nxt_motor_set_speed(U32 n, int speed_percent, int brake)
  48 {
  49   if (n < NXT_N_MOTORS) {
  50     if (speed_percent > 100)
  51       speed_percent = 100;
  52     if (speed_percent < -100)
  53       speed_percent = -100;
  54     motor[n].speed_percent = speed_percent;
  55     nxt_avr_set_motor(n, speed_percent, brake);
  56   }
  57 }
  58
  59 void
  60 nxt_motor_command(U32 n, int cmd, int target_count, int speed_percent)
  61 {
  62   if (n < NXT_N_MOTORS) {
  63     motor[n].target_count = target_count;
  64     motor[n].speed_percent = speed_percent;
  65   }
  66 }
  67
  68
  69 void
  70 nxt_motor_1kHz_process(void)
  71 {
  72   if (nxt_motor_initialised) {
  73     interrupts_this_period = 0;
  74     *AT91C_PIOA_IER = MOTOR_INTERRUPT_PINS;
  75   }
  76
  77 }
  78
  79 void
  80 nxt_motor_quad_decode(struct motor_struct *m, U32 value)
  81 {
  82 #if 0
  83   if (m->last != value) {
  84     if ((m->last + 1) & 3 == value)
  85       m->current_count--;
  86     else if ((value + 1) & 3 == m->last)
  87       m->current_count++;
  88     m->last = value;
  89   }
  90 #endif
  91
  92   U32 dir = value & 2;
  93   U32 edge = value & 1;
  94
  95   if (edge != m->last) {
  96     if (edge && !dir)
  97       m->current_count++;
  98     else if (edge && dir)
  99       m->current_count--;
 100     else if (!edge && dir)
 101       m->current_count++;
 102     else if (!edge && !dir)
 103       m->current_count--;
 104     m->last = edge;
 105   }
 106 }
 107
 108
 109 extern void nxt_motor_isr_entry(void);
 110
 111 void
 112 nxt_motor_isr_C(void)
 113 {
 114   U32 i_state = interrupts_get_and_disable();
 115
 116   U32 pinChanges = *AT91C_PIOA_ISR;     // Acknowledge change
 117   U32 currentPins = *AT91C_PIOA_PDSR;   // Read pins
 118
 119   U32 pins;
 120
 121   interrupts_this_period++;
 122   if (interrupts_this_period > 4) {
 123     *AT91C_PIOA_IDR = MOTOR_INTERRUPT_PINS;
 124     // Todo : tacho speed fault
 125   }
 126
 127
 128   /* Motor A */
 129   pins = ((currentPins >> MA0) & 1) | ((currentPins >> (MA1 - 1)) & 2);
 130   nxt_motor_quad_decode(&motor[0], pins);
 131
 132   /* Motor B */
 133   pins = ((currentPins >> MB0) & 1) | ((currentPins >> (MB1 - 1)) & 2);
 134   nxt_motor_quad_decode(&motor[1], pins);
 135
 136   /* Motor C */
 137   pins = ((currentPins >> MC0) & 1) | ((currentPins >> (MC1 - 1)) & 2);
 138   nxt_motor_quad_decode(&motor[2], pins);
 139
 140   if (i_state)
 141     interrupts_enable();
 142
 143 }
 144
 145
 146
 147 void
 148 nxt_motor_init(void)
 149 {
 150   *AT91C_PMC_PCER = (1 << AT91C_PERIPHERAL_ID_PIOA);    /* Power to the pins! */
 151   *AT91C_PIOA_IDR = ~0;
 152   *AT91C_PIOA_IFER = MOTOR_PIN_MASK;
 153   *AT91C_PIOA_PPUDR = MOTOR_PIN_MASK;
 154   *AT91C_PIOA_PER = MOTOR_PIN_MASK;
 155   *AT91C_PIOA_ODR = MOTOR_PIN_MASK;
 156
 157   /* Enable ISR */
 158   aic_mask_off(AT91C_PERIPHERAL_ID_PIOA);
 159   aic_set_vector(AT91C_PERIPHERAL_ID_PIOA, AIC_INT_LEVEL_NORMAL,
 160                  nxt_motor_isr_entry);
 161   aic_mask_on(AT91C_PERIPHERAL_ID_PIOA);
 162
 163   *AT91C_PIOA_IER = MOTOR_INTERRUPT_PINS;
 164
 165   nxt_motor_initialised = 1;
 166
 167
 168 }