Loading files
This file is pinned to the selected release. Download the original to open it in your design tool.
#include <stdlib.h>
#include <wirish/wirish.h>
#include <string.h>
#include "dxl_adc.h"
#include "dxl_protocol.h"
// A/D
int directPins[] = {3, 4, 5, 6, 7};
#define DIRECTPINS (sizeof(directPins)/sizeof(int))
#define ADC_CHANNELS 5
#define ADC_WINDOW 16
#define ADC_ADDR 0x24
/**
* Samples a channel (abstracting the multiplexer)
* - 0 -> 7: direct pins
*/
int sampleChannel(int channel)
{
if (channel < DIRECTPINS) {
return analogRead(directPins[channel]);
}
}
/**
* This flag is set to true @1Khz (see below)
*/
static bool sample = false;
struct dxl_adc_private
{
struct dxl_registers registers;
unsigned int sums[ADC_CHANNELS];
unsigned int windows[ADC_CHANNELS][ADC_WINDOW];
};
/**
* The tick function do a sample if necessary and store it in the
* data stucture
*/
static void dxl_adc_tick(volatile struct dxl_device *self)
{
volatile struct dxl_adc_private *data = (volatile dxl_adc_private *)self->data;
static int currentChannel = 0;
static int cursor = 0;
if (sample) {
sample = false;
currentChannel = 0;
cursor = (cursor+1);
if (cursor >= ADC_WINDOW) {
cursor = 0;
}
}
if (currentChannel < ADC_CHANNELS) {
int channel = currentChannel;
int newSample = sampleChannel(channel);
data->sums[channel] -= data->windows[channel][cursor];
data->windows[channel][cursor] = newSample;
data->sums[channel] += data->windows[channel][cursor];
currentChannel++;
}
}
static bool dxl_adc_check_id(volatile struct dxl_device *self, ui8 id)
{
volatile struct dxl_adc_private *data = (volatile dxl_adc_private *)self->data;
return (id == data->registers.eeprom.id);
}
static void dxl_adc_write_data(volatile struct dxl_device *self, ui8 id, ui8 addr, ui8 *values, ui8 length)
{
volatile struct dxl_adc_private *data = (volatile dxl_adc_private *)self->data;
memcpy(((ui8 *)(&data->registers))+addr, values, length);
}
static void dxl_adc_read_data(volatile struct dxl_device *self, ui8 id, ui8 addr, ui8 *values, ui8 length, ui8 *error)
{
volatile struct dxl_adc_private *data = (volatile dxl_adc_private *)self->data;
if (addr == ADC_ADDR) {
unsigned short adcValues[ADC_CHANNELS];
for (int i=0; i<ADC_CHANNELS; i++) {
adcValues[i] = data->sums[i]/ADC_WINDOW;
}
memcpy(values, ((ui8*)adcValues), length);
} else {
memcpy(values, ((ui8*)&data->registers)+addr, length);
}
*error = 0;
}
static void dxl_adc_process(volatile struct dxl_device *self, volatile struct dxl_packet *packet)
{
dxl_process(self, packet, dxl_adc_check_id, dxl_adc_write_data, dxl_adc_read_data);
}
static void timer_tick()
{
sample = true;
}
static void dxl_adc_init_hardware()
{
// Timer1 @1Khz
HardwareTimer timer(1);
timer.pause();
timer.setPrescaleFactor(7200);
timer.setOverflow(10);
timer.setCompare(1, 1);
timer.attachInterrupt(1, timer_tick);
timer.refresh();
timer.resume();
// All the direct sampling pins are input analog
for (int i=0; i<DIRECTPINS; i++) {
pinMode(directPins[i], INPUT_ANALOG);
}
}
void dxl_adc_init(volatile struct dxl_device *device, ui8 id)
{
int i, j;
static bool timer_inited = false;
if (!timer_inited) {
timer_inited = true;
dxl_adc_init_hardware();
}
volatile struct dxl_adc_private *data = (volatile struct dxl_adc_private *)malloc(sizeof(struct dxl_adc_private));
dxl_device_init(device);
device->tick = dxl_adc_tick;
device->process = dxl_adc_process;
device->data = (void *)data;
data->registers.eeprom.modelNumber = DXL_ADC_MODEL;
data->registers.eeprom.firmwareVersion = 1;
data->registers.eeprom.id = id;
data->registers.ram.presentPosition = 0;
data->registers.ram.goalPosition = 0;
data->registers.ram.torqueEnable = 0;
data->registers.ram.torqueLimit = 0;
for (i=0; i<ADC_CHANNELS; i++) {
data->sums[i] = 0;
for (j=0; j<ADC_WINDOW; j++) {
data->windows[i][j] = 0;
}
}
}
8fb290cbdThis file is pinned to the selected release. Download the original to open it in your design tool.