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 ...@@ -28,12 +28,14 @@ volatile int32_t Ke = 45; // 6 frac bits
volatile int32_t Ksat = 3; // 6 frac bits volatile int32_t Ksat = 3; // 6 frac bits
// encoder variables // encoder variables
#define ENCODERY (PIND&(uint8_t)(1<<2)) //Positional encoder pins //#define ENCODERY (PIND&(uint8_t)(1<<2)) //Positional encoder pins
#define ENCODERX (PINB&(uint8_t)(1<<1)) //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 pos = 0;
volatile int16_t posTemp = 0; volatile int16_t posTemp = 0;
volatile int16_t oldPos = 0; volatile int16_t oldPos = 0;
volatile int16_t deltaPos = 0;
volatile int8_t newX; volatile int8_t newX;
volatile int8_t newY; volatile int8_t newY;
volatile int8_t oldX; volatile int8_t oldX;
...@@ -86,46 +88,81 @@ void setPos() ...@@ -86,46 +88,81 @@ void setPos()
} }
/* Timer 2, Encoder */ /* Timer 2, Encoder counter, 73 kHz */
SIGNAL(SIG_OUTPUT_COMPARE2) { SIGNAL(SIG_OUTPUT_COMPARE2) {
deltaPos = 0;
// Update position from encoder // Update position from encoder
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); //Find state sum = (oldX<<2)+oldY+newX+(newY>>2); //Find state
if (sum == 2 || sum == 4 || sum == 11 || sum == 13) { //Predetermined values determine direction if (sum == 2 || sum == 4 || sum == 11 || sum == 13) { //Predetermined values determine direction
//pos = pos+1; pos = pos+1;
deltaPos = 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;
deltaPos = -1; } else {
brake = 1; // emergency brake
}
*/
if ((oldX == newY) && (oldY != newX)) {
pos = pos+1;
} else if ((oldX != newY) && (oldY == newX)) {
pos = pos-1;
} else { } else {
brake = 1; brake = 1;
// emergency brake
} }
oldX = newX; oldX = newX;
oldY = newY; oldY = newY;
} }
}
/* 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
// velocity estimation cut-off frequency 500 Hz uint8_t status = UCSRA;
pos = pos+deltaPos; // update oldPos, 0 frac bits on pos and oldPos 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 */ /* Timer 0, control loop , 1 kHz */
SIGNAL(SIG_OUTPUT_COMPARE0) { SIGNAL(SIG_OUTPUT_COMPARE1A) {
TIMSK &= ~(BV(OCIE0)|BV(OCIE1A));
sei(); // to enable interupts from timer2 sei(); // to enable interupts from timer2
TIMSK &= ~BV(OCIE0);
PORTC |= 0x10; // to clock calulation time
// linear velocity estimator // linear velocity estimator
// velocity estimate in mm/s // velocity estimate in mm/s
...@@ -221,41 +258,12 @@ SIGNAL(SIG_OUTPUT_COMPARE0) { ...@@ -221,41 +258,12 @@ SIGNAL(SIG_OUTPUT_COMPARE0) {
// stop transmission // stop transmission
outp(BV(TWINT)|BV(TWEN)|BV(TWSTO),TWCR); outp(BV(TWINT)|BV(TWEN)|BV(TWSTO),TWCR);
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,22 +276,54 @@ int main() ...@@ -268,22 +276,54 @@ int main()
outp(0x10,DDRC); // timer calculation port outp(0x10,DDRC); // timer calculation port
/* Timer section */ /* Timer section */
// Enable timer2 compare match interrupts // Enable timer0, timer1, timer2 compare match interrupts
outp(BV(OCIE0)|BV(OCIE2),TIMSK); 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(BV(WGM21)|BV(CS20),TCCR2);
outp(200,OCR2); outp(200,OCR2);
/* Reset timer 2 */ /* Reset timer 2 */
outp(0,TCNT2); 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 */ /* 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(BV(WGM01)|BV(CS01)|BV(CS00),TCCR0);
outp(230,OCR0); //outp(200,OCR0);
outp(23,OCR0); // 10 kHz
/* Reset timer 0 */ /* Reset timer 0 */
outp(0,TCNT0); outp(0,TCNT0);
//----------------------------------------------------------
//Serial communication //Serial communication
outp(0x00, UCSRA); // USART: outp(0x00, UCSRA); // USART:
outp(0x18, UCSRB); // USART: RxEnable|TxEnable outp(0x18, UCSRB); // USART: RxEnable|TxEnable
......
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