Commit 36135468 authored by Pontus Giselsson's avatar Pontus Giselsson
Browse files

Velocity control with low jitter on simulink communication

parent 63447c91
......@@ -28,12 +28,14 @@ volatile int32_t Ke = 45; // 6 frac bits
volatile int32_t Ksat = 3; // 6 frac bits
// encoder variables
#define ENCODERY (PIND&(uint8_t)(1<<2)) //Positional encoder pins
#define ENCODERX (PINB&(uint8_t)(1<<1)) //Positional encoder pins
//#define ENCODERY (PIND&(uint8_t)(1<<2)) //Positional encoder pins
//#define ENCODERX (PINB&(uint8_t)(1<<1)) //Positional encoder pins
#define ENCODERY (PIND&0x04) //Positional encoder pins
#define ENCODERX ((PINB&0x02)<<1) //Positional encoder pins
volatile int16_t pos = 0;
volatile int16_t posTemp = 0;
volatile int16_t oldPos = 0;
volatile int16_t deltaPos = 0;
volatile int8_t newX;
volatile int8_t newY;
volatile int8_t oldX;
......@@ -86,46 +88,81 @@ void setPos()
}
/* Timer 2, Encoder */
/* Timer 2, Encoder counter, 73 kHz */
SIGNAL(SIG_OUTPUT_COMPARE2) {
deltaPos = 0;
// Update position from encoder
newX = ENCODERX;
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); //Find state
if (sum == 2 || sum == 4 || sum == 11 || sum == 13) { //Predetermined values determine direction
//pos = pos+1;
deltaPos = 1;
pos = pos+1;
} else if (sum == 1 || sum == 7 || sum == 8 || sum == 14) {
//pos = pos-1;
deltaPos = -1;
pos = pos-1;
} else {
brake = 1; // emergency brake
}
*/
if ((oldX == newY) && (oldY != newX)) {
pos = pos+1;
} else if ((oldX != newY) && (oldY == newX)) {
pos = pos-1;
} else {
brake = 1;
// emergency brake
}
oldX = newX;
oldY = newY;
}
// velocity estimation cut-off frequency 500 Hz
pos = pos+deltaPos; // update oldPos, 0 frac bits on pos and oldPos
}
/* Timer 0, control loop */
/* Timer 1, serial communication */
SIGNAL(SIG_OUTPUT_COMPARE0) {
TIMSK &= ~(BV(OCIE1A)|BV(OCIE0));
sei(); // enable interrupts from timer 0 and timer 2
//PORTC |= 0x10; // to clock calulation time
uint8_t status = UCSRA;
if (status & (1<<RXC)) {
char ch = UDR;
pccom_receiveByte(ch);
if (status & ((1<<FE)|(1<<DOR)|(1<<PE))) {
//main_emergencyStop(); // stop on USART error
}
}
// Poll UART sender
if (UCSRA & (1<<UDRE)) {
int16_t toSend = pccom_getNextByteToSend();
if (toSend >= 0) UDR = (char)toSend;
}
//PORTC &= ~0x10;
TIMSK |= (BV(OCIE1A)|BV(OCIE0));
}
/* Timer 0, control loop , 1 kHz */
SIGNAL(SIG_OUTPUT_COMPARE1A) {
TIMSK &= ~(BV(OCIE0)|BV(OCIE1A));
sei(); // to enable interupts from timer2
TIMSK &= ~BV(OCIE0);
PORTC |= 0x10; // to clock calulation time
// linear velocity estimator
// velocity estimate in mm/s
......@@ -223,39 +260,10 @@ SIGNAL(SIG_OUTPUT_COMPARE0) {
TIMSK |= (BV(OCIE0)|BV(OCIE1A));
// ------- Noncritical section -------
// Poll UART receiver
int8_t tempVar = 0;
for (tempVar = 0;tempVar < 10; tempVar++) {
uint8_t status = UCSRA;
if (status & (1<<RXC)) {
char ch = UDR;
pccom_receiveByte(ch);
if (status & ((1<<FE)|(1<<DOR)|(1<<PE))) {
//main_emergencyStop(); // stop on USART error
}
}
// Poll UART sender
if (UCSRA & (1<<UDRE)) {
int16_t toSend = pccom_getNextByteToSend();
//if (toSend >= 0) UDR = (char)toSend;
while (toSend >= 0) {
UDR = (char)toSend;
while ((UCSRA & (1<<UDRE)) == 0) {} // wait for previous send
toSend = pccom_getNextByteToSend();
}
}
}
PORTC &= ~0x10;
TIMSK |= BV(OCIE0);
}
......@@ -268,20 +276,52 @@ int main()
outp(0x10,DDRC); // timer calculation port
/* Timer section */
// Enable timer2 compare match interrupts
outp(BV(OCIE0)|BV(OCIE2),TIMSK);
// Enable timer0, timer1, timer2 compare match interrupts
outp(BV(OCIE0)|BV(OCIE1A)|BV(OCIE2),TIMSK);
/* Timer 2, 59 kHz Prescaler 1 */
/* Timer 2, 73 kHz Prescaler 1 */
outp(BV(WGM21)|BV(CS20),TCCR2);
outp(200,OCR2);
/* Reset timer 2 */
outp(0,TCNT2);
// --------old-----------------------------------------------
/* Timer 1, Prescaler 1, 73/8 kHz*/
//outp(BV(WGM12)|BV(CS10),TCCR1B);
//outp(0x06,OCR1AH);
//outp(0x40,OCR1AL);
//outp(0,TCNT1H);
//outp(0,TCNT1L);
/* Timer 0, 1 kHz Prescaler 64 */
//outp(BV(WGM01)|BV(CS01)|BV(CS00),TCCR0);
//outp(230,OCR0);
/* Reset timer 0 */
//outp(0,TCNT0);
// --------new-----------------------------------------------
/* Timer 1, 1 kHz */
outp(BV(WGM12)|BV(CS10),TCCR1B);
outp(0x39,OCR1AH);
outp(0x99,OCR1AL);
outp(0,TCNT1H);
outp(0,TCNT1L);
/* Timer 0, 10 kHz, Prescaler 64 */
outp(BV(WGM01)|BV(CS01)|BV(CS00),TCCR0);
outp(230,OCR0);
//outp(200,OCR0);
outp(23,OCR0); // 10 kHz
/* Reset timer 0 */
outp(0,TCNT0);
//----------------------------------------------------------
//Serial communication
......
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