diff --git a/linear_pendulum_2009/avr/current_control.c b/linear_pendulum_2009/avr/current_control.c
index 8f5c2200018e091efb9a6ddaefa71212ed38bf65..26454eaa10348147a48d1a07a8f0224239e46955 100644
--- a/linear_pendulum_2009/avr/current_control.c
+++ b/linear_pendulum_2009/avr/current_control.c
@@ -17,7 +17,7 @@
// reference generation variables
-volatile int16_t ref = 200;
+volatile int16_t ref = 100;
volatile int16_t refCount = 0;
volatile int8_t refFlag = 0;
@@ -25,14 +25,30 @@ volatile int8_t refFlag = 0;
// control variables
volatile unsigned char lbyte,hbyte;
-volatile int16_t result;
-volatile int16_t ctrl;
-volatile int16_t ctrl_out;
-volatile int16_t temp;
+volatile int16_t y;
volatile uint8_t alt = 1;
-
volatile uint8_t low, high;
+//test variables
+volatile int16_t e = 0;
+volatile int16_t v = 0;
+volatile int32_t I = 0;
+volatile int16_t u = 0;
+volatile int16_t K = 20;//5;//375; // 7 frac bits
+volatile int16_t Ke = 8;//2;//6; //7 frac bits, K*h/Ti
+volatile int16_t Ksat = 51;//1; // 7 frac bits, h/Tr
+
+
+// logging variables
+#define log_len 100
+volatile int16_t ctrl_log[log_len];
+volatile int16_t error_log[log_len];
+volatile int32_t I_log[log_len];
+volatile int16_t skipSamples = 200;
+volatile int16_t countSamples = 0;
+volatile int16_t jj=0;
+volatile int8_t stop = 0;
+volatile int16_t temp;
/* Routine used to set the red LED */
@@ -86,48 +102,97 @@ static inline void writeOutput(int16_t val) {
}
+/* Send logged data over Serial connection */
+static inline void sendData() {
+ int16_t ii = 0;
+ while (ii < log_len) {
+
+ putchar((unsigned char) ((ctrl_log[ii]&0xff00)>>8));
+ putchar((unsigned char) (ctrl_log[ii]&0x00ff));
+
+ putchar((unsigned char) ((error_log[ii]&0xff00)>>8));
+ putchar((unsigned char) (error_log[ii]&0x00ff));
+
+ putchar((unsigned char) ((I_log[ii]&0xff000000)>>24));
+ putchar((unsigned char) ((I_log[ii]&0x00ff0000)>>16));
+
+ putchar((unsigned char) ((I_log[ii]&0x0000ff00)>>8));
+ putchar((unsigned char) (I_log[ii]&0x000000ff));
+
+
+ ii++;
+ }
+}
+
/* Interrupt when AD-conversion completes */
SIGNAL(SIG_ADC)
{
setPA4(1);
+
// Read input
low = inp(ADCL);
high = inp(ADCH);
- result = ((high<<8) | low) - 512;
-
+ y = ((high<<8) | low) - 512; //y 9 frac bits
+
// Scale incoming current measurement
- result = result*3; //!!!!!!!!!!!!!!!!!!!!!!!
+ //y = y*3; //!!!!!!!!!!!!!!!!!!!!!!!
// Scale incoming current measurement
- // result += 13;
-
- //control
- ctrl = ref-result;
- ctrl = ((ctrl + (1<< 2)) >> 3);
+ y += 70;
+
+ //control, since negative measurements
+ e = ref+y; //e 9 frac bits
+
+ v = (int16_t)(((K*e+(1<<6)) >> 7)+(int16_t)((I+(1<<6))>>7));
+
//saturation/rounding to 8 bit
- if (ctrl > 511)
- ctrl_out = 511;
- else if (ctrl < -511)
- ctrl_out = -511;
+ if (v > 511)
+ u = 511;
+ else if (v < -512)
+ u = -512;
else
- ctrl_out = ctrl;
+ u = v;
+ I = I +((int32_t)Ke)*((int32_t)e) + ((int32_t)Ksat)*((int32_t)(u-v)); //16 frac bits
+
+ // write output, inverting mode means set pwm to 127-ctrl_out
+
+ u = (u)>>2; //7 frac bits to pwm
- // write output
- if (ctrl_out < 0) {
+
+
+ if (u < 0) {
PORTC = 0x80+(PORTC & 0x7F);
- OCR1BL = (unsigned char) (-ctrl_out);
+ OCR1BL = (unsigned char) (128-(-u));
} else {
PORTC = (PORTC & 0x7F);
- OCR1BL = (unsigned char) (ctrl_out);
+ OCR1BL = (unsigned char) (127-u);
+ }
+
+ // For logging
+ countSamples++;
+ if (countSamples == skipSamples) {
+ ctrl_log[jj] = y;
+ I_log[jj] = I;
+ error_log[jj] = e;
+ jj++;
+ countSamples = 0;
+ }
+
+ if ((jj == (log_len-1)) & !stop) {
+ outp(0x7f,OCR1BL);
+ stop = 1;
+ sendData();
}
+
setPA4(0);
}
+
/* reference square- or triangle wave generator with timer 0 */
SIGNAL(SIG_OVERFLOW0) {
int8_t rectangle = 1;
@@ -146,12 +211,12 @@ SIGNAL(SIG_OVERFLOW0) {
}
}
} else {
- if (refCount <= 400) { // ref*2
- ref -= 1;
+ if (refCount <= 20) { // ref*2
+ ref -= 2;
} else {
- ref += 1;
+ ref += 2;
}
- if (refCount == 800) { // ref*4
+ if (refCount == 40) { // ref*4
refCount = 0;
}
}
@@ -160,6 +225,7 @@ SIGNAL(SIG_OVERFLOW0) {
int main()
{
+ cli();
int i,j;
//Port directions
@@ -176,16 +242,17 @@ int main()
// Enable timer0 overflow interrupts
outp(BV(TOIE0),TIMSK);
- /* Timer 1, 8 bit fast PWM no prescaling -> h_pwm=17.35 micros */
- outp(BV(COM1A1)|BV(COM1B1)|BV(WGM10),TCCR1A);
- // Fast PWM
- // outp(BV(WGM12)|BV(CS10),TCCR1B);
- // Phase and frequency correct pwm
- outp(BV(CS10),TCCR1B);
- /* Reset Timer1*/
+ // Timer 1, fast PWM no prescaling (inverting mode (start low, switch to high))
+ outp(BV(COM1A1)|BV(COM1B1)|BV(COM1A0)|BV(COM1B0)|BV(WGM11)|BV(WGM10),TCCR1A);
+ outp(BV(CS10)|BV(WGM13)|BV(WGM12),TCCR1B);
- //outp(0x00,TCNT1H); // Reset TCNT1 high
- //outp(0x00,TCNT1L); // Reset TCNT1 low
+ // Reset Timer1 and set TOP-value to 128 (means 7-bit pwm-signal-> h_pwm=8.61 micros)
+ outp(0x00,OCR1AH);
+ outp(0x7f,OCR1AL);
+ outp(0x00,TCNT1H);
+ outp(0x00,TCNT1L);
+ outp(0x00,OCR1BH);
+ outp(0x7f,OCR1BL); // to not start motor-rotation before control
@@ -216,14 +283,19 @@ int main()
/* Wait a little bit, probably not needed...*/
int tmp;
- for(i=0;i<2000;i++)
- for(j=0;j<400;j++)
- tmp = j*j*j;
+ // for(i=0;i<2000;i++)
+ // for(j=0;j<400;j++)
+ // tmp = j*j*j;
//Enable interrupts
sei();
// loop
- while (1) {}
+ while (1) {
+ if (stop) {
+ cli();
+ OCR1BL = 0x7f;
+ }
+ }
}