app/smotor/motor.c

   1 /** cerveaumotor Spejbla **/
   2
   3 #include <stdlib.h>
   4 #include <types.h>
   5 #include <limits.h>
   6 #include <lpc21xx.h>
   7 #include <periph/can.h>
   8 #include <string.h>
   9 #include "pwm.h"
  10
  11 /* * */
  12
  13 /* CAN message IDs */
  14 #define CAN_CLOCK_ID  0x401
  15 //#define CAN_CLOCK_ID  0x481
  16 #define CAN_CFGMSG_ID 0x4ab
  17 #define CAN_UNDEF_ID  0xf800
  18
  19 /* unique motor/CAN ID */
  20 #define CANLOAD_ID (*((volatile unsigned long *) 0x40000120))
  21 #define MOTOR_ID CANLOAD_ID
  22
  23 /* potentiometer bounds */
  24 #define POS_MIN 0x060
  25 #define POS_MAX 0x3a0
  26 /* reserved value for power-off */
  27 #define CTRL_OFF 0xffff
  28 #define CTRL_MAX SHRT_MAX
  29
  30 /* own message for current position signalling */
  31 can_msg_t tx_msg = {.flags = 0, .dlc = 2};
  32
  33 /* command message ID and index */
  34 uint16_t cmd_msg_id = CAN_UNDEF_ID, cmd_msg_ndx = 0;
  35
  36 /* command value, current position */
  37 volatile uint16_t cmd_value = CTRL_OFF, rx_cmd_value = CTRL_OFF,
  38   position, pos_data;
  39 volatile uint8_t tx_request = 0;
  40
  41 /* * */
  42
  43 void timer_irq() __attribute__((interrupt));
  44
  45 void timer_init(uint32_t prescale, uint32_t period) {
  46   T0PR = prescale - 1;
  47   T0MR1 = period - 1;
  48   T0MCR = 0x3<<3; /* interrupt and counter reset on match1 */
  49   T0EMR = 0x1<<6 | 0x2; /* set MAT0.1 low on match and high now */
  50   T0CCR = 0x0; /* no capture */
  51   T0TCR |= 0x1; /* go! */
  52 }
  53
  54 void timer_init_irq(unsigned irq_vect) {
  55   /* set interrupt vector */
  56   ((uint32_t*)&VICVectAddr0)[irq_vect] = (uint32_t)timer_irq;
  57   ((uint32_t*)&VICVectCntl0)[irq_vect] = 0x20 | 4;
  58   /* enable timer int */
  59   VICIntEnable = 0x00000010;
  60 }
  61
  62 void adc_init(unsigned clk_div) {
  63   ADCR = 0x01 | ((clk_div&0xff)<<8) | 0x200000;
  64   ADCR |= 0x04000000; /* conversion started on falling edge of MAT0.1 */
  65 }
  66
  67 /* * */
  68
  69 inline void sample() {
  70   /* get last position measured (weak point! sampling jitter!) */
  71   position = pos_data;
  72   /* request transmission of obtained value by idle loop */
  73   tx_request = 1;
  74 }
  75
  76 inline void actuate() {
  77   /* set previously received command rx_cmd_value */
  78   if (rx_cmd_value == CTRL_OFF) {
  79     if (cmd_value != CTRL_OFF) {
  80       /* switch H-bridge off */
  81       pwm_set(4, 0);
  82       pwm_set(6, 0);
  83     }
  84   }
  85   else
  86     if (cmd_value == CTRL_OFF) {
  87       /* switch H-bridge on */
  88
  89     }
  90   /* set the value */
  91   cmd_value = rx_cmd_value;
  92 }
  93
  94 /* .oOo. * controller * .oOo. */
  95
  96 #define REG_P_GAIN (0.0012 * 15)
  97 #define REG_P_GAIN_INV ((int16_t)(1.0/REG_P_GAIN))
  98 inline int16_t regulate(uint16_t w, uint16_t y) {
  99   int16_t e = (int16_t)w - (int16_t)y;
 100   int32_t m;
 101
 102   if (e > REG_P_GAIN_INV)
 103     return(CTRL_MAX);
 104   if (e < -REG_P_GAIN_INV)
 105     return(-CTRL_MAX);
 106   /* REG_P_GAIN << 1 */
 107   m = (int32_t)e*CTRL_MAX;
 108   return(m/REG_P_GAIN_INV);
 109 }
 110
 111 void timer_irq() {
 112   int16_t u;
 113
 114   /* wait for A/D conversion to finish */
 115   while ((ADDR&0x80000000) == 0);
 116   T0EMR |= 0x2; /* set MAT0.1 high again */
 117   pos_data = (ADDR>>6)&0x3ff;
 118   if (cmd_value != CTRL_OFF) {
 119     /* compute the control */
 120     u = regulate(cmd_value, pos_data);
 121     /* apply it */
 122     if (((pos_data < POS_MIN) && (u < 0)) ||
 123         ((pos_data > POS_MAX) && (u > 0))) {
 124       /* protect the gear against self-destruction */
 125       pwm_set(4, 0);
 126       pwm_set(6, 0);
 127     }
 128     else {
 129       if (u > 0) {
 130         pwm_set(4, PWMMR0*u/CTRL_MAX);
 131         pwm_set(6, 0);
 132       }
 133       else {
 134         pwm_set(4, 0);
 135         pwm_set(6, PWMMR0*(-u)/CTRL_MAX);
 136       }
 137     }
 138   }
 139   /* clear int flag */
 140   T0IR = 0xffffffff;
 141   /* int acknowledge */
 142   VICVectAddr = 0;
 143 }
 144
 145 void can_rx(can_msg_t *msg) {
 146   can_msg_t rx_msg;
 147
 148   memcpy(&rx_msg, msg, sizeof(can_msg_t));
 149   switch (rx_msg.id) {
 150   case CAN_CLOCK_ID:
 151     sample();
 152     actuate();
 153     break;
 154   case CAN_CFGMSG_ID:
 155     if (((uint16_t*)rx_msg.data)[0] == MOTOR_ID) {
 156       cmd_msg_id = ((uint16_t*)rx_msg.data)[1];
 157       cmd_msg_ndx = ((uint16_t*)rx_msg.data)[2];
 158     }
 159     break;
 160   default:
 161     if (rx_msg.id == cmd_msg_id)
 162       rx_cmd_value = ((uint16_t*)rx_msg.data)[cmd_msg_ndx];
 163     break;
 164   }
 165 }
 166
 167 int main() {
 168   /* peripheral clock = CPU clock (10MHz) */
 169   VPBDIV = 1;
 170   /** map exception handling to RAM **/
 171   MEMMAP = 0x2;
 172   /* init Vector Interrupt Controller */
 173   VICIntEnClr = 0xFFFFFFFF;
 174   VICIntSelect = 0x00000000;
 175   /* * */
 176   /* 10MHz VPB, 1000kb/s TSEG1=6, TSEG2=3, SJW= */
 177   can_init(0x250000, 14, can_rx);
 178   tx_msg.id = MOTOR_ID;
 179   /***/
 180   adc_init(3);
 181   pwm_channel(4, 0);
 182   pwm_channel(6, 0);
 183   pwm_init(1, 500 /*20kHz*/);
 184   /* initially, switch H-bridge off */
 185   pwm_set(4, 0);
 186   pwm_set(6, 0);
 187   /* * */
 188   timer_init(10, 1000 /*1000Hz*/);
 189   timer_init_irq(13);
 190   /* jamais fatigue */
 191   for (;;)
 192     if (tx_request) {
 193       *((uint16_t*)tx_msg.data) = position;
 194       while (can_tx_msg(&tx_msg));
 195       tx_request = 0;
 196     }
 197 }
 198
 199 /* KOHE||, */