Skip to content
Snippets Groups Projects
Commit 36135468 authored by Pontus Giselsson's avatar Pontus Giselsson
Browse files

Velocity control with low jitter on simulink communication

parent 63447c91
Branches
No related tags found
No related merge requests found
......@@ -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
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment