diff --git a/linear_pendulum_2009/avr/vel_control.c b/linear_pendulum_2009/avr/vel_control.c
index 5024d75bfe7272adfd579bfc083e4b0e7e585880..3b10f60d33e7c2359f979b26ecf1ec538e20b266 100644
--- a/linear_pendulum_2009/avr/vel_control.c
+++ b/linear_pendulum_2009/avr/vel_control.c
@@ -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;
- }
+ }
+
+
+
+}
+
+
+/* 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
- pos = pos+deltaPos; // update oldPos, 0 frac bits on pos and oldPos
+ 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 */
-SIGNAL(SIG_OUTPUT_COMPARE0) {
+/* 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
@@ -221,41 +258,12 @@ SIGNAL(SIG_OUTPUT_COMPARE0) {
// stop transmission
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()
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
outp(0x00, UCSRA); // USART:
outp(0x18, UCSRB); // USART: RxEnable|TxEnable