Commit 45ce6676 authored by Anders Blomdell's avatar Anders Blomdell
Browse files

Change to new style I/O

Make TWI communication interrupt controlled
parent cf36849b
...@@ -10,7 +10,6 @@ ...@@ -10,7 +10,6 @@
#include <avr/io.h> #include <avr/io.h>
#include <avr/interrupt.h> #include <avr/interrupt.h>
#include <inttypes.h> #include <inttypes.h>
#include <compat/deprecated.h>
#include "vel_control.h" #include "vel_control.h"
#include "serialio.h" #include "serialio.h"
...@@ -44,16 +43,16 @@ ...@@ -44,16 +43,16 @@
static volatile uint8_t pccom_poll=0; static volatile uint8_t pccom_poll=0;
// Keep alive timer
volatile uint16_t nbrSamples = 0; // nbr of samples between ctrl-ref updates
// reference variables // reference variables
volatile int32_t vel_ref = 0; // 11 frac bits (variable in mm/s, max 2^12) volatile int32_t g_vel_ref = 0; // 11 frac bits (variable in mm/s, max 2^12)
volatile int32_t refCtrl = 0; // ref used in ctrl-loop (=ref sent from simulink) volatile int32_t refCtrl = 0; // ref used in ctrl-loop (=ref sent from simulink)
volatile int32_t acc_ref = 0; // 14 frac bits (variable in mm/s, max 2^15) volatile int32_t g_acc_ref = 0; // 14 frac bits (variable in mm/s, max 2^15)
volatile uint16_t nbrSamples = 0; // nbr of samples between ctrl-ref updates
// velocity control variables // velocity control variables
volatile int32_t u = 0; // 11 frac bits volatile int32_t u = 0; // 11 frac bits
volatile int32_t uSend = 0; // ctrl sent to simulink
volatile int32_t v = 0; // 11 frac bits volatile int32_t v = 0; // 11 frac bits
volatile int32_t vSat = 0; volatile int32_t vSat = 0;
volatile int8_t brake = 0; // brake variable if pos-sample missed volatile int8_t brake = 0; // brake variable if pos-sample missed
...@@ -62,7 +61,7 @@ volatile int32_t e = 0; // 11 frac bits ...@@ -62,7 +61,7 @@ volatile int32_t e = 0; // 11 frac bits
volatile int8_t intCond = 0; volatile int8_t intCond = 0;
volatile int32_t K = 807; // 6 frac bits, prop constant volatile int32_t K = 807; // 6 frac bits, prop constant
volatile int32_t Ke = 13; // 6 frac bits, integral constant volatile int32_t Ke = 13; // 6 frac bits, integral constant
volatile int8_t fr_comp = (10<<3); volatile int8_t fr_comp = (10<<3);
#define V_MAX (120<<4) #define V_MAX (120<<4)
...@@ -84,7 +83,6 @@ volatile int8_t oldY; // previous encoder signal ...@@ -84,7 +83,6 @@ volatile int8_t oldY; // previous encoder signal
// velocity estimation parameters // velocity estimation parameters
volatile int32_t velEst = 0; // vel-estimate, 5 frac bits volatile int32_t velEst = 0; // vel-estimate, 5 frac bits
volatile int32_t velEstSend = 0; // vel-estimate sent to simulink
int16_t a = 116; // 7 frac bits (parameter in velocity estimate) int16_t a = 116; // 7 frac bits (parameter in velocity estimate)
int16_t b = 152; // 5 frac bits (parameter in velocity estimate) int16_t b = 152; // 5 frac bits (parameter in velocity estimate)
...@@ -105,35 +103,44 @@ int32_t getPosition() { ...@@ -105,35 +103,44 @@ int32_t getPosition() {
/* return velocity (in mm/s) */ /* return velocity (in mm/s) */
int32_t getVelocity() { int32_t getVelocity() {
return velEstSend; int32_t result;
cli();
result = velEst;
sei();
return result;
} }
/* return last angle measurement */ /* return last angle measurement */
int16_t getAngle() { int16_t getAngle() {
low = inp(ADCL); int16_t adc = ADC;
high = inp(ADCH); while (adc != ADC) {
int16_t result = ADC - 512 -angleOffset; adc = ADC;
return result; }
return adc - 512 - angleOffset;
} }
/* return current-reference */ /* return current-reference */
int32_t getCurrentRef() { int32_t getCurrentRef() {
return uSend; int32_t result;
cli();
result = u;
sei();
return result;
} }
/* Set new acceleration reference value */ /* Set new acceleration reference value */
void setAccRef(int32_t newAccRef) { void setAccRef(int32_t newAccRef) {
acc_ref = newAccRef; g_acc_ref = newAccRef;
nbrSamples = 0; nbrSamples = 0;
} }
/* Set new velocity reference value */ /* Set new velocity reference value */
void setVelRef(int32_t newVelRef) { void setVelRef(int32_t newVelRef) {
vel_ref = newVelRef; g_vel_ref = newVelRef;
} }
/* Routine used to initialize the positional encoders */ /* Routine used to initialize the positional encoders */
...@@ -149,26 +156,26 @@ SIGNAL(TIMER2_COMP_vect) { ...@@ -149,26 +156,26 @@ SIGNAL(TIMER2_COMP_vect) {
newX = ENCODERX; newX = ENCODERX;
newY = ENCODERY; newY = ENCODERY;
if((newX != oldX) || (newY != oldY)) { //Check if any value changed if((newX != oldX) || (newY != oldY)) { //Check if any value changed
/* /*
sum = (oldX<<2)+oldY+newX+(newY>>2); sum = (oldX<<2)+oldY+newX+(newY>>2);
if (sum == 2 || sum == 4 || sum == 11 || sum == 13) { if (sum == 2 || sum == 4 || sum == 11 || sum == 13) {
pos = pos+1; pos = pos+1;
} else if (sum == 1 || sum == 7 || sum == 8 || sum == 14) { } else if (sum == 1 || sum == 7 || sum == 8 || sum == 14) {
pos = pos-1; pos = pos-1;
} else { } else {
brake = 1; // emergency brake brake = 1; // emergency brake
} }
*/ */
// Works like if-clauses above, but faster! // Works like if-clauses above, but faster!
if ((oldX == newY) && (oldY != newX)) { if ((oldX == newY) && (oldY != newX)) {
pos = pos+1; pos = pos+1;
} else if ((oldX != newY) && (oldY == newX)) { } else if ((oldX != newY) && (oldY == newX)) {
pos = pos-1; pos = pos-1;
} else { } else {
brake = 1; brake = 1;
} }
oldX = newX; oldX = newX;
oldY = newY; oldY = newY;
} }
} }
...@@ -214,59 +221,50 @@ SIGNAL(USART_RXC_vect) { ...@@ -214,59 +221,50 @@ SIGNAL(USART_RXC_vect) {
} }
} }
#if 0 SIGNAL(TWI_vect) {
/* Timer 0, serial communication with simulink */ unsigned char twsr = TWSR;
SIGNAL(TIMER0_COMP_vect) { switch (twsr) {
case 0x08: {
TIMSK &= ~(_BV(OCIE0)|_BV(OCIE1A)); // Disable communication and ctrl-interrupts TWDR = 0x02; // slave is 0x02 (sla+w)
TWCR = _BV(TWINT)|_BV(TWEN)|_BV(TWIE);
sei(); // enable interrupts from encoder-counter } break;
case 0x18:
// Poll UART receiver case 0x20: {
uint8_t status = UCSRA; TWDR = u;
if (status & (1<<RXC)) { TWCR = _BV(TWINT)|_BV(TWEN)|_BV(TWIE);
char ch = UDR; } break;
pccom_receiveByte(ch); case 0x28:
case 0x30: {
if (status & ((1<<FE)|(1<<DOR)|(1<<PE))) { TWCR = _BV(TWINT)|_BV(TWEN)|_BV(TWSTO)|_BV(TWIE);
//main_emergencyStop(); // stop on USART error } break;
} default: {
} // This should never happen
TWCR = (_BV(TWINT)|_BV(TWEN)|_BV(TWIE));
// Poll UART sender } break;
if (UCSRA & (1<<UDRE)) {
int16_t toSend = pccom_getNextByteToSend();
if (toSend >= 0) UDR = (char)toSend;
} }
TIFR = (1<<OCF0); // skip pending interrupts from serial comm, (but not from ctrl)
TIMSK |= (_BV(OCIE0)|_BV(OCIE1A)); // reenable communication and ctrl-interrupts
} }
#endif
/* Timer 0, control loop , 1 kHz */ /* Timer 0, control loop , 1 kHz */
SIGNAL(TIMER1_COMPA_vect) { SIGNAL(TIMER1_COMPA_vect) {
posCtrl = pos; // store pos to use in this loop posCtrl = pos; // store pos to use in this loop
int32_t vel_ref = g_vel_ref;
sei(); // to enable interupts from encoder counter and communication int32_t acc_ref = g_acc_ref;
if (nbrSamples < 65535) {
nbrSamples++;
}
sei(); // to enable interupts from encoder counter and TWI
// velocity estimation in mm/s // velocity estimation in mm/s
velEst = (((a*velEst+64)>>7)+b*(posCtrl-oldPos)); // 5 fracbits on velEst velEst = (((a*velEst+64)>>7)+b*(posCtrl-oldPos)); // 5 fracbits on velEst
oldPos = posCtrl; oldPos = posCtrl;
// store velEst and ref to be sent/used here // store velEst and ref to be sent/used here
cli(); // cli();
velEstSend = velEst;
nbrSamples++;
refCtrl = vel_ref + ((acc_ref*nbrSamples)>>10); //shift nbrSamples 10 steps (= nbrSamples*h) refCtrl = vel_ref + ((acc_ref*nbrSamples)>>10); //shift nbrSamples 10 steps (= nbrSamples*h)
if (nbrSamples > 500) { if (nbrSamples > 500) {
refCtrl = 0; // for safety reasons if doesn't send anything in 0.5 s refCtrl = 0; // for safety reasons if doesn't send anything in 0.5 s
} }
// vel_ref = vel_ref*(1-brake); // emergency stop // vel_ref = vel_ref*(1-brake); // emergency stop
sei();
// control error // control error
e = refCtrl-((velEst+16)>>5); // mm/s e = refCtrl-((velEst+16)>>5); // mm/s
...@@ -300,32 +298,18 @@ SIGNAL(TIMER1_COMPA_vect) { ...@@ -300,32 +298,18 @@ SIGNAL(TIMER1_COMPA_vect) {
if (intCond) if (intCond)
I = I + (((Ke*e)+(1<<1))>>2); I = I + (((Ke*e)+(1<<1))>>2);
// Protect u and TWCR for concurrent updates
cli();
// scale ctrl-signal to send over twi // scale ctrl-signal to send over twi
u = (vSat+8)>>4; // u=127 gives current = 6.75 A, vSat makes u saturate at 114 u = (vSat+8)>>4; // u=127 gives current = 6.75 A, vSat makes u saturate at 114
// u that is sent to simulink
cli();
uSend = u;
sei();
// TWI-communication to set current reference on the other atmel // TWI-communication to set current reference on the other atmel
// send start command // send start command
outp(_BV(TWINT)|_BV(TWEN)|_BV(TWSTA),TWCR); if (TWCR == (_BV(TWEN)|_BV(TWIE)) || TWCR == 0) {
while (!(TWCR&_BV(TWINT))) {} TWCR = _BV(TWEN)|_BV(TWSTA)|_BV(TWIE);
}
// Contact slave
outp(0x02,TWDR); // slave is 0x02 (sla+w)
outp(_BV(TWINT)|_BV(TWEN),TWCR);
while (!(TWCR&_BV(TWINT))) {}
// Send reference byte to slave
outp((int8_t)u,TWDR); // send 8 bits reference
outp(_BV(TWINT)|_BV(TWEN),TWCR);
while (!(TWCR&_BV(TWINT))) {}
// stop transmission
outp(_BV(TWINT)|_BV(TWEN)|_BV(TWSTO),TWCR);
} }
...@@ -334,36 +318,35 @@ int main() ...@@ -334,36 +318,35 @@ int main()
cli(); cli();
//Port directions //Port directions
//outp(0xf0,DDRB); // Led output PORTC = 0x03; // Pull up for systems without TWI resistors
outp(0x10,DDRC); // timer calculation port
PORTD = 0x40; // pull up on reset switch PORTD = 0x40; // pull up on reset switch
/* Timer section */ /* Timer section */
// Enable timer0, timer1, timer2 compare match interrupts // Enable timer1, timer2 compare match interrupts
outp(_BV(OCIE0)|_BV(OCIE1A)|_BV(OCIE2),TIMSK); TIMSK = _BV(OCIE1A)|_BV(OCIE2);
outp(_BV(OCIE1A)|_BV(OCIE2),TIMSK);
/* Timer 2, 73 kHz Prescaler 1, encoder counter for position measurement */ /* Timer 2, 73 kHz Prescaler 1, encoder counter for position measurement */
outp(_BV(WGM21)|_BV(CS20),TCCR2); TCCR2 = _BV(WGM21)|_BV(CS20);
outp(200,OCR2); OCR2 = 200;
/* Reset timer 2 */ /* Reset timer 2 */
outp(0,TCNT2); TCNT2 = 0;
/* Timer 1, 1 kHz , prescaler 1, ctrl-loop */ /* Timer 1, 1 kHz , prescaler 1, ctrl-loop */
outp(_BV(WGM12)|_BV(CS10),TCCR1B); TCCR1B = _BV(WGM12)|_BV(CS10);
outp(0x38,OCR1AH); OCR1AH = 0x38;
outp(0x3f,OCR1AL); OCR1AL = 0x3f;
outp(0,TCNT1H); TCNT1H = 0;
outp(0,TCNT1L); TCNT1L = 0;
/* Timer 0, 40 kHz, Prescaler 8, serial communication */ /* Timer 0, 40 kHz, Prescaler 8, serial communication */
outp(_BV(WGM01)|_BV(CS01),TCCR0); TCCR0 = _BV(WGM01)|_BV(CS01);
//outp(184-1,OCR0); // 10 kHz //OCR0 = 184-1; // 10 kHz
outp(45-1,OCR0); // 40 kHz OCR0 = 45-1; // 40 kHz
/* Reset timer 0 */ /* Reset timer 0 */
outp(0,TCNT0); TCNT0 = 0;
// syncronization (ctrl interrupt 34 micros before communication interrupt) // syncronization (ctrl interrupt 34 micros before communication interrupt)
...@@ -371,11 +354,11 @@ int main() ...@@ -371,11 +354,11 @@ int main()
//Serial communication initialization //Serial communication initialization
outp(0x00, UCSRA); // USART: UCSRA = 0x00; // USART:
outp(0x18, UCSRB); // USART: RxEnable|TxEnable UCSRB = 0x18; // USART: RxEnable|TxEnable
outp(0x86, UCSRC); // USART: 8bit, no parity UCSRC = 0x86; // USART: 8bit, no parity
outp(0x00, UBRRH); // USART: 115200 @ 14.7456MHz UBRRH = 0x00; // USART: 115200 @ 14.7456MHz
outp(7,UBRRL); // USART: 115200 @ 14.7456MHz UBRRL = 7; // USART: 115200 @ 14.7456MHz
UCSRA = 0x00; // USART: UCSRA = 0x00; // USART:
UCSRB = 0x98; // USART: RxIntEnable|RxEnable|TxEnable UCSRB = 0x98; // USART: RxIntEnable|RxEnable|TxEnable
...@@ -385,15 +368,15 @@ int main() ...@@ -385,15 +368,15 @@ int main()
/* AREF (AREF is 5V) pin external capacitor, ADC3 for pendulum angle */ /* AREF (AREF is 5V) pin external capacitor, ADC3 for pendulum angle */
outp(_BV(REFS0)|_BV(MUX0)|_BV(MUX1),ADMUX); ADMUX = _BV(REFS0)|_BV(MUX0)|_BV(MUX1);
// Enable ADC on adc3, start first conversion, prescaler 128, free running mode // Enable ADC on adc3, start first conversion, prescaler 128, free running mode
outp(_BV(ADEN)|_BV(ADSC)|_BV(ADATE)|_BV(ADPS2)|_BV(ADPS1)|_BV(ADPS0),ADCSRA); ADCSRA = _BV(ADEN)|_BV(ADSC)|_BV(ADATE)|_BV(ADPS2)|_BV(ADPS1)|_BV(ADPS0);
// Initialize Master TWI // Initialize Master TWI
outp(0x10,TWBR); // set SCL-frequency CPU-freq/(16+2*16) TWBR = 0x10; // set SCL-frequency CPU-freq/(16+2*16)
outp(_BV(TWEN),TWCR); // enable TWI TWCR = _BV(TWEN)|_BV(TWIE);
// initialize position measurements // initialize position measurements
initPos(); initPos();
...@@ -404,6 +387,7 @@ int main() ...@@ -404,6 +387,7 @@ int main()
//Enable interrupts //Enable interrupts
sei(); sei();
// loop // loop
while (1) { while (1) {
...@@ -411,8 +395,8 @@ int main() ...@@ -411,8 +395,8 @@ int main()
// reset position, velocity estimate and integral part of ctrl if reset button pushed // reset position, velocity estimate and integral part of ctrl if reset button pushed
if (!(PIND & 0x40)) { if (!(PIND & 0x40)) {
cli(); cli();
low = inp(ADCL); low = ADCL;
high = inp(ADCH); high = ADCH;
pos = 0; // reset position pos = 0; // reset position
angleOffset = ((int16_t) ((high<<8) | low) - 512); angleOffset = ((int16_t) ((high<<8) | low) - 512);
oldPos = 0; oldPos = 0;
......
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