pccom.c 5.03 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
// pccom.c: Communication interface to PC via serialio_core.

#include "pccom.h"
#include "serialio_core.h"
#include "vel_control.h"

/*
 * Serial I/O assignments
 *
 *   AO 0 -- Axis 1  motor voltage
 *   A0 1 -- Axis 1  velocity reference !
 *
 *   EI 0 -- Axis 1  position !
 *   EI 1 -- Axis 1  filtered velocity !
 *   EI 5 -- Axis 1  position with predicted fraction
 *   EI 6 -- Axis 1  position unquantized
 *
 *   AI 2 -- Axis 1  current
 *   AI 3 -- Pendulum angle !
 *
 *   AI 4 -- Axis 1  motor voltage (actual)
 *
 *   DI 0 -- Axis 1  endpoint sensor
 */

#define POLL_AXIS1_POSITION      0x0001
#define POLL_AXIS1_VELOCITY      0x0002
//#define POLL_PEND_ANGLE          0x0004
//#define POLL_AXIS1_RESET         0x0008
#define POLL_CONFIG              0x8000

static volatile uint16_t pccom_poll=0;


// ---------------------------------- Receiver --------------------------------

// only call from UART receive interrupt
static inline void addPoll(uint16_t flags) {
  pccom_poll |= flags;
}


void pccom_receiveByte(char ch)
{
  switch (serialio_RXC(ch)) {
  case serialio_clearbit: {
    switch (serialio_channel) {
    }
  } break;
  case serialio_setbit: {
    switch (serialio_channel) {
    }
  } break;
  case serialio_pollbit: {
    switch (serialio_channel) {
      //case 0: { addPoll(POLL_AXIS1_RESET); } break;
    }
  } break;
  case serialio_pollchannel: {
    switch (serialio_channel) {
    case 0:  { addPoll(POLL_AXIS1_POSITION); } break;
    case 1:  { addPoll(POLL_AXIS1_VELOCITY); } break;
      //case 2:  { addPoll(POLL_PEND_ANGLE); } break;
    case 31: { addPoll(POLL_CONFIG); } break;
    }
  } break;
  case serialio_setchannel: {
    switch (serialio_channel) {
    case 0: { 
      setRef(serialio_value - (1L<<12));
    } break;
    }
  } break;
  case serialio_more: {
  } break;
    
  case serialio_error: {
  } break;
  }
}


// ----------------------------------- Sender ------------------------------
// return true if more to send
static uint8_t sendConfigPacket(uint8_t position)
{
  switch (position) {
    case 0: CONF_ANALOG_IN(0, CONF_RESOLUTION(16)); break;     // Position (now reference)
    case 1: CONF_ANALOG_IN(0, CONF_MIN(CONF_NEGATIVE_VOLT(1))); break;
    case 2: CONF_ANALOG_IN(0, CONF_MAX(CONF_POSITIVE_VOLT(1))); break;

      case 3: CONF_ANALOG_IN(1, CONF_RESOLUTION(18)); break;     // Velocity estimate
      case 4: CONF_ANALOG_IN(1, CONF_MIN(CONF_NEGATIVE_VOLT(1))); break;
      case 5: CONF_ANALOG_IN(1, CONF_MAX(CONF_POSITIVE_VOLT(1))); break;
 
     case 6: CONF_ANALOG_OUT(0, CONF_RESOLUTION(13)); break;    // Reference to vel-ctrl
     case 7: CONF_ANALOG_OUT(0, CONF_MIN(CONF_NEGATIVE_VOLT(1))); break;
     case 8: CONF_ANALOG_OUT(0, CONF_MAX(CONF_POSITIVE_VOLT(1))); break;
      
    default: CONF_END(); return 0;
  }
  
  return 1;
}

static uint8_t sendNextPacket() // returns 1 if a packet was available
{
  static int8_t configPosition = -1;

#define toPoll pccom_poll // OK since sender and receiver are mutexed

  // Send _first_ requested item (only one packet!)
  if (toPoll & POLL_AXIS1_POSITION) { 
    toPoll &= ~POLL_AXIS1_POSITION;
    serialio_putchannel(0, getPosition()+(1L<<15)); 
  }
  else if (toPoll & POLL_AXIS1_VELOCITY) { 
    toPoll &= ~POLL_AXIS1_VELOCITY;
    serialio_putchannel(1, getVelocity()+(1L<<17)); 
  }
  else if (toPoll & POLL_CONFIG) {
    if (configPosition < 0) configPosition = 0; // Start sending config?   
    if (!sendConfigPacket(configPosition)) {    // Last packet?
      configPosition = -1;
      toPoll &= ~POLL_CONFIG;
    }
    else configPosition++;                      // Advance to next packet
  }
  else return 0; // should never happen!

  return 1;
}

// ---- Send buffering ----

#define PCCOM_SEND_BUFFER_SIZE 6 // just enough for one serialio_putchannel() packet

uint8_t pccom_sendBuffer[PCCOM_SEND_BUFFER_SIZE]; // Updated by serialio_putchar()
uint8_t pccom_sendBufferPosition = 0;             // Updated by pccom_getNextByteToSend()
uint8_t pccom_sendBufferUsed = 0;                 // Updated by serialio_putchar(),
                                                  // and pccom_getNextByteToSend() when
                                                  // the buffer is empty.

void serialio_putchar(unsigned char ch) {
  if (pccom_sendBufferUsed < PCCOM_SEND_BUFFER_SIZE) {
    pccom_sendBuffer[pccom_sendBufferUsed] = ch;
    pccom_sendBufferUsed++;
  }
  else {
    // Buffer already full -- must never happen!
    // main_emergencyStop(); // show that something is wrong
  }
}

int16_t pccom_getNextByteToSend()
{
  if (pccom_sendBufferPosition >= pccom_sendBufferUsed) {
    // Try to refill buffer
    pccom_sendBufferPosition = 0;
    pccom_sendBufferUsed = 0;

    //if (!sendNextPacket()) return -1;
    sendNextPacket();
  }
  
  if (pccom_sendBufferPosition >= pccom_sendBufferUsed) return -1; // no data
  else {
    // Return next byte
    uint8_t data = pccom_sendBuffer[pccom_sendBufferPosition];
    pccom_sendBufferPosition++;
    return data;
  }  
}


// ------------------------------ Initialization ------------------------------

void pccom_init()
{
  serialio_init();
}