Commit 88702ed0 authored by Anders Blomdell's avatar Anders Blomdell
Browse files

Making vel and acceleration ref active at the same time.

parent 07e26162
......@@ -43,23 +43,30 @@
static volatile uint8_t pccom_poll=0;
// Keep alive timer
volatile uint16_t nbrSamples = 0; // nbr of samples between ctrl-ref updates
// reference variables
volatile int32_t g_vel_ref = 0; // 11 frac bits (variable in mm/s, max 2^12)
struct {
volatile uint16_t samples; // nbr of samples since last ctrl-ref update
struct {
volatile unsigned char pending;
volatile int32_t next;
volatile int32_t value;
} vel;
struct {
volatile int32_t value;
} acc;
} ref = {
.samples = 0,
.vel.pending = 0,
.vel.next = 0,
.vel.value = 0,
.acc.value = 0,
};
volatile int32_t refCtrl = 0; // ref used in ctrl-loop (=ref sent from simulink)
volatile int32_t g_acc_ref = 0; // 14 frac bits (variable in mm/s, max 2^15)
// velocity control variables
volatile int32_t u = 0; // 11 frac bits
volatile int32_t v = 0; // 11 frac bits
volatile int32_t vSat = 0;
volatile int8_t brake = 0; // brake variable if pos-sample missed
volatile int32_t I = 0; // 11 frac bits
volatile int32_t e = 0; // 11 frac bits
volatile int8_t intCond = 0;
volatile int32_t K = 807; // 6 frac bits, prop constant
volatile int32_t Ke = 13; // 6 frac bits, integral constant
......@@ -133,14 +140,23 @@ int32_t getCurrentRef() {
/* Set new acceleration reference value */
void setAccRef(int32_t newAccRef) {
g_acc_ref = newAccRef;
nbrSamples = 0;
// Called from serial interrupt, so should be atomic by itself
if (ref.vel.pending) {
ref.vel.pending = 0;
ref.vel.value = ref.vel.next;
ref.acc.value = newAccRef;
ref.samples = 0;
} else {
// TODO: report error
}
}
/* Set new velocity reference value */
void setVelRef(int32_t newVelRef) {
g_vel_ref = newVelRef;
// Called from serial interrupt, so should be atomic by itself
ref.vel.pending = 1;
ref.vel.next = newVelRef;
}
/* Routine used to initialize the positional encoders */
......@@ -246,31 +262,34 @@ SIGNAL(TWI_vect) {
/* Timer 0, control loop , 1 kHz */
SIGNAL(TIMER1_COMPA_vect) {
posCtrl = pos; // store pos to use in this loop
int32_t vel_ref = g_vel_ref;
int32_t acc_ref = g_acc_ref;
if (nbrSamples < 65535) {
nbrSamples++;
if (ref.samples <= 500) {
ref.samples++;
}
sei(); // to enable interupts from encoder counter and TWI
posCtrl = pos; // store pos to use in this loop
int32_t vel_ref = ref.vel.value;
int32_t acc_ref = ref.acc.value;
int16_t samples = ref.samples;
sei(); // to enable interupts from encoder counter, serial and TWI
// velocity estimation in mm/s
velEst = (((a*velEst+64)>>7)+b*(posCtrl-oldPos)); // 5 fracbits on velEst
oldPos = posCtrl;
if (samples > 500) {
// Communication lost, stop and reset
I = 0;
// Protect u and TWCR for concurrent updates
cli();
u = 0;
} else {
// store velEst and ref to be sent/used here
// cli();
refCtrl = vel_ref + ((acc_ref*nbrSamples)>>10); //shift nbrSamples 10 steps (= nbrSamples*h)
if (nbrSamples > 500) {
refCtrl = 0; // for safety reasons if doesn't send anything in 0.5 s
}
// vel_ref = vel_ref*(1-brake); // emergency stop
refCtrl = vel_ref + ((acc_ref*samples)>>10); //shift nbrSamples 10 steps (= nbrSamples*h)
// control error
e = refCtrl-((velEst+16)>>5); // mm/s
int32_t e = refCtrl-((velEst+16)>>5); // mm/s
// temporary ctrl-signal
v = (((K*e+(1<<5))>>6)+((I+(1<<3))>>4));
int32_t v = (((K*e+(1<<5))>>6)+((I+(1<<3))>>4));
// friction compensation
if (refCtrl > 0) {
......@@ -280,30 +299,30 @@ SIGNAL(TIMER1_COMPA_vect) {
}
// variable that decides if I-part should be updated
intCond = 1;
int8_t intCond = 1;
// saturation of v
int32_t vSat;
if (v > V_MAX) {
vSat = V_MAX;
if (e > 0)
intCond = 0;
if (e > 0) { intCond = 0; }
} else if (v < V_MIN) {
vSat = V_MIN;
if (e < 0)
intCond = 0;
if (e < 0) { intCond = 0; }
} else {
vSat = v;
}
if (intCond)
if (intCond) {
I = I + (((Ke*e)+(1<<1))>>2);
}
// Protect u and TWCR for concurrent updates
cli();
// scale ctrl-signal to send over twi
u = (vSat+8)>>4; // u=127 gives current = 6.75 A, vSat makes u saturate at 114
}
// TWI-communication to set current reference on the other atmel
// send start command
......@@ -353,15 +372,9 @@ int main()
//Serial communication initialization
UCSRA = 0x00; // USART:
UCSRB = 0x18; // USART: RxEnable|TxEnable
UCSRC = 0x86; // USART: 8bit, no parity
UBRRH = 0x00; // USART: 115200 @ 14.7456MHz
UBRRL = 7; // USART: 115200 @ 14.7456MHz
UCSRA = 0x00; // USART:
UCSRB = 0x98; // USART: RxIntEnable|RxEnable|TxEnable
UCSRC = 0x86; // USART: 8bit, no parity, 9600
UCSRC = 0x86; // USART: 8bit, no parity
UBRRH = 0x0; // USART: 115200 @ 14.7456MHz
UBRRL = 7; // USART: 115200 @ 14.7456MHz
......
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