Commit e3797c11 authored by Sam Calisch's avatar Sam Calisch

added firmware

parent 34c3252a
#include <avr/io.h>
#include <util/delay.h>
#include <avr/pgmspace.h>
#define output(directions,pin) (directions |= pin) // set port direction for output
#define set(port,pin) (port |= pin) // set port pin
#define clear(port,pin) (port &= (~pin)) // clear port pin
#define pin_test(pins,pin) (pins & pin) // test for port pin
#define bit_test(byte,bit) (byte & (1 << bit)) // test for bit set
/* USART buffer defines. */
/* \brief Receive buffer size: 2,4,8,16,32,64,128 or 256 bytes. */
#define USART_RX_BUFFER_SIZE 128
/* \brief Transmit buffer size: 2,4,8,16,32,64,128 or 256 bytes */
#define USART_TX_BUFFER_SIZE 128
/* \brief Receive buffer mask. */
#define USART_RX_BUFFER_MASK ( USART_RX_BUFFER_SIZE - 1 )
/* \brief Transmit buffer mask. */
#define USART_TX_BUFFER_MASK ( USART_TX_BUFFER_SIZE - 1 )
#if ( USART_RX_BUFFER_SIZE & USART_RX_BUFFER_MASK )
#error RX buffer size is not a power of 2
#endif
#if ( USART_TX_BUFFER_SIZE & USART_TX_BUFFER_MASK )
#error TX buffer size is not a power of 2
#endif
/* \brief USART transmit and receive ring buffer. */
typedef struct USART_Buffer
{
/* \brief Receive buffer. */
volatile uint8_t RX[USART_RX_BUFFER_SIZE];
/* \brief Transmit buffer. */
volatile uint8_t TX[USART_TX_BUFFER_SIZE];
/* \brief Receive buffer head. */
volatile uint8_t RX_Head;
/* \brief Receive buffer tail. */
volatile uint8_t RX_Tail;
/* \brief Transmit buffer head. */
volatile uint8_t TX_Head;
/* \brief Transmit buffer tail. */
volatile uint8_t TX_Tail;
} USART_Buffer_t;
/*! \brief Struct used when interrupt driven driver is used.
*
* Struct containing pointer to a usart, a buffer and a location to store Data
* register interrupt level temporary.
*/
typedef struct Usart_and_buffer
{
/* \brief Pointer to USART module to use. */
USART_t * usart;
/* \brief Data register empty interrupt level. */
USART_DREINTLVL_t dreIntLevel;
/* \brief Data buffer. */
USART_Buffer_t buffer;
} USART_data_t;
#define USART_RxdInterruptLevel_Set(_usart, _rxdIntLevel) \
((_usart)->CTRLA = ((_usart)->CTRLA & ~USART_RXCINTLVL_gm) | _rxdIntLevel)
#define USART_Format_Set(_usart, _charSize, _parityMode, _twoStopBits) \
(_usart)->CTRLC = (uint8_t) _charSize | _parityMode | \
(_twoStopBits ? USART_SBMODE_bm : 0)
#define USART_Baudrate_Set(_usart, _bselValue, _bScaleFactor) \
(_usart)->BAUDCTRLA =(uint8_t)_bselValue; \
(_usart)->BAUDCTRLB =(_bScaleFactor << USART_BSCALE0_bp)|(_bselValue >> 8)
#define USART_Rx_Enable(_usart) ((_usart)->CTRLB |= USART_RXEN_bm)
#define USART_Rx_Disable(_usart) ((_usart)->CTRLB &= ~USART_RXEN_bm)
#define USART_Tx_Enable(_usart) ((_usart)->CTRLB |= USART_TXEN_bm)
#define USART_Tx_Disable(_usart) ((_usart)->CTRLB &= ~USART_TXEN_bm)
_Bool USART_TXBuffer_FreeSpace(USART_data_t * usart_data)
{
/* Make copies to make sure that volatile access is specified. */
uint8_t tempHead = (usart_data->buffer.TX_Head + 1) & USART_TX_BUFFER_MASK;
uint8_t tempTail = usart_data->buffer.TX_Tail;
/* There are data left in the buffer unless Head and Tail are equal. */
return (tempHead != tempTail);
}
_Bool USART_TXBuffer_PutByte(USART_data_t * usart_data, uint8_t data)
{
uint8_t tempCTRLA;
uint8_t tempTX_Head;
_Bool TXBuffer_FreeSpace;
USART_Buffer_t * TXbufPtr;
TXbufPtr = &usart_data->buffer;
TXBuffer_FreeSpace = USART_TXBuffer_FreeSpace(usart_data);
if(TXBuffer_FreeSpace)
{
tempTX_Head = TXbufPtr->TX_Head;
TXbufPtr->TX[tempTX_Head]= data;
/* Advance buffer head. */
TXbufPtr->TX_Head = (tempTX_Head + 1) & USART_TX_BUFFER_MASK;
/* Enable DRE interrupt. */
tempCTRLA = usart_data->usart->CTRLA;
tempCTRLA = (tempCTRLA & ~USART_DREINTLVL_gm) | usart_data->dreIntLevel;
usart_data->usart->CTRLA = tempCTRLA;
}
return TXBuffer_FreeSpace;
}
_Bool USART_RXBufferData_Available(USART_data_t * usart_data)
{
/* Make copies to make sure that volatile access is specified. */
uint8_t tempHead = usart_data->buffer.RX_Head;
uint8_t tempTail = usart_data->buffer.RX_Tail;
/* There are data left in the buffer unless Head and Tail are equal. */
return (tempHead != tempTail);
}
uint8_t USART_RXBuffer_GetByte(USART_data_t * usart_data)
{
USART_Buffer_t * bufPtr;
uint8_t ans;
bufPtr = &usart_data->buffer;
ans = (bufPtr->RX[bufPtr->RX_Tail]);
/* Advance buffer tail. */
bufPtr->RX_Tail = (bufPtr->RX_Tail + 1) & USART_RX_BUFFER_MASK;
return ans;
}
_Bool USART_RXComplete(USART_data_t * usart_data)
{
USART_Buffer_t * bufPtr;
_Bool ans;
bufPtr = &usart_data->buffer;
/* Advance buffer head. */
uint8_t tempRX_Head = (bufPtr->RX_Head + 1) & USART_RX_BUFFER_MASK;
/* Check for overflow. */
uint8_t tempRX_Tail = bufPtr->RX_Tail;
uint8_t data = usart_data->usart->DATA;
if (tempRX_Head == tempRX_Tail) {
ans = 0;
}else{
ans = 1;
usart_data->buffer.RX[usart_data->buffer.RX_Head] = data;
usart_data->buffer.RX_Head = tempRX_Head;
}
return ans;
}
void USART_DataRegEmpty(USART_data_t * usart_data)
{
USART_Buffer_t * bufPtr;
bufPtr = &usart_data->buffer;
/* Check if all data is transmitted. */
uint8_t tempTX_Tail = usart_data->buffer.TX_Tail;
if (bufPtr->TX_Head == tempTX_Tail){
/* Disable DRE interrupts. */
uint8_t tempCTRLA = usart_data->usart->CTRLA;
tempCTRLA = (tempCTRLA & ~USART_DREINTLVL_gm) | USART_DREINTLVL_OFF_gc;
usart_data->usart->CTRLA = tempCTRLA;
}else{
/* Start transmitting. */
uint8_t data = bufPtr->TX[usart_data->buffer.TX_Tail];
usart_data->usart->DATA = data;
/* Advance buffer tail. */
bufPtr->TX_Tail = (bufPtr->TX_Tail + 1) & USART_TX_BUFFER_MASK;
}
}
void USART_InterruptDriver_Initialize(USART_data_t * usart_data,
USART_t * usart,
USART_DREINTLVL_t dreIntLevel)
{
usart_data->usart = usart;
usart_data->dreIntLevel = dreIntLevel;
usart_data->buffer.RX_Tail = 0;
usart_data->buffer.RX_Head = 0;
usart_data->buffer.TX_Tail = 0;
usart_data->buffer.TX_Head = 0;
}
void USART_InterruptDriver_DreInterruptLevel_Set(USART_data_t * usart_data,
USART_DREINTLVL_t dreIntLevel)
{
usart_data->dreIntLevel = dreIntLevel;
}
void usart_send_byte(USART_data_t * usart_data, char msg){
while(!USART_TXBuffer_FreeSpace(usart_data)){}
USART_TXBuffer_PutByte(usart_data,msg);
}
void usart_send_uint32(USART_data_t * usart_data, uint32_t n){
unsigned char * nn = (unsigned char *)&n;
usart_send_byte(usart_data, nn[0] );
usart_send_byte(usart_data, nn[1] );
usart_send_byte(usart_data, nn[2] );
usart_send_byte(usart_data, nn[3] );
}
void usart_send_int32(USART_data_t * usart_data, int32_t n){
unsigned char * nn = ( unsigned char *)&n;
usart_send_byte(usart_data, nn[0] );
usart_send_byte(usart_data, nn[1] );
usart_send_byte(usart_data, nn[2] );
usart_send_byte(usart_data, nn[3] );
}
void usart_send_uint16(USART_data_t * usart_data, uint16_t n){
unsigned char * nn = (unsigned char *)&n;
usart_send_byte(usart_data, nn[0] );
usart_send_byte(usart_data, nn[1] );
}
void usart_send_int16(USART_data_t * usart_data, int16_t n){
unsigned char * nn = ( unsigned char *)&n;
usart_send_byte(usart_data, nn[0] );
usart_send_byte(usart_data, nn[1] );
}
uint32_t parse_uint32(char* b){return *(uint32_t *) b;}
int32_t parse_int32(char* b){return *(int32_t *) b;}
uint16_t parse_uint16(char* b){return *(uint16_t *) b;}
/*
void usart_write_buffer(char* buffer){
//write \n terminated buffer over usart
int j=0;
do{
usart_send_byte( buffer[j] );
j++;
} while( j < PACKET_SIZE);
//} while( buffer[j-1] != 10);
}
*/
This diff is collapsed.
PROJECT=sewing-driver
SOURCES=$(PROJECT).c
MMCU=atxmega32a4u
F_CPU = 32000000
TARGET = x32a4u
PROGRAMMER= atmelice_pdi
CFLAGS=-mmcu=$(MMCU) -Wall -Os -DF_CPU=$(F_CPU)
$(PROJECT).hex: $(PROJECT).out
avr-objcopy -O ihex $(PROJECT).out $(PROJECT).c.hex;\
avr-size --mcu=$(MMCU) --format=avr $(PROJECT).out
$(PROJECT).out: $(SOURCES)
avr-gcc $(CFLAGS) -I./ -o $(PROJECT).out $(SOURCES)
program: $(PROJECT).hex
avrdude -p $(TARGET) -c $(PROGRAMMER) -U flash:w:$(PROJECT).c.hex
#!/usr/bin/env python
from numpy import cumsum,asarray,arange
from math import *
import serial, time
import struct
import sys
import pylab as plt
plt.style.use('bmh')
def read_int32(bytes,signed=False):
try:
n = struct.unpack("<i",bytearray(bytes))[0] if signed else struct.unpack("<I",bytearray(bytes))[0]
return n
except:
print "not properly formatted bytes: ",list(bytes)
return None
def read_int16(bytes,signed=False):
try:
n = struct.unpack("<h",bytearray(bytes))[0] if signed else struct.unpack("<H",bytearray(bytes))[0]
return n
except:
print "not properly formatted bytes: ",list(bytes)
return None
def encode_int32(n,signed=False):
b = struct.pack("<i",n) if signed else struct.pack("<I",n)
return list(b)
def encode_int16(n,signed=False):
b = struct.pack("<h",n) if signed else struct.pack("<H",n)
return list(b)
def encode_char(n,signed=False):
b = struct.pack("<b",n) if signed else struct.pack("<B",n)
return list(b)
def _readline(ser=None):
if ser is None:
time.sleep(.05)
return bytearray([200,200,200,200,200]) + bytearray([0 for i in range(44)]) + bytearray([201,201,201,201,201])
else:
line = bytearray()
while True:
c = ser.read(1)
if c:
line += c
if line[-5:] == bytearray([201,201,201,201,201]):
#if len(line) in [11,46]:
if len(line) in [11,82]:
break
else:
print "improper line length: ",len(line)
else:
break
return line
def write(cmd, ser=None):
if ser is None:
return 0
else:
#print cmd
cmd += bytearray([sum(cmd)%256]) #add simple checksum
c = bytearray([200,200,200,200,200]) + cmd + bytearray([201,201,201,201,201])
#print 'sending: ',list(c)
ser.write(c)
#time.sleep(.01)
rtrn = _readline(ser=ser)
if len(rtrn)<6 or rtrn[5] != 6:
print "didn't get checksum acknowledgement!"
print 'return: ',list(rtrn)
sys.exit(0)
#else:
# print "command acknowledged!", list(rtrn)
def request_state_command():
return bytearray([110])
def request_state(ser=None):
write(request_state_command(),ser=ser)
def read_state(ser=None):
request_state(ser=ser)
line = _readline(ser=ser)
#print "received: ",list(line)
if len(line)>4 and line[0]==200 and line[1]==200 and line[2]==200 and line[3]==200 and line[4]==200:
line = line.strip('\n')[5:] #ha, make sure we strip outside the framing, not the inside!!!
#items = [ read_int16(map(int,line[x:x+2]),signed=False) for x in xrange(0,len(line)-5,2) ]
#items = [ read_int16(line[x:x+2],signed=False) for x in xrange(0,len(line)-5,2) ]
#items = [ read_int32(line[x:x+4],signed=True) for x in xrange(0,6*4,4) ] + \
items = [ read_int32(line[x:x+4],signed=True) for x in xrange(0,15*4,4) ] + \
[ read_int16(line[x:x+2],signed=False) for x in xrange(6*4,len(line)-5,2) ]
print 'items: ',items
if None not in items:
return items
else:
print 'garbage: ', list(line)
else:
print 'didnt get framing: ',read_int16(line.strip('\n'),signed=True)
sys.exit(0)
def pwm_command(a,ad,b,bd,c,cd):
#a and b are 16 bit ints, we break them into bytes and send
return bytearray([100]\
+ encode_int16(a,signed=False)\
+ encode_char(ad,signed=False)\
+ encode_int16(b,signed=False)\
+ encode_char(bd,signed=False)\
+ encode_int16(c,signed=False)\
+ encode_char(cd,signed=False))
def posd_command(a,b,c):
#a and b are 16 bit ints, we break them into bytes and send
return bytearray([101]\
+ encode_int32(a,signed=True)\
+ encode_int32(b,signed=True)\
+ encode_int32(c,signed=True))
def stop_command():
return bytearray([102])
def main():
try:
ser = serial.Serial(port='/dev/tty.usbserial-FT9L39VL',baudrate=230400,timeout=1.)
ser.isOpen()
except(OSError):
#no serial port
print "Couldn't find the serial port, entering debug mode"
ser = None
print "Starting up..."
#motor1: take-up
#motor2: bobbin
#motor3: needle
N = 100
dt = .03
amp = 1000
times = [dt*i for i in range(N)]#[0, 1., 2., 3.]
f = lambda i: int(amp/2*(1-cos(2*pi*i/(N-1))))
#pwms = [(f(i), 1, 0, 0 ,0, 0) for i,t in enumerate(times)]#[(0,0,0), (255,1000,300), (0,0,0),(0,0,0)]
posds = [(0,f(i),0) for i,t in enumerate(times)]
#posds = [(0,0,0) for i,t in enumerate(times)]
data = []
#assert (len(times)==len(pwms))
t0 = time.time()
current_frame = 0
current_time = lambda: time.time() - t0
try:
while( current_time() <= times[-1] ):
if current_time() >= times[current_frame]:
#print 'writing frame %d'%current_frame
#write( pwm_command( *pwms[current_frame] ) , ser=ser)
write( posd_command( *posds[current_frame] ) , ser=ser)
data.append(read_state(ser=ser))
#pos = data[-1][:3]
current_frame += 1
except(KeyboardInterrupt):
write( pwm_command(0,0,0,0,0,0) , ser=ser)
print 'quitting'
if ser is not None:
ser.close()
sys.exit(0)
print "Finished sending keyframes"
write(stop_command())
#write( posd_command(pos[0],pos[1],pos[2]) , ser=ser)
#write( pwm_command(0,0,0,0,0,0) , ser=ser)
with open('sewing_data.csv', 'w') as f:
for l in data:
f.write( ','.join(map(str,l))+'\n' )
data = asarray(data)
fig,axs = plt.subplots(4,1,figsize=(8,8))
axs[0].plot(data[:,1],label='pos_2')
axs[0].plot(data[:,4],label='posd_2')
axs[0].plot(data[:,1]-data[:,4],label='e_2')
axs[1].plot(data[:,7],label='de_2')
axs[2].plot(data[:,10],label='ie_2')
#axs[1].plot(data[:,-6],label='pwm1')
axs[3].plot(data[:,13],label='out2')
axs[3].plot(data[:,-5],label='pwm2')
#axs[1].plot(data[:,-4],label='pwm3')
for i in range(4):
axs[i].legend(loc=(.9,0))
plt.show()
if __name__ == '__main__':
main()
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment