ball_and_beam-2018.c 6.93 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
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
#include <avr/eeprom.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <inttypes.h>
#include "serialio.h"

/*
 * Serial I/O assignments:
 *   
 *   DI 0 -- Motor Overtemp
 *
 *   DO 0 -- Motor Brake
 *   DO 1 -- Accelerometer Self-Test
 *
 *   AI 0 -- Ball Position 
 *   AI 1 -- Motor Current
 *   AI 2 -- Accelerometer X
 *   AI 3 -- Accelerometer Y
 *   AI 4 -- Accelerometer Z
 *   EI 5 -- Motor Position
 *
 *   AO 0 -- Motor Speed
  */

/*
 * Used I/O pins
 *
 * PA0  Ball Position
 * PA1  Motor Current
 * PA2  Accelerometer X
 * PA3  Accelerometer Y
 * PA4  Accelerometer Z
 *
 * PB0  Ball On Beam
 * PB1  Motor direction
 * PB2  Encoder Z
 * PB3  Motor Brake
 * PB4  Motor Thermal Flag
 * PB5  Programming MOSI
 * PB6  Programming MISO
 * PB7  Programming SCK
 *
 * PD0  Serial In
 * PD1  Serial Out
 * PD2  Encoder A
 * PD3  Encoder B
 * PD5  Motor PWM
 */

volatile unsigned char error;


volatile uint8_t serial_readbits;
volatile uint8_t serial_readchannels;
volatile uint8_t serial_readconfig;

struct values {
  /* Outputs */
  uint8_t motorBrake;
  uint16_t motorSpeed;
  uint8_t accelerometerSelfTest;

  /* Inputs */
  int16_t ballPosition;
  int16_t motorCurrent;
  int16_t accelerometerX;
  int16_t accelerometerY;
  int16_t accelerometerZ;
  int32_t motorEncoder;
  int8_t motorThermalFlag;
  int8_t ballOnBeam;
  uint8_t timer;
};
volatile struct values global;

SIGNAL(ADC_vect)
{
  unsigned char channel = ADMUX & 0x0f;
  unsigned int value = ADCW;

  if (global.timer) {
    global.timer--;
    PORTD &= ~0x40;
  } else {
    PORTD |= 0x40;
  }
  switch (channel) {
    case 0: { 
      channel = 1; 
      global.ballPosition = value;
    } break;
    case 1: { 
      channel = 2; 
      global.motorCurrent = value;
    } break;
    case 2: { 
      channel = 3; 
      global.accelerometerX = value;
    } break;
    case 3: { 
      channel = 4; 
      global.accelerometerY = value;
    } break;
    case 4: { 
      channel = 5; 
      global.accelerometerZ = value;
    } break;
    default: { 
      channel = 0; 
    } break;
  }
  ADMUX = 0x00 | channel; // Vref = external, right adjust
  ADCSRA = 0xcf;          // Enable ADC interrupts, Clock/128 
}

typedef enum { cmd_clear_bit, cmd_set_bit, 
	       cmd_read_bit, cmd_read_chan } command;

SIGNAL(USART_RXC_vect)
{
  char ch = UDR;
  
  global.timer = 0xff;
  switch (serialio_RXC(ch)) {
    case serialio_clearbit: {
      switch (serialio_channel) {
        case 0: { PORTB &= ~0x08; } break;
        case 1: { PORTC &= ~0x80; } break;
      }
    } break;
   case serialio_setbit: {
      switch (serialio_channel) {
        case 0: { PORTB |= 0x08; } break;
        case 1: { PORTC |= 0x80; } break;
      }
    } break;
    case serialio_pollbit: {
      if (serialio_channel < 1) { 
        serial_readbits |= (1<<serialio_channel);
      }
    } break;
    case serialio_pollchannel: { 
      if (serialio_channel < 6) { 
        serial_readchannels |= (1<<serialio_channel); 
      } else if (serialio_channel == 31) { 
        serial_readconfig = 1; 
      }
    } break;
    case serialio_setchannel: {
      switch (serialio_channel) {
	case 0: { 
          int16_t speed;
          if (serialio_value < 0x400) {
            speed = 0x400 - serialio_value;
            PORTB |= 0x02;
          } else {
            speed = serialio_value - 0x400;
            PORTB &= ~0x02;
          }          
	  OCR1A = speed & 0x3ff;
	} break;
      } break;
    } break;
    case serialio_error: {
    } break;
    case serialio_more: {
    } break;
  }
}

int main() 
{
  serialio_init();
  serial_readbits = 0;
  serial_readchannels = 0;
  serial_readconfig = 0;
  PORTA = 0x00;     // PortA, pull-ups
  DDRA = 0x00;      // PortA, all inputs
  PORTB = 0x00;     // PortB, pull-ups / initial values
  DDRB = 0x0a;      // PortB, bits 1 & 3 outputs
  PORTC = 0x00;     // PortC, pull-ups / initial values
  DDRC = 0x80;      // PortC, bit 7 output
  PORTD = 0x00;     // PortD, pull-ups / initial values
  DDRD = 0x20;      // PortD, bit 5 output
  TCCR0 = 0x05;	    // Timer0, Clock / 1024 
  TCCR1A = 0x83;    // OC1A 10 bit PWM (Phase Correct), clear A on match
  TCCR1B = 0x01;    // Clock / 1 
  OCR1A = 0 & 0x3ff;
  UCSRA = 0x00;     // USART: 
  UCSRB = 0x98;     // USART: RxIntEnable|RxEnable|TxEnable 
  UCSRC = 0x86;     // USART: 8bit, no parity
  UBRRH = 0;        // USART: 115200 @ 14.7456MHz
  UBRRL = 7;        // USART: 115200 @ 14.7456MHz
  ADMUX = 0x00;     // Vref = external, right adjust
  ADMUX = 0x0e;     // Vref = external, right adjust, read 1.22V (Vbg)
  ADCSRA = 0xcf;    // Enable ADC interrupts, Clock/128 

  SREG = 0x80;	    // Global interrupt enable 
  while (1) {
    unsigned char bits, channels, config;
    struct values local;

    SREG = 0x00;    // Global interrupt disable 
    bits = serial_readbits;
    serial_readbits = 0;
    channels = serial_readchannels;
    serial_readchannels = 0;
    config = serial_readconfig;
    serial_readconfig = 0;
    local = global;
    SREG = 0x80;    // Global interrupt enable 
    if (channels & 0x01) { serialio_putchannel(0, local.ballPosition); }
    if (channels & 0x02) { serialio_putchannel(1, local.motorCurrent); }
    if (channels & 0x04) { serialio_putchannel(2, local.accelerometerX); }
    if (channels & 0x08) { serialio_putchannel(3, local.accelerometerY); }
    if (channels & 0x10) { serialio_putchannel(4, local.accelerometerZ); }
    if (channels & 0x20) { serialio_putchannel(5, local.motorEncoder); }
    if (bits & 0x01) { serialio_putbit(0, PORTB & 0x10); }  // thermalFlag
    if (config) {
      CONF_DIGITAL_IN(0, CONF_RESOLUTION(1));   // motorTemp

      CONF_DIGITAL_OUT(0, CONF_RESOLUTION(1));  // motorBrake
      CONF_DIGITAL_OUT(1, CONF_RESOLUTION(1));  // accelerometerSelfTest
      
      CONF_ANALOG_IN(0, CONF_RESOLUTION(10));	// ballPosition
      CONF_ANALOG_IN(0, CONF_MIN(CONF_NEGATIVE_MILLIVOLT(10000)));
      CONF_ANALOG_IN(0, CONF_MAX(CONF_POSITIVE_MILLIVOLT(10000)));
      CONF_ANALOG_IN(1, CONF_RESOLUTION(10));	// motorCurrent
      CONF_ANALOG_IN(1, CONF_MIN(CONF_NEGATIVE_MILLIVOLT(10000)));
      CONF_ANALOG_IN(1, CONF_MAX(CONF_POSITIVE_MILLIVOLT(10000)));
      CONF_ANALOG_IN(2, CONF_RESOLUTION(10));	// accelerometerX
      CONF_ANALOG_IN(2, CONF_MIN(CONF_NEGATIVE_MILLIVOLT(10000)));
      CONF_ANALOG_IN(2, CONF_MAX(CONF_POSITIVE_MILLIVOLT(10000)));
      CONF_ANALOG_IN(3, CONF_RESOLUTION(10));	// accelerometerY
      CONF_ANALOG_IN(3, CONF_MIN(CONF_NEGATIVE_MILLIVOLT(10000)));
      CONF_ANALOG_IN(3, CONF_MAX(CONF_POSITIVE_MILLIVOLT(10000)));
      CONF_ANALOG_IN(4, CONF_RESOLUTION(10));	// accelerometerZ
      CONF_ANALOG_IN(4, CONF_MIN(CONF_NEGATIVE_MILLIVOLT(10000)));
      CONF_ANALOG_IN(4, CONF_MAX(CONF_POSITIVE_MILLIVOLT(10000)));
      CONF_ENCODER_IN(5, CONF_RESOLUTION(32));  // motorPosition
      
      CONF_ANALOG_OUT(0, CONF_RESOLUTION(11));	// motorSpeed
      CONF_ANALOG_OUT(0, CONF_MIN(CONF_NEGATIVE_MILLIVOLT(12000)));
      CONF_ANALOG_OUT(0, CONF_MAX(CONF_POSITIVE_MILLIVOLT(12000)));

      CONF_END();
    }
  }
}